import processing.serial.*;
String serial_port="COM3";
Serial serial;
String serialString=""; //last read string
int serial_endchar=10; //10=ASCII Linefeed

static int SERVO_COUNT=5;
int servoPositionsActual[]=new int[SERVO_COUNT];

PrintWriter fileoutput;
String recorddir="recordings/";

boolean recording=false;
String filename="";
int recordingStartTime;
int linesWritten;

void setup() {
  size(800,600);
  background(0);
  

  printArray(Serial.list());
  // Open the port you are using at the rate you want:
  serial = new Serial(this, serial_port, 57600);
  
  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;


  
}

void draw() {
  clear();
  
  textSize(30);
  textAlign(LEFT);
  text("Press R to start recording.\nPress S to stop recording.",20,height-70);
  
  
  int valRange=1024; //maximum value for bargraph scale
  
  String[] list=readSerial();
  if (list!=null) {
    
    if (list[0].equalsIgnoreCase("dxlgp")) { //is position message
        
      boolean firstValueWritten=false;
      for (int i=1;i<list.length;i++) {
        String _number=list[i].replaceAll("[^0-9.]", ""); //remove non digit characters
        print(i); print("="); print(_number); print(" parsed="); println(parseInt(_number));
        servoPositionsActual[i-1]=parseInt(_number);
         
         
        if (recording) {
          if (!firstValueWritten) {
            fileoutput.print( (millis()-recordingStartTime)/1000.0); //write time
          }else{
            fileoutput.print(","); 
          }
          fileoutput.print(_number);
        }
        firstValueWritten=true;
        
      }
      
      if (recording) {
        linesWritten++;
        fileoutput.println();
        fileoutput.flush();
      }
      
      
    }
  }
  

  //Draw Recording Info
  if (recording) {
    textSize(16);
    textAlign(RIGHT);
    text(filename,width-10,10+50+20);
    text("t="+nf((millis()-recordingStartTime)/1000.0, 1,3) ,width-10,10+50+20+20);
    text("lines="+linesWritten ,width-10,10+50+20+20+20);
    
    fill(255,0,0);
  }else{
    fill(50,50,50);
  }
  rect(width-50-10,10,50,50);
  
  
  for (int i=0;i<servoPositionsActual.length;i++) {
    
    drawBargraph(servoPositionsActual[i],valRange,10,30+50*i);
  }
  
}

void keyPressed() {
  if (key == CODED) {
    
  }else{
    //print("Code=");println(keyCode);
    if (keyCode==82) { //r
      if (!recording) {
        filename=year()+"-"+month()+"-"+day()+"_"+hour()+"-"+minute()+"-"+second()+".txt";
        fileoutput = createWriter(recorddir+""+filename);
        print("Recording started. Filename: "); println(filename);
        recording=true;
        recordingStartTime=millis();
        
        //Write header
        fileoutput.println("time,servo0,servo1,servo2,servo3,servo4");
      }
      
    }else if(keyCode==83) { //s
      if (recording) {
        fileoutput.close();
        recording=false;
        println("Stopped Recording");
      }
    }
  }
}




void drawBargraph(float val, float maxvalue, int posX, int posY) {
  int graphW=300;
  int graphH=20;
  
  fill(255);
  rect(posX,posY,val*graphW/maxvalue,graphH); //graph
  
  noFill();
  stroke(127);
  rect(posX,posY,graphW,graphH); //border
  
  textAlign(CENTER);
  textSize(20);
  text(val,posX+graphW/2,posY);
}


String[] readSerial() {
  if (serial.available() > 0) {
      serialString = serial.readStringUntil(serial_endchar);
      //println("read:"+serialString);
      if (serialString != null) {
        println(serialString);
        String[] list = split(serialString, ',');
        return list;
      }
  }
  return null;
}