import processing.serial.*;


Serial myPort;  // The serial port

float[] serialInArray = new float[4];    // Array to receive
float[] serialInArrayB = new float [3];  //Array to keep the last values
float currentAVG;
float lastAVG;
float  menos;
int serialCount = 0;       
float v= 0.0;          // A count of how many bytes are received

// initial variables:
int i = 1;                   // counter
int inByte = -1;             // data from serial port
int wrote = 0;
int offset=0;
int offsettext = 25;

float xpos, ypos, zpos = 0;
int bxpos, bypos, bzpos =0;
boolean firstContact = false;

float loc=0;
float vel=0;

void setup() {
  size(600,600);
  smooth();
  framerate(30);

  
     println(Serial.list());
  myPort = new Serial(this, Serial.list()[1], 9600);
   myPort.write(65);    // Send a capital A to start the microcontroller sending
  }
  


void draw() {
  background(250);
  while (myPort.available() > 0) {
    processByte(myPort.read());
    // Contact with Arduino:
    firstContact = true;
  }
  // If there's no serial data, send again until we get some.
  // (in case you tend to start Processing before you start your 
  // external device):
  if (firstContact == false) {
    delay(300);
    myPort.write(65);
  }

  
 
    
 // }
  

  
  

}





void drawGraph () {

  stroke(255,0,0,50);
 
  zpos= (zpos*0.3)+(serialInArrayB[2]*0.7);
  line(i, height, i, (height - height*(zpos/255.0))+20);
  xpos = (xpos*0.3)+(serialInArrayB[0]*0.7);
  stroke(0,255,0,50);
  line(width, i,  (width - width*(xpos/255.0))+20,i);
  ypos= (ypos*0.3)+(serialInArrayB[1]*0.7);
  stroke(0,0,255,50);
  line(width, i, i, (width - width*(ypos/255.0))+20);

  menos = zpos -139;
  vel += menos;
  loc += vel;
  float scal= ypos;
  noStroke();
    fill(250,100,100);
    ellipse(width/2-xpos,(height/2)- zpos,scal,scal);  

  // at the edge of the screen, go back to the beginning:
  if (i >= width-2) {
    i = 0;
    background(0); 
  } 
  else {
    i++;
  }

if (wrote > 27)
  {
 
  wrote = 0;
  }
  wrote++;





  //finding the difference

  serialInArrayB[0]=xpos;
  serialInArrayB[1]=ypos;
  serialInArrayB[2]= zpos;




}

void processByte( int inByte) {
  // Add the latest byte from the serial port to array:
  serialInArray[serialCount] = inByte;
  serialCount++;
  // If we have 3 bytes:
  if (serialCount > 3 ) {
    xpos = serialInArray[0];
    ypos = serialInArray[1];
    zpos = serialInArray[2];
    v= 1000-zpos;
    v= v*0.001;

    //   println(xpos + "\t" + ypos + "\t" + zpos);
    drawGraph();
    // Send a capital A to request new sensor readings:
    myPort.write(65);
    // Reset serialCount:
    serialCount = 0;
  }
}