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

//trackball variables
float xmag, ymag = 0;
float newXmag, newYmag = 0; 
int baseX, baseY = 0;
float rotX,rotY = 0.;
float oldrotX,oldrotY =0.;
float zoom = 150.;
float shiftx=0,shifty=0;
float sigma = 1.;

//stuff for setting up foot mesh
int nx = 100;
int ny = 10;
float[] xx = new float[nx];
float[][] yy = new float[nx][ny];
BufferedReader reader;
String line;

//this value will be controlled by sensor
float amp = 1.;
float amp_scale=1.;
float eps = 0.75; // filter fraction
float filt = 0.0; // filtered value

//stuff for color mapping
//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){
    x = x-shiftx;
    y = y-shifty;
    return -amp_scale*amp*exp(sigma*(-x*x-y*y));
}
//helper function
void make_colored_vertex(float x,float y){
  temp_map(f(x,y),-1.,1.);
  vertex(x,y,f(x,y));       
}
//helper function
void make_black_vertex(float x,float y){
  stroke(0.);
  vertex(x,y,f(x,y));       
}



//function to get data from sensor
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){
      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;
  return filt;
}





void setup() 
{ 
  //size(1920, 1080, P3D); 
  size(1080, 720, P3D); 
  String portName = Serial.list()[0];
  println(portName);
  myPort = new Serial(this, portName, 9600); 
  myPort.clear();
  reader = createReader("foot-points");    

  noStroke(); 
  colorMode(RGB, 1.); 

  //read in coordinates
  int linenum=0;
  do{
    try {
      line = reader.readLine();
    } catch (IOException e) {
      e.printStackTrace();
      line = null;
    }
    if (line == null) {
      break;  
    } else {
      String[] pieces = split(line, ",");
      float bx = (float(pieces[0])-30.)/10.;
      float by = (float(pieces[1])-10.)/10.;
      float ty = (float(pieces[3])-10.)/10.;
      if (by>ty){
        float tmp = by;
        by = ty;
        ty = tmp; 
      }
      xx[linenum]= bx;
      for (int k=0; k<ny; ++k){
        yy[linenum][k]= by+ (ty-by)*k/float(ny-1);
      }
      linenum++;
    }
  } while(line!=null);   

} 


//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();
  noStroke();
  for (int i = 0; i < nx-1; ++i){
    for (int j = 0; j < ny-1; ++j){
      make_colored_vertex(xx[i],yy[i][j]);
      make_colored_vertex(xx[i+1],yy[i+1][j]);
      make_colored_vertex(xx[i+1],yy[i+1][j+1]);
      make_colored_vertex(xx[i],yy[i][j+1]);
    }
  }
  endShape();
  noFill();
  beginShape();
  for (int i=0; i< nx-1; ++i){
    make_black_vertex(xx[i],yy[i][0]);
  }
  for (int i=nx-1; i>=0; --i){
    make_black_vertex(xx[i],yy[i][ny-1]);
  }
  endShape(CLOSE);
  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_scale += .05;
   } else if (key == 'z'){
    amp_scale -= .05; 
   }
   if (key == 's'){
    sigma += .05;
   } else if (key == 'x'){
    sigma -= .05; 
   }
   else if (keyCode == RIGHT){
    shiftx += .1; 
   }
   else if (keyCode == LEFT){
    shiftx -= .1; 
   }
   else if (keyCode == UP){
    shifty += .1; 
   }
   else if (keyCode == DOWN){
    shifty -= .1; 
   }
}