import processing.serial.*;

Serial serialport;
Visualization visVoltage;
Visualization visCurrent;
Visualization visSpeedl;
Visualization visSpeedr;
Visualization visYaw;
Visualization visGT;
Visualization visGT_Vertical;

int speedl=0;
int speedr=0;
int booleanvalues=0;
float voltage = 50;
float current = 0.0;
float yaw=0;
int gt_length=0;
int gt_horizontal=0;
int gt_vertical=0;

long lastReceive=0; //last time serial received
long lastDelay=0;


void setup() {
  size(640, 450);
  frameRate(100);
  
  printArray(Serial.list());
  serialport = new Serial(this, Serial.list()[32], 115200);
  
  
  visVoltage = new BarV(50,150,10,100,10*3.3,12*4.2);
  visVoltage.setcmain(color(100,100,100));
  //vis = new BarH(150,150,100,10,0,100);
  //visVoltage = new Tacho(150,150,100,0,100);
  //vis = new Direction(150,150,100,0,100,0,1,0);
  
  visVoltage.setShowMinMax(true);
  visVoltage.setTitle("Voltage [V]");
  
  visCurrent= new Tacho(150+100,150,100,0,100);
  visCurrent.setShowMinMax(true);
  visCurrent.setTitle("Current [A]");
  
  visSpeedl = new BarV(20,300,10,100,-1000,1000);
  visSpeedl.setTitle("SpeedL");
  
  visSpeedr = new BarV(20+80,300,10,100,-1000,1000);
  visSpeedr.setTitle("SpeedR");
  
  visYaw = new Direction(150+100,300,100,0,360,0,1,0);
  visYaw.setTitle("Yaw");
  
  visGT = new Direction(150+100+220,300,100,-127/30*180,127/30*180,0,2500,PI/2+PI);
  visGT.setTitle("Gametrak");
  visGT_Vertical = new BarV(150+100+220+110,300+50,10,100,-127,127);
  visGT_Vertical.setTitle("Vertical");
  
}

void draw() {
  receive();
  
  background(255);
  visVoltage.setValue(voltage);
  visCurrent.setValue(current);
  visSpeedl.setValue(speedl);
  visSpeedr.setValue(speedr);
  visYaw.setValue(yaw);
  visGT.setValue(-gt_horizontal);
  visGT.setValue2(gt_length);
  visGT_Vertical.setValue(gt_vertical);
  
  
  visVoltage.drawVis();
  visCurrent.drawVis();
  visSpeedl.drawVis();
  visSpeedr.drawVis();
  visYaw.drawVis();
  visGT.drawVis();
  visGT_Vertical.drawVis();
  
  fill(color(0,0,0));
  textSize(12);
  long _delay=lastDelay;
  if (millis()-lastReceive>1000){ //show counting up if update too long ago
    _delay=millis()-lastReceive;
  }
  text("Delay="+(_delay), 5,12);
}


public void receive()
{
  boolean received=false;
  byte[] inBuffer = new byte[17];
  while(serialport.available()>0) {
    
    inBuffer = serialport.readBytes();
    serialport.readBytes(inBuffer);
    if (inBuffer != null && inBuffer.length==17) {
      received=true;
      int _address=0;
      speedl = extract_int16_t(inBuffer,_address); _address+=2;
      speedr = extract_int16_t(inBuffer,_address); _address+=2;
      booleanvalues = extract_uint8_t(inBuffer,_address); _address+=1;
      voltage = extract_float(inBuffer, _address);  _address+=4;
      //current = extract_float(inBuffer,_address);_address+=4;
      yaw = extract_float(inBuffer,_address);_address+=4;
      gt_length = extract_uint16_t(inBuffer,_address); _address+=2;
      gt_horizontal = extract_int8_t(inBuffer,_address); _address+=1;
      gt_vertical = extract_int8_t(inBuffer,_address); _address+=1;
      //println("yaw="+yaw);
      //println("gt_horizontal="+gt_horizontal);
      
      boolean motorenabled=boolean(booleanvalues & (1<<0)>>0); //check bit 0
      int controlmode=(booleanvalues & (3<<1))>>1; //check bit 1 and 2
     
      println("motorenabled="+motorenabled+" controlmode="+controlmode);
    }
    
  }
  if (received){
    lastDelay=millis()-lastReceive;
    lastReceive=millis();
  }
}
  
public int extract_int8_t(byte array[], int startbyte) {
  if ( ((int)array[startbyte] & 0x80) == 0x00 ) { //2's complement, not negative
    return ((int)array[startbyte] & 0xff);
  }else{
    return -128 + ( (int)array[startbyte] & 0x7f);
  }
}

public int extract_uint8_t(byte array[], int startbyte) {
  return ((int)array[startbyte] & 0xff);
}

public int extract_uint16_t(byte array[], int startbyte) {
  return ((int)array[startbyte] & 0xff) | ((int)array[startbyte+1] & 0xff)<<8;
}

public int extract_int16_t(byte array[], int startbyte) {
  if ( ((int)array[startbyte+1] & 0x80) == 0x00 ) { //2's complement, not negative
    return ( ((int)array[startbyte] & 0xff) | ((int)array[startbyte+1] & 0x7f)<<8 );
  }else{ //value is negative
    return -32768 + ( ((int)array[startbyte] & 0xff) | ((int)array[startbyte+1] & 0x7f)<<8 );
  }
}

public float extract_float(byte array[], int startbyte) {
    int MASK = 0xff;
    int bits = 0;
    int i = startbyte+3; //+3 because goes backwards and has 4 bytes
    for (int shifter = 3; shifter >= 0; shifter--) {
    bits |= ((int) array[i] & MASK) << (shifter * 8);
    i--;
    }
    return Float.intBitsToFloat(bits);
}