import processing.serial.*;
Serial myPort;
//DECLARE
Arm[] arrayArm=new Arm[8];
Arm myArm;
int numSegments = 50;
float[] x = new float[numSegments];
float[] y = new float[numSegments];
float[] angle = new float[numSegments];
float segLength = 12;
float targetX, targetY;
float xpos;
float ypos;
float xa;
float ya;
float armpos;
float armA;
void setup() {
  println(Serial.list());
  String portName = Serial.list()[0];
  myPort = new Serial(this, "/dev/tty.usbmodem1411", 9600);
  myPort.bufferUntil('\n');
  //noCursor(); 
  size(1000, 800);
  //INITIALIZE
  myArm = new Arm();
  for (int t=0; t<arrayArm.length; t++) {
    arrayArm[t]=new Arm();
  }
}
void draw() {
  background(0, 180, 195);
  fill(255, 120, 90);
  //ellipse(width*3/5, height*2/5, 220 + 10*sin(millis()/100), 220 + 10*cos(millis()/100));
  for (int t=0; t<arrayArm.length; t++) {
    pushMatrix();
    translate(width*4/6, height*2/5);
    ellipse(width-width, height-height, 100 + 10*sin(millis()/50), 100 + 10*cos(millis()/50));
    rotate (PI*t/13);
    arrayArm[t].display();
    for (int i=0; i<x.length; i++) {
      arrayArm[t].segment(x[i], y[i], angle[i], (i+1)*2);
    }
    popMatrix();
  }
  myArm.reachSegment(0, xpos, ypos);
  for (int i=1; i<numSegments; i++) {
    myArm.reachSegment(i, targetX, targetY);
  }
  for (int i=x.length-1; i>=1; i--) {
    myArm.positionSegment(i, i-1);
  }
}
void serialEvent(Serial myPort) {
  String myString = myPort.readStringUntil('\n');
  if (myString != null) {
    myString = trim(myString);
    int sensors[] = int(split(myString, ','));
    
    println();
    if (sensors.length >1 ) {
     
      xpos = map(sensors[0], 100, 118, PI, PI/10);
      ypos = map(sensors[1], 90, 200, 120, 1500 );
      //armA = map(sensors[0], 115, 142, 0, height); 
        
      }
    
    for (int sensorNum = 0; sensorNum < sensors.length; sensorNum++) {
      print("Sensor " + sensorNum + ": " + sensors[sensorNum] + "\t");
    }
    }
  }
 
class Arm {
  
//int sensorVal;
//int getSensorVal(){ return this.sensorVal; }
//int setSensorVal(int sensorVal){ 
//this.sensorVal = sensorVal; }
//
//for (int i = 0; i < mySensors.length; i++){
//         myArmArray[i].setSensorVal(mySensors[i]);
//}
  //CONSTRUCTOR
  Arm() {
 
  }
  
  //FUNCTIONS
  
  void pulseBody(){ 
    fill(255, 120, 90);
    
    //need to add accelerometer sensor values to body pulsing
   
  }
 
      void display(){
        
        strokeWeight(20.0);
        stroke(255, 120, 90);
        
       //set (x,y) to zero to perform transform and then rotate 
      x[x.length-1] = width-width;   // Set base x-coordinate
      y[x.length-1] = height-height;  // Set base y-coordinate
     
      
 // x[x.length-1] = width/3;   // Set base x-coordinate
 // y[x.length-1] = height/2;  // Set base y-coordinate
    
  }
  
  void positionSegment(int a, int b) {
  x[b] = x[a] + cos(angle[a]) * segLength+xpos/2.0;
  y[b] = y[a] + sin(angle[a]) * segLength - xpos/4.0;
}
void reachSegment(int i, float xin, float yin) {
  float dx = xin - x[i];
  float dy = yin - y[i];
  angle[i] = atan2(dy, dx);  
  targetX = xin - cos(angle[i]) * segLength;
  targetY = yin - sin(angle[i]) * segLength;
}
void segment(float x, float y, float a, float sw) {
  strokeWeight(sw);
  pushMatrix();
  translate(x, y);
  rotate(a);
  line(0, 0, segLength, 0);
  popMatrix();
}
  
}