import processing.serial.*;
Serial myPort;  // Create object from Serial class

float xmag, ymag = 0;
float newXmag, newYmag = 0; 
int baseX, baseY = 0;
float rotX,rotY = 0.;
float oldrotX,oldrotY =0.;

float amp = 1.;
float zoom = 90.;
float x,y=0.;
int nx=100;
float dx = 3*PI/n;
float dy = 2*PI/n;

void setup() 
{ 
  size(1920, 1080, P3D); 
  String portName = Serial.list()[0];
  println(portName);
  myPort = new Serial(this, portName, 9600); 
  myPort.clear();
  
  noStroke(); 
  colorMode(RGB, 1.); 
} 

//pulse function [0,1]->[0,1]
float pulse(float in, float mid, float width_top, float width_bottom){
    if (abs(mid-in)< .5*width_top){
      return 1.0; 
    } else if( abs(mid-in)<.5*width_bottom ){
      if (in>mid){
         return 1.-2*(in-(mid+.5*width_top))/(width_bottom-width_top); 
      } else{
         return 1.-2*((mid-.5*width_top)-in)/(width_bottom-width_top);
      }
    } else{
     return  0.; 
    }
}


//use pulse train in RGB to create temperature map
void temp_map(float x, float xmin, float xmax){
   if (xmin > xmax){
      float xtemp = xmin;
      xmin=xmax;
      xmax=xtemp; 
   }
   if (x<xmin){
     x=0;
   } else if (x>xmax){
     x=1;
   } else{
    x = (x-xmin)/(xmax-xmin);
    fill(pulse(x,.25,.3,.75),pulse(x,.5,.3,.75),pulse(x,.75,.3,.75));
   }
}

//test function
float f(float x, float y){
    return amp*sin(x*y);
}

//helper function
void make_colored_vertex(float x,float y){
  temp_map(f(x,y),-1.,1.);
  vertex(x,y,f(x,y));       
}


float eps = 0.75; // filter fraction
float filt = 0.0; // filtered value

float take_reading(){
  int b1,b2=0,b3=0,b4=0;
  int ul,uh,dl,dh=0;
  int uv,dv,value=0;
  myPort.clear();
  do
  {
    while(myPort.available() == 0); 
    b1=b2; b2=b3; b3=b4;
    b4 = myPort.read();
    if(b1==1 && b2==2 && b3==3 && b4==4){
      print("found framing\n");
      break;
    }
  } while (true);
  while(myPort.available()==0);ul=myPort.read(); 
  while(myPort.available()==0);uh=myPort.read();
  while(myPort.available()==0);dl=myPort.read(); 
  while(myPort.available()==0);dh=myPort.read();
  uv=256*uh+ul; dv=256*dh+dl;
  value = uv-dv;
  filt = (1-eps)*filt + eps*value; //digital filtering
  filt = 1.5e-4*filt -.5;
  print(str(ul)+", "+str(uh)+", "+str(dl)+", "+str(dh)+"\n");
  print("filtered value = "+str(filt)+"\n");
  return filt;
}

//main loop
void draw() 
{
  background(.98,.94,.98);
  translate(width/2, height/2, -30);    
  pushMatrix(); 
  scale(zoom);
  beginShape(QUADS);
  rotateX(rotY); 
  rotateY(-rotX);
  amp = take_reading();
  for (int i = 0; i < n-1; i = i+1){
    for (int j = 0; j < n-1; j = j+1){
      x = dx*(i-.5*n);
      y = dy*(j-.5*n);
      make_colored_vertex(x,y);
      make_colored_vertex(x+dx,y);
      make_colored_vertex(x+dx,y+dy);
      make_colored_vertex(x,y+dy);
    }
  }
  endShape();
  popMatrix(); 
} 

void mousePressed(){
  baseX = mouseX;
  baseY = mouseY;
  //don't forget where we were.
  oldrotX = rotX;
  oldrotY = rotY;  
}

void mouseDragged(){ 
  if (keyCode==SHIFT){
    zoom += 3*(mouseY-baseY)/float(height);
  } else{
    //trackball stuff
  newXmag = (mouseX-baseX)/float(width) * TWO_PI;
  newYmag = (mouseY-baseY)/float(height) * TWO_PI;
  float diff = xmag-newXmag;
  if (abs(diff) >  0.01) { xmag -= diff/4.0; }
  diff = ymag-newYmag;
  if (abs(diff) >  0.01) { ymag -= diff/4.0; }
  rotY = oldrotY-ymag; //this is not quite right yet -- fix it. 
  rotX = oldrotX-xmag;
  }
}

//this is dirty, but get out of zooming mode when release shift.
void keyReleased(){
   if(keyCode == SHIFT){
       keyCode = TAB;
   } 
}

//interactive amplitude
void keyPressed(){
   if (key == 'a'){
    amp += .05;
   } else if (key == 'z'){
    amp -= .05; 
   }
}