import processing.serial.*;

int vis_textsize=12; //copy from Visualization class

String logfile_name="/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/logdata_visualization/logfixer/20211004_171509.csv";
int columnCount=20;

boolean useSerial=false; //false=read from csv log, true=read from serial port
//String serial_port="COM3";
String serial_port="/dev/ttyUSB0";
Serial serial;
String serialString=""; //last read string
int serial_endchar=10; //10=ASCII Linefeed

float minCurrentDisplayed=-5;
float maxCurrentDisplayed=20;

float minRPMDisplayed=-100;
float maxRPMDisplayed=1200;

//timeline
float logdata_start_time=0;
float logdata_end_time=0;

Visualization vis_cmd_FrontL;
Visualization vis_cmd_FrontR;
Visualization vis_cmd_RearL;
Visualization vis_cmd_RearR;

Visualization vis_current_FrontL;
Visualization vis_current_FrontR;
Visualization vis_current_RearL;
Visualization vis_current_RearR;

Visualization vis_rpm_FrontL;
Visualization vis_rpm_FrontR;
Visualization vis_rpm_RearL;
Visualization vis_rpm_RearR;

Visualization vis_throttle;
Visualization vis_brake;

Visualization vis_currentAll;

Visualization vis_c_speed;

//vis_c means calculated value, not raw value from log
Visualization vis_c_rpm_mean;

Visualization vis_graph_currentAll;
Visualization vis_graph_speed_mean;


Visualization vis_c_graph_receivedelay;

Visualization vis_c_rpmvector;
Visualization vis_c_currentvector;

boolean showTimeline=!useSerial;
Timeline tl;
int timeoffset=0; //for moving timeslider


Table logdata;
int nextID=0; //next row number to be displayed
long lastTimeData=0; //last time data received
int nextTimeData=0; //time of nextID row
int lastTimeMillis=0; //local time
int nextTimeMillis=0; //local time

boolean newdataforced=true;

int dataErrorCount=0;

boolean running=true;
int timePaused=0;

//Data from log
int cmd_FrontL;
int cmd_FrontR;
int cmd_RearL;
int cmd_RearR;
float current_FrontL;
float current_FrontR;
float current_RearL;
float current_RearR;
int rpm_FrontL;
int rpm_FrontR;
int rpm_RearL;
int rpm_RearR;
float temp_Front;
float temp_Rear;
float vbat_Front;
float vbat_Rear;
float currentAll;
int throttle;
int brake;
float speed;
float trip;
float currentConsumed;

color bg=color(0);

void setup() {
  //size(1920, 1080); //Full HD
  size(1000, 800); //Laptop Preview
  frameRate(100);
  
  if (useSerial) {
    printArray(Serial.list());
    // Open the port you are using at the rate you want:
    serial = new Serial(this, serial_port, 115200);
    
    serial.clear();
    // Throw out the first reading, in case we started reading 
    // in the middle of a string from the sender.
    println("readUntil");
    serialString = serial.readStringUntil(serial_endchar);
    println("read:"+serialString);
    serialString = null;
  }else{
    logdata = loadTable(logfile_name, "header, csv");
    float _checkTimeLast=-100;
    
    
    for (int i=0; i < logdata.getRowCount();i++) {
      float _checkTimeCurrent=logdata.getRow(i).getFloat("time")*1000;
      
      if (logdata.getRow(i).getString(0).charAt(0)=='#') { //check if row starts with # (comment)
          print("removed comment:");
          for (int is=0;is< logdata.getRow(i).getColumnCount() && logdata.getRow(i).getString(is) != null; is++)
          {
              print(", "+logdata.getRow(i).getString(is));
          }
          println();
          logdata.removeRow(i); i--;
      }else if (Float.isNaN(_checkTimeCurrent) || _checkTimeCurrent <= _checkTimeLast) { //check if time is plausible
          print("removed unplausible time:");
          for (int is=0;is< logdata.getRow(i).getColumnCount() && logdata.getRow(i).getString(is) != null; is++)
          {
              print(", "+logdata.getRow(i).getString(is));
          }
          println();
          logdata.removeRow(i); i--;
      }
    }
    logdata_start_time=logdata.getRow(0).getFloat("time");
    logdata_end_time = logdata.getRow(logdata.getRowCount()-1).getFloat("time");
    println("loaded "+logdata.getRowCount()+" lines. Starttime: "+logdata_start_time+"s , Endtime: "+logdata_end_time+"s");
  }
  
  PVector pos_vis_cmd = new PVector(100,200);
  PVector size_vis_cmd = new PVector(10,100);
  PVector dist_vis_cmd = new PVector(80,150);
  
  colorMode(RGB, 255, 255, 255);
  
  //cmd
  color c_cmd=color(255,50,0);
  vis_cmd_FrontL = new BarV_cmd((int)pos_vis_cmd.x,(int)pos_vis_cmd.y,(int)size_vis_cmd.x,(int)size_vis_cmd.y,-1000,1000);
  
  vis_cmd_FrontR = new BarV_cmd((int)(pos_vis_cmd.x+dist_vis_cmd.x),(int)pos_vis_cmd.y,(int)size_vis_cmd.x,(int)size_vis_cmd.y,-1000,1000);
  vis_cmd_FrontR.setTitle("cmd");  
  
  vis_cmd_RearL = new BarV_cmd((int)pos_vis_cmd.x,(int)(pos_vis_cmd.y+dist_vis_cmd.y),(int)size_vis_cmd.x,(int)size_vis_cmd.y,-1000,1000);
  
  vis_cmd_RearR = new BarV_cmd((int)(pos_vis_cmd.x+dist_vis_cmd.x),(int)(pos_vis_cmd.y+dist_vis_cmd.y),(int)size_vis_cmd.x,(int)size_vis_cmd.y,-1000,1000);
  
  vis_cmd_FrontL.setcmain(c_cmd);
  vis_cmd_FrontR.setcmain(c_cmd);
  vis_cmd_RearL.setcmain(c_cmd);
  vis_cmd_RearR.setcmain(c_cmd);
  
  // Speed
  color c_speed=color(50,50,255);
  vis_rpm_FrontL = new BarV_cmd((int)(pos_vis_cmd.x-size_vis_cmd.x*2),(int)pos_vis_cmd.y+vis_textsize,(int)size_vis_cmd.x,(int)size_vis_cmd.y,minRPMDisplayed,maxRPMDisplayed); 
  
  vis_rpm_FrontR = new BarV_cmd((int)(pos_vis_cmd.x+dist_vis_cmd.x+size_vis_cmd.x*2),(int)pos_vis_cmd.y+vis_textsize,(int)size_vis_cmd.x,(int)size_vis_cmd.y,minRPMDisplayed,maxRPMDisplayed);
  vis_rpm_FrontR.setTitle("speed");
  
  vis_rpm_RearL = new BarV_cmd((int)(pos_vis_cmd.x-size_vis_cmd.x*2),(int)(pos_vis_cmd.y+vis_textsize+dist_vis_cmd.y),(int)size_vis_cmd.x,(int)size_vis_cmd.y,minRPMDisplayed,maxRPMDisplayed);
  vis_rpm_RearL.setValueUnit("rpm");
  
  vis_rpm_RearR = new BarV_cmd((int)(pos_vis_cmd.x+dist_vis_cmd.x+size_vis_cmd.x*2),(int)(pos_vis_cmd.y+vis_textsize+dist_vis_cmd.y),(int)size_vis_cmd.x,(int)size_vis_cmd.y,minRPMDisplayed,maxRPMDisplayed);
  
  vis_rpm_FrontL.setcmain(c_speed);
  vis_rpm_FrontR.setcmain(c_speed);
  vis_rpm_RearL.setcmain(c_speed);
  vis_rpm_RearR.setcmain(c_speed);
  
  // Current  
  color c_current=color(255,200,50);
  vis_current_FrontL = new BarV_cmd((int)(pos_vis_cmd.x-size_vis_cmd.x*2*2),(int)pos_vis_cmd.y+vis_textsize*2,(int)size_vis_cmd.x,(int)size_vis_cmd.y,minCurrentDisplayed,maxCurrentDisplayed);

  vis_current_FrontR = new BarV_cmd((int)(pos_vis_cmd.x+dist_vis_cmd.x+size_vis_cmd.x*2*2),(int)pos_vis_cmd.y+vis_textsize*2,(int)size_vis_cmd.x,(int)size_vis_cmd.y,minCurrentDisplayed,maxCurrentDisplayed);
  vis_current_FrontR.setTitle("current");
  
  vis_current_RearL = new BarV_cmd((int)(pos_vis_cmd.x-size_vis_cmd.x*2*2),(int)(pos_vis_cmd.y+vis_textsize*2+dist_vis_cmd.y),(int)size_vis_cmd.x,(int)size_vis_cmd.y,minCurrentDisplayed,maxCurrentDisplayed);
  vis_current_RearL.setValueUnit("A");
  
  vis_current_RearR = new BarV_cmd((int)(pos_vis_cmd.x+dist_vis_cmd.x+size_vis_cmd.x*2*2),(int)(pos_vis_cmd.y+vis_textsize*2+dist_vis_cmd.y),(int)size_vis_cmd.x,(int)size_vis_cmd.y,minCurrentDisplayed,maxCurrentDisplayed);
  
  vis_current_FrontL.setcmain(c_current);
  vis_current_FrontR.setcmain(c_current);
  vis_current_RearL.setcmain(c_current);
  vis_current_RearR.setcmain(c_current);
  
  //Inputs
  PVector pos_vis_inputs = new PVector(width/2,height-50); //will be center for x
  PVector size_vis_inputs = new PVector(200,40);
  
  color c_throttle=color(255,150,50);
  vis_throttle = new BarH_cmd((int)pos_vis_inputs.x,(int)pos_vis_inputs.y,(int)size_vis_inputs.x,(int)size_vis_inputs.y,0,1000);
  vis_throttle.setcmain(c_throttle);
  vis_throttle.setTitle("Throttle");
  color c_brake=color(200,200,50);
  vis_brake = new BarH_cmd((int)(pos_vis_inputs.x-size_vis_inputs.x),(int)pos_vis_inputs.y,(int)size_vis_inputs.x,(int)size_vis_inputs.y,0,1000);
  vis_brake.setcmain(c_brake);
  vis_brake.setTitle("Brake");
  
  //Speed
  vis_c_speed = new Tacho(width/2-150,height-150,100,-10,50);
  vis_c_speed.setTitle("Speed");
  vis_c_speed.setValueUnit("km/h");
  vis_c_speed.setShowMinMax(true);
  
  //RPM Man
  vis_c_rpm_mean = new Tacho(width/2-400,height-150,75,minRPMDisplayed,maxRPMDisplayed);
  vis_c_rpm_mean.setTitle("RPM");
  vis_c_rpm_mean.setValueUnit("rpm");
  vis_c_rpm_mean.setShowMinMax(true);
  
  //Current
  color c_currentall=color(240,255,50);
  vis_currentAll = new Tacho(width/2+150,height-150,100,minCurrentDisplayed,maxCurrentDisplayed);
  vis_currentAll.setTitle("currentAll");
  vis_currentAll.setValueUnit("A");
  vis_currentAll.setcmain(c_currentall); //currentAll color
  vis_currentAll.setShowMinMax(true);
  vis_currentAll.setValue2Unit("A");
  vis_currentAll.setTitle2("avgCurrent/wheel");
  vis_currentAll.setcmain2(c_current);
  
  //Graph
  PVector size_vis_graph1= new PVector(400,200);
  PVector pos_vis_graph1= new PVector(width-size_vis_graph1.x-60,250);
  
  vis_graph_speed_mean = new GraphRoll_minimal(int(pos_vis_graph1.x), int(pos_vis_graph1.y), int(size_vis_graph1.x),int(size_vis_graph1.y),minRPMDisplayed,maxRPMDisplayed,1);
  vis_graph_speed_mean.setcborder(c_speed);
  vis_graph_speed_mean.setcmain(c_speed);
  vis_graph_speed_mean.setValueUnit("rpm");
  
  vis_graph_currentAll = new GraphRoll_minimal(int(pos_vis_graph1.x), int(pos_vis_graph1.y),  int(size_vis_graph1.x),int(size_vis_graph1.y),minCurrentDisplayed,maxCurrentDisplayed,1);
  vis_graph_currentAll.setcborder(c_currentall);
  vis_graph_currentAll.setcmain(c_currentall);
  vis_graph_currentAll.setValueUnit("A");
  
  color c_receivedelay=color(150,150,150);
  vis_c_graph_receivedelay = new GraphRoll_minimal(5, vis_textsize*2+40, 200,40,0,1000,1);
  vis_c_graph_receivedelay.setcborder(c_receivedelay);
  vis_c_graph_receivedelay.setcmain(c_receivedelay);
  vis_c_graph_receivedelay.setValueUnit("ms");
  vis_c_graph_receivedelay.setTitle("receivedelay");
  
  
  vis_c_rpmvector = new VectorDisplay(int(pos_vis_cmd.x+dist_vis_cmd.x/2+size_vis_cmd.x/2), int(pos_vis_cmd.y+dist_vis_cmd.y/2), 50, maxRPMDisplayed/20); 
  vis_c_rpmvector.setcmain(c_speed);
  
  vis_c_currentvector = new VectorDisplay(int(pos_vis_cmd.x+dist_vis_cmd.x/2+size_vis_cmd.x/2), int(pos_vis_cmd.y+dist_vis_cmd.y/2+50), 50, maxCurrentDisplayed/20); 
  vis_c_currentvector.setcmain(c_current);
  
  
  if (showTimeline) {
    println("Preparing Timeline");
    tl = new Timeline(30,height-30, width-30*2, 28);
    tl.setTimes(logdata_start_time,logdata_end_time);
    tl.generatePreview(logdata);
    println("Timeline prepared");
  }
  
}

void draw() {
  int loopmillis=0;
  if (running) {
    loopmillis=millis()-timeoffset;
  }else{ //paused
    loopmillis=timePaused-timeoffset;
  }
  
  if (useSerial) {
    if (serial.available() > 0) {
      serialString = serial.readStringUntil(serial_endchar);
      //println("read:"+serialString);
      if (serialString != null) {
        println(serialString);
        String[] list = split(serialString, ',');
        
        if (list.length==20) { //data ok
          lastTimeMillis=nextTimeMillis;
          nextTimeMillis=loopmillis;
          
          lastTimeData=nextTimeData;
          nextTimeData=int(parseFloat(list[0])*1000);        
          cmd_FrontL=parseInt(list[1]);
          cmd_FrontR=parseInt(list[2]);
          cmd_RearL=parseInt(list[3]);
          cmd_RearR=parseInt(list[4]);
          current_FrontL=parseFloat(list[5]);
          current_FrontR=parseFloat(list[6]);
          current_RearL=parseFloat(list[7]);
          current_RearR=parseFloat(list[8]);
          rpm_FrontL=parseInt(list[9]);
          rpm_FrontR=parseInt(list[10]);
          rpm_RearL=parseInt(list[11]);
          rpm_RearR=parseInt(list[12]);
          temp_Front=parseFloat(list[13]);
          temp_Rear=parseFloat(list[14]);
          vbat_Front=parseFloat(list[15]);
          vbat_Rear=parseFloat(list[16]);
          currentAll=parseFloat(list[17]);    
          throttle=parseInt(list[18]);
          brake=parseInt(list[19]);
          speed=parseFloat(list[20]);
          trip=parseFloat(list[21]);
          currentConsumed=parseFloat(trim(list[22]));
        }else{ //data missing or too much 
          dataErrorCount++;
        }
        
      }
    }
  }else{
    while (newdataforced || loopmillis>=nextTimeData && nextID+1<logdata.getRowCount()){ //New Data
      newdataforced=false; //reset flag
      if ((nextID+1 < logdata.getRowCount()) && (nextID >= 0)) { //valid row
        TableRow row = logdata.getRow(nextID);
        
        lastTimeData=nextTimeData;
        nextTimeData=(int)(logdata.getRow(nextID+1).getFloat("time")*1000); //get time and convert from seconds to ms
        
        lastTimeMillis=nextTimeMillis;
        nextTimeMillis=loopmillis;
        
        cmd_FrontL=row.getInt("cmd_FrontL");
        cmd_FrontR=row.getInt("cmd_FrontR");
        cmd_RearL=row.getInt("cmd_RearL");
        cmd_RearR=row.getInt("cmd_RearR");
        current_FrontL=row.getFloat("current_FrontL");
        current_FrontR=row.getFloat("current_FrontR");
        current_RearL=row.getFloat("current_RearL");
        current_RearR=row.getFloat("current_RearR");
        rpm_FrontL=row.getInt("rpm_FrontL");
        rpm_FrontR=row.getInt("rpm_FrontR");
        rpm_RearL=row.getInt("rpm_RearL");
        rpm_RearR=row.getInt("rpm_RearR");
        temp_Front=row.getFloat("temp_Front");
        temp_Rear=row.getFloat("temp_Rear");
        vbat_Front=row.getFloat("vbat_Front");
        vbat_Rear=row.getFloat("vbat_Rear");
        currentAll=row.getFloat("currentAll");    
        throttle=row.getInt("throttle");
        brake=row.getInt("brake");
        speed=row.getFloat("speed");
        trip=row.getFloat("trip");
        currentConsumed=row.getFloat("currentConsumed");
          
      if (loopmillis-nextTimeData>1000 && nextTimeData>lastTimeData) {//too much behind
        long _timestep=nextTimeData-lastTimeData; //approximated time step
        nextID+=(loopmillis-nextTimeData)/_timestep* 0.9; //fast forward estimated time steps
      }
      nextID++;
      }
    }
  }
  
  
  
  
  background(bg);
  
  vis_cmd_FrontL.setValue(cmd_FrontL); vis_cmd_FrontL.drawVis();
  vis_cmd_FrontR.setValue(cmd_FrontR); vis_cmd_FrontR.drawVis();
  vis_cmd_RearL.setValue(cmd_RearL); vis_cmd_RearL.drawVis();
  vis_cmd_RearR.setValue(cmd_RearR); vis_cmd_RearR.drawVis();
  
  vis_rpm_FrontL.setValue(rpm_FrontL); vis_rpm_FrontL.drawVis();
  vis_rpm_FrontR.setValue(rpm_FrontR); vis_rpm_FrontR.drawVis();
  vis_rpm_RearL.setValue(rpm_RearL); vis_rpm_RearL.drawVis();
  vis_rpm_RearR.setValue(rpm_RearR); vis_rpm_RearR.drawVis();
  
  vis_current_FrontL.setValue(current_FrontL); vis_current_FrontL.drawVis();
  vis_current_FrontR.setValue(current_FrontR); vis_current_FrontR.drawVis();
  vis_current_RearL.setValue(current_RearL); vis_current_RearL.drawVis(); 
  vis_current_RearR.setValue(current_RearR); vis_current_RearR.drawVis(); 
  
  vis_throttle.setValue(throttle); vis_throttle.drawVis();
  vis_brake.setValue(brake); vis_brake.drawVis();
  
  vis_c_speed.setValue(speed*3.6); vis_c_speed.drawVis(); //from m/s in km/h
  
  textAlign(LEFT);
  textSize(vis_textsize);
  text(trip+" m", width/2-10,height-110); //trip
  text(currentConsumed+" Ah", width/2-10,height-110+vis_textsize); //consumed Current
  
  int speed_mean=int((rpm_FrontL+rpm_FrontR+rpm_RearL+rpm_RearR)/4.0);
  vis_c_rpm_mean.setValue(speed_mean); vis_c_rpm_mean.drawVis();
  
  
  vis_currentAll.setValue(currentAll); 
  float current_mean=(current_FrontL+current_FrontR+current_RearL+current_RearR)/4.0;
  vis_currentAll.setValue2(current_mean); vis_currentAll.drawVis();
  
  vis_graph_speed_mean.setValue(speed_mean); vis_graph_speed_mean.drawVis();
  vis_graph_currentAll.setValue(currentAll); vis_graph_currentAll.drawVis();
  
  
  vis_c_graph_receivedelay.setValue(loopmillis-lastTimeMillis); vis_c_graph_receivedelay.drawVis();
  
  vis_c_rpmvector.setValue2((-rpm_FrontL-rpm_FrontR+rpm_RearL+rpm_RearR)/4.0/sqrt(2)); //y component
  vis_c_rpmvector.setValue((-rpm_FrontL+rpm_FrontR-rpm_RearL+rpm_RearR)/4.0/sqrt(2)); //x component
  vis_c_rpmvector.drawVis();
  
  vis_c_currentvector.setValue2((-current_FrontL-current_FrontR+current_RearL+current_RearR)/4.0/sqrt(2)); //y component
  vis_c_currentvector.setValue((-current_FrontL+current_FrontR-current_RearL+current_RearR)/4.0/sqrt(2)); //x component
  vis_c_currentvector.drawVis();
  
  
  
  //Temperature
  PVector pos_temperature = new PVector(500,12);
  colorMode(HSB, 360, 100, 100);
  fill(color(map(temp_Front,16,50,180,360),50,100));
  text("temp_Front="+(temp_Front)+"°C", pos_temperature.x,pos_temperature.y);
  fill(color(map(temp_Rear,16,50,180,360),50,100));
  text("temp_Rear="+(temp_Rear)+"°C", pos_temperature.x,pos_temperature.y+12);
  //Voltage
  PVector pos_voltage = new PVector(pos_temperature.x+150,12);
  colorMode(HSB, 360, 100, 100);
  fill(color(map(vbat_Front,12*3,12*4.2,0,120),50,100));
  text("vbat_Front="+(vbat_Front)+"V", pos_voltage.x,pos_voltage.y);
  fill(color(map(vbat_Rear,12*3,12*4.2,0,120),50,100));
  text("vbat_Rear="+(vbat_Rear)+"V",  pos_voltage.x,pos_voltage.y+12);
  
  
  colorMode(RGB, 255, 255, 255);
  fill(color(200,200,200));
  textAlign(LEFT);
  textSize(vis_textsize);
  text("d="+(nextTimeData-lastTimeData)+"ms", 5+75,12);
  if (!useSerial && loopmillis-lastTimeData>(nextTimeData-lastTimeData)*10) { //deviation too high when reading from file
    text("ff="+(loopmillis-lastTimeData)+"ms", 5+75*2,12); //show warning
  }
  if (!running) {
    fill(color(255,100,100));
  }
  text("t="+(loopmillis/1000.0)+"s", 5,12);
  text("nextID="+nextID, 5,12+12);
  fill(color(200,200,200));
  
  
  
  text(""+(dataErrorCount)+" errors", 5+70*3,12);
  
  
  if(showTimeline) {
    tl.drawTL(loopmillis/1000.0);
  }
  
  
  
}

void keyPressed() {
  if (key == CODED) {
    if (!running) { //paused
      if (keyCode == LEFT) {
        
      } else if (keyCode == RIGHT) {
        timeoffset=timePaused-nextTimeData;
      } 
    }
    
    
  } else {
    //println("key="+keyCode);
    if (keyCode==82) { //82=r
      println("restart");
      timeoffset=millis(); //rest to time 0
      nextID=0;
      newdataforced=true;
    }
    if (keyCode == 32) {
      if (running) { //switching from running to pause
        timePaused=millis();
      }else{ //unpause
        timeoffset=+millis()-timePaused+timeoffset;
      }
      running=!running;
    }
  }
}

void mouseClicked()
{
  if (showTimeline)  
  {
    int temp_timeoffset=(int)(tl.checkMouse(mouseX,mouseY)*1000);
    if (temp_timeoffset!=0) {
      
      if (!(nextID+1 < logdata.getRowCount()) && (nextID >= 0) || nextID<0) { //nextID not valid
        nextID=0; //rest to good value
      }
      
      print("Jump nextID from "+nextID);
      //find nextID to jump to
      while ((nextID+1 < logdata.getRowCount()) && (nextID >= 0) && (millis()-temp_timeoffset > (long)(logdata.getRow(nextID+1).getFloat("time")*1000))) { //jumped forward
        nextID++;
      }
      print(" over "+nextID);
      while ((nextID+1 < logdata.getRowCount()) && (nextID > 0) && (millis()-temp_timeoffset < (long)(logdata.getRow(nextID+1).getFloat("time")*1000))) { //jumped backward
        nextID--;
      }
      println(" to "+nextID);
      
      timeoffset=temp_timeoffset;
      newdataforced=true; //force read line for nextID
      
    }
  }
}