/* sgn june 2022
* much of this code is mine, however a lot, including the "3d arm model" system come from others.
* the arm is from : "Nekodigi" found at the below link:
* https://nekodigi.hatenablog.com/entry/2020/01/20/%E3%80%90Processing%E3%80%91ForwardKinematics%E3%81%A7%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%E3%82%A2%E3%83%BC%E3%83%A0%E3%82%92%E5%8B%95%E3%81%8B%E3%81%99
*
* and a bunch of help was provided by looking at one of the original arm processing apps from
* "DZL THEEVILGENIUS"
* found here: http://blog.dzl.dk/2018/08/21/3d-digitizer/
*/

import processing.serial.*;  // import library to handle serial port
import controlP5.*; // for user interface
import peasy.*;   // for 3d camera and mouse control of view
import processing.opengl.*;

//for saving CSV values
PrintWriter output;

//for 3d view camera
PeasyCam cam;

int cp5buttonValue = 1;  // test button
int cp5buttonValue2 = 2;  // save CSV file button

int myColor = color(255, 0, 0);

Serial myPort;   // Create object from Serial class
ControlP5 cp5;  // for gui


String portName;
int serialListIndex;

//THESE ARE (now less) WRONG
float BASE = 200;     // Base height above table in mm
float ARM1 = 175;     // Arm Segment 1 length in mm
float ARM2 = 175;     // Arm Segment 2 length in mm
//THESE ARE WRONG
//float X_OFFSET = -125.0;   // Distance from E0 axis to preset position in mm
//float Y_OFFSET = 125.0;    // Distance from E0 axis to preset position in mm
//float Z_OFFSET = 200.0;

float X_OFFSET = -25.0;   // Distance from E0 axis to preset position in mm
float Y_OFFSET = 25.0;    // Distance from E0 axis to preset position in mm
float Z_OFFSET = 150.0;


//angles from the setup of arm at home position
//THESE ARE (now less) WRONG
float presetAngle1 = radians(0);        // rotary base angle offset
float presetAngle2 = radians(232.048);  //first arm angle offset
float presetAngle3 = radians(-113.174);  //second arm angle offset

//THESE NEED TO BE PLAYED WITH
float STATES_PER_REV = 2000;  //Encoder half of number of counts per revolution. Why half?

String val;      // Data received from the serial port

String[] encoders;  // arrays to hold encoder data temporarily

// direct value from encoders
int seg1Value = 0;   
int seg2Value = 0;
int seg3Value = 0;

int button_state = 1;  // physical button from unit

Arm arm;   // call the Arm Class

float angle0;   // Prepare the angle variables
float angle1;   
float angle2;

/*
float previousAngle0;   // Prepare the angle variables
float previousAngle1;   
float previousAngle2;
*/

//location of the tip in x, y, z plane.
// data is coming direct from samd21 board
float r;       // radius from center to tip 
float tipX;
float tipY;
float tipZ;


//this is our array counter
int k = 1;
int previous_k = 1;
//number of points to store
int amount = 1024 * 10;
//store values of saved points
float pointX[] = new float[amount];
float pointY[] = new float[amount];
float pointZ[] = new float[amount];

void setup() {
  frameRate(90);
  size(1000, 680, P3D);  //window size, tells it to be 3d mode.
  colorMode(HSB);
  
 
  cam = new PeasyCam(this, 500, 300, 0, 700); //turns on 3d camera, center views, in x, y, pushes out
  cam.setMinimumDistance(50);      // closest you can zoom in. 
  cam.setMaximumDistance(1300);    // farthest you can zoom out.
  
  cp5 = new ControlP5(this);  // this is for GUI and buttons
  cp5.setAutoDraw(false);  
  
  println("Serial Ports Available: ");
  println(Serial.list());                // get list of all serial ports
  String portName = Serial.list()[0];    // Open whatever port is the one you're using. [0] is first, [1] is second, etc
  myPort = new Serial(this, portName, 115200);  //baudrate don't work over USB serial
  
  strokeWeight(10);  //arm width
  arm = new Arm(new PVector(0, 0));  //builds the arm structure
  arm.addBone(BASE, 0);  // base of the arm
  arm.addBone(ARM1, 2);  // first limb (middle)
  arm.addBone(ARM2, 2);  // second limb (top)

  cp5.addButton("Change Color", 10, 100, 60, 80, 20).setId(1);  // test button - changes colors
  cp5.addButton("Save File", 20, 100, 100, 80, 20).setId(2);  // saves CSV file if pressed.
  cp5.addButton("Remove Last Point", 20, 100, 140, 80, 20).setId(3);  // saves CSV file if pressed.
  

}

void draw(){
   background(0);  //black background
   lights();       // turns on 3d lights, barely works.  
   
    if ( myPort.available() > 0) {                //read incoming data from serial port
      val = myPort.readStringUntil('\n');         // read it and store it in val
      if(val != null) {            // because processing likes to shit itself with nulls when reading serial.
        //println(val);            // print the value we're receiving from serial port
    
        encoders = trim(splitTokens(val, ","));  //split on comma
        
        //takes serial data from board, sends to individual variables
        seg1Value = int(encoders[0]);    // encoder 1 - rotary base encoder
        seg2Value = int(encoders[1]);    // encoder 2 - first arm
        seg3Value = int(encoders[2]);    // encoder 3 - second arm (end effector)
        button_state = int(encoders[3]); // button
        r = float(encoders[4]);           // r
        tipX = float(encoders[5]);       // X tip position
        tipY = float(encoders[6]);       // Y tip position
        tipZ = float(encoders[7]);       // Z tip position
        
        print(seg1Value);  // for testing, as always.
        print(", ");
        print(seg2Value);
        print(", ");
        print(seg3Value);
        print(", ");
        print(button_state);
        print(", ");
        print(r);
        print(", ");
        print(tipX);
        print(", ");
        print(tipY);
        print(", ");
        print(tipZ);
        println(" ");
        
        
        if (button_state == 0) {  //if button pushed...
          // and none of the values are the same as the previous values...
          if ( tipX != pointX[k - 1] && tipY != pointY[k - 1] && tipZ != pointZ[k - 1]) { 
            
            //save the X, Y, and Z values in the arrays below
            pointX[k] = tipX;
            pointY[k] = tipY;
            pointZ[k] = tipZ;
            print(" VALUED SAVED: X: ");
            print(pointX[k]);
            print(", Y: ");
            print(pointY[k]);
            print(", Z: ");
            print(pointZ[k]);
            print(", POINT: ");
            print(k);
            println(" ");
            
            k++;  // prepare to save next point
          }
        }
        
      }
   }
        
      angle0 = seg1Value * PI * 2 / (STATES_PER_REV * 4);  // * 4 due to different encoders
      angle1 = seg2Value * PI * 2 / (STATES_PER_REV * 2);  // top
      angle2 = seg3Value * PI * 2 / (STATES_PER_REV * 2);  //turn into degrees
      /*
      print(angle0);  // for testing, as always.
      print(", ");
      print(angle1);
      print(", ");
      print(angle2);
      println(" ");
      */

      //float angle1 = (float)mouseX/500;
      //float angle2 = (float)mouseY/500;
      translate(width/2, height - 50);
      arm.show();
      arm.setAngle(0, angle0 + presetAngle1);   // base (og angle2)
      arm.setAngle(1, angle1 + presetAngle2);   // mid  (og angle0)
      arm.setAngle(2, angle2 + presetAngle3);   // end  (og angle1)
      //angle0 += 0.01;
      
      //make a box for arm to sit on.
      arm_base();
      
      //these are the points that have been saved, displayed on screen.
      for (int i = 1;  i < k; i++){
        fill(myColor);
        noStroke();
        pushMatrix();
        rotateX(PI/2);   // rotate so everything is in correct plane. Don't know why I need this though honestly
                         // found out why I need it, the arm defaults to a sideways orientation!
        translate(pointX[i] - X_OFFSET, pointY[i] - Y_OFFSET, pointZ[i] - Z_OFFSET);  // add offsets for start position, may need to change later
        //translate(pointX[i], pointY[i], pointZ[i]); 
        sphere(5);
        popMatrix();
      }
      
      //draw axes
      xyz_axes();
      
      gui();  // this is for the buttons
    
}

//make a box for arm to sit on.
void arm_base() {  
      // this also changes colors, for the button test.
      fill(myColor);
      pushMatrix();
      translate(0, 10, 0);
      //rotateY(1.25);
      //rotateX(-0.4);
      noStroke();
      box(200,10,200);
      popMatrix();
}


void xyz_axes() {

     pushMatrix();
      
     translate(-200,0,0); 
     stroke(255, 240, 255); // red
     line(0, 0, 0, 50, 0, 0);
     stroke(120, 230, 255); // green
    
     line(0, 0, 0, 0, 50, 0);
     stroke(150, 240, 255); // blue?
  
     line(0, 0, 0, 0, 0, 50);
     popMatrix();
   }
  
 
// This saves the data to a CSV .txt file
// file name is number of points saved plus a random value. I'll due date/time sometime in future.
void dataOut() {
    output = createWriter(k + "-" + int(random(1000)) + "positions.txt");  // save file name, need to give unique names; date or at least random
        for (int i = 1; i < k; i++){
          if ( pointX[i] != pointX[i - 1] && pointY[i] != pointY[i - 1] && pointZ[i] != pointZ[i - 1]) { 
            output.print(i);
            output.print(",");
            output.print(pointX[i]);
            output.print(",");
            output.print(pointY[i]);
            output.print(",");
            output.println(pointZ[i]);
          }
       }
    output.flush();
    output.close();  
}
  
  
//remove value from array
//from: https://stackoverflow.com/questions/2459780/best-way-to-remove-an-object-from-an-array-in-processing
float[] removeByIndex(float[] array, int index) {
  int index2 = array.length-1;
  float old = array[index];
  array[index] = array[index2];
  array[index2] = old;
  array = shorten(array);
  return array;
}  
  
//remove a previous saved point  
void removePoint() {
  if (k > 0) {
    pointX = removeByIndex(pointX , k);
    pointY = removeByIndex(pointY , k); 
    pointZ = removeByIndex(pointZ , k);
    k--;
  }
}
   
//for having a GUI and peasycam at the same time.   
void gui() {
  hint(DISABLE_DEPTH_TEST);
  cam.beginHUD();
  cp5.draw();
  cam.endHUD();
  hint(ENABLE_DEPTH_TEST);
}   

 
//for cp5 button
void controlEvent(ControlEvent theEvent) {
  println(theEvent.getController().getId());
  switch(theEvent.getController().getId()) {
    case(1):  // Change Colors
    myColor = color(random(255), random(255), random(255));
    println("color changed: " + myColor);
    break;
    case(2):  // Save data
    dataOut();  
    println("File Saved. ");
    break;
    case(3):  // remove last point saved
    removePoint();
    println("Remove previous point: " + k);
    break;
  }
}

   
   
class Arm {
  
  ArrayList<Bone> bones = new ArrayList<Bone>();
  PVector pos;
  Arm(PVector pos){
  this.pos = pos;
  }

  void addBone(float len, int type){
    if(bones.isEmpty()){
      bones.add(new Bone(len, type));
    } else {
      bones.add(new Bone(bones.get(bones.size()-1), len, type));
    }
  }

  void setAngle(int index, float angle){
    bones.get(index).angle = angle;
  }
  
  void show(){
    pushMatrix();
    translate(pos);
    for (Bone bone : bones){
      bone.show();
    }
    popMatrix();
  }
}

class Bone {
  Bone parent;
  int type; // 0 ==yaw, 1 = pitch, 2 = roll
  float len;
  float angle;
  
  Bone(float len, int type){
    this.len = len;
    this.type = type;
  }
  
  Bone(Bone bone, float len, int type){
    this.parent = bone;
    this.len = len;
    this.type = type;
  }
  
  void show(){
    noStroke();
    fill(125, 100, 120);
    box(20);
    if(type == 0){
      rotateY(angle);
    } else if (type == 1) {
      rotateX(angle);
    } else if (type == 2) {
      rotateZ(angle);
    }
    stroke(255);
    line(0, 0, 0, 0, -len, 0);
    translate(0, -len, 0);
    noStroke();
    //box(20);
  }
}

void translate(PVector p) {
  rotateX(p.x);
  rotateY(p.y);
  rotateZ(p.z);
}

void line(PVector a, PVector b){
  line(a.x, a.y, a.z, b.x, b.y, b.z);
}