// import serial database******************************
 import processing.serial.*;

Serial myPort1;  // Create object from Serial class

int myString1;
int COUNT = 0;
int READY = 0; // ready to send data
int STAGE = 0; // what stage of the program

byte STARTCALL[] = {0x05};
byte GO60[] = {0x5a, 0x04, 0x05, 0x00, 0x00, 0x32, 0x33};
byte GO50[] = {0x5a, 0x04, 0x05, 0x00, 0x00, 0x3c, 0x3d};
int ENDCALL = 0x84;
 
 //CARS************************
 int x=75;
 int STORE = 0; // store the last x value to compare.
    
    void setup() {
      size(600,400);
      
    // Setup the serial port for FANUC sensor input. The 'O' is of odd parity.
    myPort1 = new Serial(this, "COM3", 4800, 'O', 8, 1);
                //Serial(parent, portName, baudRate, parity, dataBits, stopBits)  
      
    }
    
    void draw() {
      background(100);
      fill (200);
      rect (75, 25, 400, 50);
      stroke(0);
      
      if(mousePressed) {
      if (mouseX >75 && mouseX <= 475)
        {x=mouseX;}
        }
      fill(127,0,0);
      rect (x, 20, 9, 60); 
      fill (255);
 
      // Left Button
      fill (200);
      rect (10, 25, 50, 50);
    {
      if (mousePressed == true) {
        if (mouseX <= 50 && (mouseY >= 40 && mouseY <= 60)) {
        fill(255);
        if (x>100){
        x-=20;
      } else {
        x=75;
      }
      } else {
        fill(0);
      }
      }
      if (mousePressed == false) {
      fill (0);
      }
      triangle (50, 60, 50, 40, 15, 50);
    }
      
      // Right button
      fill (200);
      rect (490, 25, 50, 50);
      {
      if (mousePressed == true) {
        if (mouseX >= 500 && (mouseY >= 40 && mouseY <= 60)) {
        fill(255);
        if (x<470){
          x+=20;
      } 
      else {
        x=470;
      }
        } else {
        fill(0);
      }
 }
      if (mousePressed == false) {
      fill (0);
      }
      triangle (500, 60, 500, 40, 535, 50);
    }
      //println(x);
      //print(x);
      //print("|HEX=");
      //print(hex(x,4));
      //print("|BCC=");
      
    
   
     
     // serial send section**********************************
     if(x != STORE){
       //x = x - 70;
      //STORE = x;
       print("STAGE=");println(STAGE);
      //0x04 0x05 0x00 0x00 0x3C
      short VAL0 = 0x5a;
      short VAL1 = 0x04;
      short VAL2 = 0x05;
      short VAL3 = 0x00;
      short VAL4 = 0x00;
      short VAL5 = 0x3C;
      
     // if larger than 255 grab a 4 digit hex, and cut off the last two and make that the second to last slot before BCC
    if(x >= 255){
    String VAL = hex(x,4);
    VAL = VAL.substring(0, VAL.length() - 2);
    VAL4 = Short.parseShort(VAL, 16) ;
    
    }
       
       // store value of x into last byte before BCC
       if(x >= 0){
       //String VAL5A =hex(x,2);
       //VAL5 =  convertStringToHex(VAL5A);
       VAL5 = Short.parseShort(hex(x,2), 16) ;
       }
       
      // debug print to terminal to see what is being sent.
      print("Decimal="); print(x);print("|HEX="); // decimal value for debugging
      print(hex(VAL1,2)); print("-"); print(hex(VAL2,2)); print("-"); print(hex(VAL3,2)); print("-"); print(hex(VAL4,2)); print("-"); print(hex(VAL5,2)); print("-"); // print serial string
      println(hex(VAL1 ^ VAL2 ^ VAL3 ^ VAL4 ^ VAL5,2)); // calculate BCC
      
      
      
      
    // (X) Send to serial port, start the process.***************
    // Start the call and wait for a response  
    if(STAGE == 0){   
    myPort1.write(STARTCALL);
    //delay(10);
    STAGE = 1;
    READY = 0;
    //debug("stage = 1");
    println("* (X) START CALL *");
    }
    
    // send out the serial data here ***************
    if(STAGE == 1 && READY == 1){
    //myPort1.write(GO50);
    
    // X AXIS ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    VAL2 = 0x05; // #5 (x)
    myPort1.write(VAL0); // TCC what it is trying to send ( Register info )
    myPort1.write(VAL1); // COUNT how many data bytes will follow ( for this always 4 ) 
    myPort1.write(VAL2); // R[] number this is what register to write to ( for this always #5 )
    myPort1.write(VAL3); // D1 First byte of the data ( For this always 0 ) Leading zeros are used here
    myPort1.write(VAL4); // D2 Second Byte of the data ( Only used if over 255 )
    myPort1.write(VAL5); // D3 Trd Byte of data, this is used for every number from 0-255
    myPort1.write(VAL1 ^ VAL2 ^ VAL3 ^ VAL4 ^ VAL5); //BCC Error correcting byte done with XOR ( Exclusive or comparison '^') 
        
    println("* (X) DATA SENT *");
    
    
    //delay(10);
    READY = 0;
    STAGE = 3;
    }
    
    // data sent, end the call.
    if(STAGE == 3 && READY == 1){
    myPort1.write(ENDCALL);
    //delay(10);
    STAGE = 10;
    
    println("* (X) END CALL *");
    }
    
    
    
    

    
    
      
      
      
    //(Y) Send to serial port, start the process.***************
    // Start the call and wait for a response  
    if(STAGE == 10){   
    //delay(50);
    myPort1.write(STARTCALL);
    //delay(10);
    STAGE = 11;
    READY = 0;
    //debug("stage = 1");
    println("* (Y) START CALL *");
    }
    
    // send out the serial data here ***************
    if(STAGE == 11 && READY == 1){
    //myPort1.write(GO50);
    
    // Y AXIS ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    VAL2 = 0x06; // #6 (y)
    myPort1.write(VAL0); // TCC what it is trying to send ( Register info )
    myPort1.write(VAL1); // COUNT how many data bytes will follow ( for this always 4 ) 
    myPort1.write(VAL2); // R[] number this is what register to write to ( for this always #5 )
    myPort1.write(VAL3); // D1 First byte of the data ( For this always 0 ) Leading zeros are used here
    myPort1.write(VAL4); // D2 Second Byte of the data ( Only used if over 255 )
    myPort1.write(VAL5); // D3 Trd Byte of data, this is used for every number from 0-255
    myPort1.write(VAL1 ^ VAL2 ^ VAL3 ^ VAL4 ^ VAL5); //BCC Error correcting byte done with XOR ( Exclusive or comparison '^') 
        
    println("* (Y) DATA SENT *");
    
    
    //delay(10);
    READY = 0;
    STAGE = 13;
    }
    
    // data sent, end the call.
    if(STAGE == 13 && READY == 1){
    myPort1.write(ENDCALL);
   // delay(10);
    STAGE = 20;
    STORE = x;
    println("* (Y) END CALL *");
    }  
      
   
      
      
          // (RUN) Send to serial port, start the process.***************
    // Start the call and wait for a response  
    if(STAGE == 20){   
    myPort1.write(STARTCALL);
    //delay(10);
    STAGE = 21;
    READY = 0;
    //debug("stage = 1");
    println("* (RUN) START CALL *");
    }
    
    // send out the serial data here ***************
    if(STAGE == 21 && READY == 1){
    //myPort1.write(GO50);
    
    // X AXIS ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
    VAL2 = 0x08; // #8 (RUN)
    VAL4 = 0x00; // leading zeros
    VAL5 = 0x01; // #1 = All data sent, run command. #0 is robot ran and is now waiting.
    myPort1.write(VAL0); // TCC what it is trying to send ( Register info )
    myPort1.write(VAL1); // COUNT how many data bytes will follow ( for this always 4 ) 
    myPort1.write(VAL2); // R[] number this is what register to write to ( for this always #5 )
    myPort1.write(VAL3); // D1 First byte of the data ( For this always 0 ) Leading zeros are used here
    myPort1.write(VAL4); // D2 Second Byte of the data ( Only used if over 255 )
    myPort1.write(VAL5); // D3 Trd Byte of data, this is used for every number from 0-255
    myPort1.write(VAL1 ^ VAL2 ^ VAL3 ^ VAL4 ^ VAL5); //BCC Error correcting byte done with XOR ( Exclusive or comparison '^') 
        
    println("* (RUN) DATA SENT *");
    
    
    //delay(10);
    READY = 0;
    STAGE = 23;
    }
    
    // data sent, end the call.
    if(STAGE == 23 && READY == 1){
    myPort1.write(ENDCALL);
    //delay(10);
    STAGE = 0;
    
    println("* (RUN) END CALL *");
    }
      
      
      
      //x = x + 70;
     }// end of x!0 to store
      
      
      
      line (115, 60, 115, 90);
      line (155, 60, 155, 90);
      line (195, 60, 195, 90);
      line (235, 60, 235, 90);
      line (275, 60, 275, 90);
      line (315, 60, 315, 90);
      line (355, 60, 355, 90);
      line (395, 60, 395, 90);
      line (435, 60, 435, 90);
   
  



// Check the serial port for data *********************************************

if ( myPort1.available() > 0) {  // If data is available,
    //val = myPort.read();         // read it and store it in val
    //print(val);
myString1 = myPort1.read();
println("* RESPONSE FROM FANUC *");

READY = 1; // got a response send data.



//myPort2.write(COUNT)


}











}