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;

//Matrix Weights
String mwheader[] = new String[32];
float matrixweights[][] = new float[32][32]; //[servo][weight]
int matrixrows=0; //when matrix received this will be the number of rows

float torquemultiplier=0;

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();
      }
      
      
    }else if (list[0].equalsIgnoreCase("mwheader")) { //is matrix weights header message
      for (int i=0;i<mwheader.length;i++) {
        mwheader[i]=""; //clear header array
      }
      for (int i=1;i<list.length;i++) {
        String entry=list[i].strip();
        mwheader[i-1]=entry;        
      }
      matrixrows=list.length-1;
      
      print("mwheader size="); println(calcMWHeaderLength());
      
      
      
    }else if (list[0].equalsIgnoreCase("mw")) { //is matrix weights header message
      int mwheaderlength=calcMWHeaderLength();
      int s=0;
      for (int i=1;i<list.length;i++) {        
        float _number=parseFloat(list[i].replaceAll("[^0-9.]", "")); //remove non digit characters
        int num=i-1; //position
        int w=num%mwheaderlength; //weight array pos
        s=num/mwheaderlength;
        matrixweights[s][w]=_number;
      }
      matrixrows=s+1;
      print("Matrixrows="); println(matrixrows);
    }else if (list[0].equalsIgnoreCase("torque")) { //is torque multiplier value
      torquemultiplier=parseFloat(list[1].replaceAll("[^0-9.]", "")); //remove non digit characters
      
    }
  }
  

  //Draw Recording Info
  if (recording) {
    textSize(16);
    textAlign(RIGHT);
    text(filename,width-10,height-200+10+50+20);
    text("t="+nf((millis()-recordingStartTime)/1000.0, 1,3) ,width-10,height-200+10+50+20+20);
    text("lines="+linesWritten ,width-10,height-200+10+50+20+20+20);
    
    fill(255,0,0);
  }else{
    fill(50,50,50);
  }
  ellipse(width-50-10,height-200+10,50,50);
  
  
  //Bargraphs
  for (int i=0;i<servoPositionsActual.length;i++) {
    
    drawBargraph(servoPositionsActual[i],valRange,10,30+50*i);
  }
  
  //Torque Multiplier
  textSize(16); color(255);
  textAlign(LEFT);
  text("Torque="+torquemultiplier, width-200,20);
  
  //Matrix Weights
  int matrixPosX=10;
  int matrixPosY=300;
  int matrixCellW=100;
  int matrixCellH=20;
  int mwheaderlength=calcMWHeaderLength();
  color(255);
  textAlign(LEFT);
  for (int s=0;s<matrixrows+1;s++){ //horizontal lines
    line(matrixPosX,matrixPosY+matrixCellH*s,matrixPosX+matrixCellW*mwheaderlength ,matrixPosY+matrixCellH*s);
  }
  for (int w=0;w<mwheaderlength+1;w++){ //vertical lines
    line(matrixPosX+matrixCellW*w,matrixPosY-matrixCellH,matrixPosX+matrixCellW*w ,matrixPosY+matrixCellH*matrixrows);
  }
  
  for (int h=0;h<mwheaderlength;h++){
    text(mwheader[h],matrixPosX+matrixCellW*h,matrixPosY); 
  }
  for (int s=0;s<matrixrows;s++){
     for (int w=0;w<mwheaderlength;w++){
       text(matrixweights[s][w],matrixPosX+matrixCellW*w,matrixPosY+matrixCellH*(s+1));
     }
     
  }
  
    
  
}

int calcMWHeaderLength() {
  int count=0;
  for (int i=0;i<mwheader.length;i++) {
    if (mwheader[i]!="" && mwheader[i]!=null) {
      count++;
    }
  }
  return count;
}

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");
      }
    }else if(keyCode==84){ //t
      //Matrix
      int mwheaderlength=calcMWHeaderLength();
      for (int h=0;h<mwheaderlength;h++){
        print(","); print(mwheader[h]); 
      }
      println();
      for (int s=0;s<matrixrows;s++){
         for (int w=0;w<mwheaderlength;w++){
           print(","); print(matrixweights[s][w]);
         }
         println();
      }
    }
    
  }
}




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;
}