int vis_textsize=12; //copy from Visualization class

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_speed_FrontL;
Visualization vis_speed_FrontR;
Visualization vis_speed_RearL;
Visualization vis_speed_RearR;


long lastTimeData=0; //last time data received


Table logdata;
int nextID=0; //next row number to be displayed
long nextTime=0; //time of nextID row

//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 speed_FrontL;
int speed_FrontR;
int speed_RearL;
int speed_RearR;
float temp_Front;
float temp_Rear;
float vbat_Front;
float vbat_Rear;
float currentAll;
int throttle;
int brake;

void setup() {
  size(1920, 1080);
  frameRate(100);
  
  logdata = loadTable("LOG00008_rumfahren_neu.TXT", "header, csv");
  
  println("loaded "+logdata.getRowCount()+" lines. Times: "+logdata.getRow(0).getFloat("time")+"s to "+logdata.getRow(logdata.getRowCount()-1).getFloat("time")+"s");
  
  PVector pos_vis_cmd = new PVector(100,150);
  PVector size_vis_cmd = new PVector(10,100);
  PVector dist_vis_cmd = new PVector(80,150);
  
  //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_FrontL.setTitle("");
  
  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_RearL.setTitle("");
  
  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_RearR.setTitle("");
  
  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_speed_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,-100,600);
  vis_speed_FrontL.setTitle("");
  
  vis_speed_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,-100,600);
  vis_speed_FrontR.setTitle("speed");
  
  vis_speed_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,-100,600);
  vis_speed_RearL.setTitle("");
  
  vis_speed_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,-100,600);
  vis_speed_RearR.setTitle("");
  
  vis_speed_FrontL.setcmain(c_speed);
  vis_speed_FrontR.setcmain(c_speed);
  vis_speed_RearL.setcmain(c_speed);
  vis_speed_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,-1,10);
  vis_current_FrontL.setTitle("");
  
  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,-1,10);
  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,-1,10);
  vis_current_RearL.setTitle("");
  
  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,-1,10);
  vis_current_RearR.setTitle("");
  
  vis_current_FrontL.setcmain(c_current);
  vis_current_FrontR.setcmain(c_current);
  vis_current_RearL.setcmain(c_current);
  vis_current_RearR.setcmain(c_current);
  
  
  
}

void draw() {
  long loopmillis=millis()+25000;
  if (loopmillis>=nextTime){
    TableRow row = logdata.getRow(nextID);
    lastTimeData=nextTime;
    nextTime=(long)(row.getFloat("time")*1000); //get time and convert from seconds to ms
    
    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");
    speed_FrontL=row.getInt("speed_FrontL");
    speed_FrontR=row.getInt("speed_FrontR");
    speed_RearL=row.getInt("speed_RearL");
    speed_RearR=row.getInt("speed_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");
        
    if (loopmillis-nextTime>1000) {//too much behind
      long _timestep=nextTime-lastTimeData; //approximated time step
      nextID+=(loopmillis-nextTime)/_timestep* 0.9; //fast forward estimated time steps
    }
    nextID++;
    nextID=nextID%logdata.getRowCount();
  }
  
  
  
  
  background(0,0,0);
  
  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_speed_FrontL.setValue(speed_FrontL); vis_speed_FrontL.drawVis();
  vis_speed_FrontR.setValue(speed_FrontR); vis_speed_FrontR.drawVis();
  vis_speed_RearL.setValue(speed_RearL); vis_speed_RearL.drawVis();
  vis_speed_RearR.setValue(speed_RearR); vis_speed_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();
  
  
  
  
  fill(color(200,200,200));
  textAlign(LEFT);
  textSize(12);
  text("d="+(nextTime-lastTimeData)+"ms", 5+70,12);
  if (loopmillis-lastTimeData>(nextTime-lastTimeData)*10) { //
    text("ff="+(loopmillis-lastTimeData)+"ms", 5+70+70,12);
  }
  
  text("t="+(loopmillis/1000.0)+"s", 5,12);
}