/* 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.*;
import processing.sound.*;

//for saving CSV values
PrintWriter output;

//for 3d view camera
PeasyCam cam;

//a beep sound.
SoundFile beep;
int beep_state = 1;


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

int myColor = color(135, 60, 90);

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



String portName;
int serialListIndex;

//THESE ARE (now less) WRONG
float BASE = 150;     // Base height above table in mm
float ARM1 = 210;     // Arm Segment 1 length in mm
float ARM2 = 220;     // 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 = 0.0;  // -167.0;   // Distance from E0 axis to preset position in mm
float Y_OFFSET = 0.0;    // Distance from E0 axis to preset position in mm
float Z_OFFSET = 50.0;


//angles from the setup of arm at home position
//THESE ARE (now less) WRONG
float presetAngle1 = radians(270); //radians(180);        // rotary base angle offset
float presetAngle2 = radians(60);
//THIS SOMEHOW WORKS?
//float presetAngle2 = radians(240);  //first arm angle offset // was 232.048
float presetAngle3 = radians(120);  //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 enc0Deg;
float enc1Deg;
float enc2Deg;

/*
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];

//our units start out as mm, but can be changed to inches.
String units = "mm";

int arm_hide = 0;

PFont f;

void setup() {
  frameRate(120);
  size(1000, 680, P3D);  //window size, tells it to be 3d mode.
  colorMode(HSB);
 
  f = createFont("Arial", 20);
 
  cam = new PeasyCam(this, 500, 300, 0, 700); //turns on 3d camera, center views, in x, y, z, pushes out
  cam.setMinimumDistance(30);      // closest you can zoom in. 
  cam.setMaximumDistance(1600);    // farthest you can zoom out.
  
  cp5 = new ControlP5(this);  // this is for GUI and buttons
  cp5.setAutoDraw(false);  
  

  if (Serial.list() !=  null ) {  
    try{
      println("Serial Ports Available: ");
      println(Serial.list());                // get list of all serial ports
      String portName = Serial.list()[0];
      myPort = new Serial(this, portName, 115200);  //baudrate don't work over USB serial
      println("Connecting to: " + portName );
   } catch (Exception e){
       println("error opening serial port ");
       e.printStackTrace();
       println ("Exiting.");
       exit();
   }
   println( "Units are in: " + units );
}
  
  
  strokeWeight(8);  //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, 50, 40, 80, 20).setId(1);  // test button - changes colors
  cp5.addButton("Save File", 20, 50, 65, 80, 20).setId(2);  // saves CSV file if pressed.
  cp5.addButton("Remove Last Point", 20, 50, 90, 80, 20).setId(3);  // saves CSV file if pressed.
  cp5.addButton("Beep on/off", 20, 50, 115, 80, 20).setId(5);  // Turn Beep Off
  cp5.addButton("Hide Arm", 20, 50, 140, 80, 20).setId(6);  // Turn Beep Off
  cp5.addButton("Units: ", 20, 50, 165, 80, 20).setId(4);  // Change the Units (WIP)
  cp5.addButton("Clear Points", 20, 20, height - 40, 80, 20).setId(7);  // CLEAR ALL THE POINT!!!
  
  //let's add some noise.
  beep = new SoundFile(this, "boop.wav");
}

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(", ");
        
        //Change the output based on units.
        if (units == "mm" ) {
          print(tipX);
          print(", ");
          print(tipY);
          print(", ");
          print(tipZ);
          println(" ");
        }
        else if ( units == "in" ) {
          print(tipX / 25.4);
          print(", ");
          print(tipY / 25.4);
          print(", ");
          print(tipZ / 25.4);
          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(" ");
            
            //play a beep for button press.
            if (beep_state == 1) {
              if (!beep.isPlaying()) {
                beep.play();
              }
            }
            
            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;
      if (arm_hide == 0) {
        translate(width/2, height - 150);
        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();
      
        xyz_axes();
      }
      
      //getRealPoints();
      
      
      //these are the points that have been saved, displayed on screen.
      for (int i = 1;  i < k; i++){
        fill(255, 240, 255);
        noStroke();
        pushMatrix();
        //rotateZ(radians(180));
        rotateX(radians(90));
        //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]); 
        box(3);
        popMatrix();
      }   
      

      gui();  // this is to keep buttons in same location
    
}


void getRealPoints() {
    enc0Deg = seg1Value * 360 / (STATES_PER_REV * 4);
    enc1Deg = seg2Value * 360 / (STATES_PER_REV * 2);
    enc2Deg = seg3Value * 360 / (STATES_PER_REV * 2);
    
    //starting states:
    //ENC0 = 0
    //ENC1 = -30
    //ENC2 = 70
    
    //testing degrees
    //println(enc0Deg + ", " + enc1Deg + ", " + enc2Deg); 
    
    float theta1 = enc0Deg;
    float theta2 = enc1Deg + degrees(presetAngle2);
    float theta3 = enc2Deg + degrees(presetAngle3);
    float theta4 = 180 - theta2 - 90;
    float theta5 = theta3 - theta4;
    
    //println( theta1 + ", " + theta2 + ", " +  theta3 + ", " +  theta4 + ", " + theta5);
    
    //get height of arms, Z
    float d3 = ARM1 * sin(theta2);
    float d6 = ARM2 * cos(theta5);
    float zed = Z_OFFSET + d3 - d6;
    
    // testing thetas
    //println( nf(theta4,0,2) + ", " + nf(theta5,0,2) + ", " +  nf(d3,0,2) + ", " +  nf(d6,0,2) + ", " + nf(zed,0,2));
    
    //get R (d1)
    float d4 = ARM1 * cos(theta2);
    float d5 = ARM2 * sin(theta5);
    float d1 = d4 + d5;
    
    float xxx = cos(theta1) * d1;
    float why = sin(theta1) * d1;
    
    // testing x, y and z
    //println(xxx + ", " + why + ", " + zed);
    
    
    // encoder 1 - rotary base encoder
    // This is with encoder at home position, and their preset angles. 
    // Encoder position = preset_angle / 360 * states_per_revolution
}



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




void xyz_axes() {

     pushMatrix();
      
     translate(-200,100,0); 
     stroke(255, 240, 255); // red
     line(0, 0, 0, -50, 0, 0);
     
     stroke(150, 240, 255); // blue
     line(0, 0, 0, 0, -50, 0);
    
     stroke(120, 230, 255); // green
     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]) { 
                    //Change the output based on units.
              if (units == "mm" ) {
                output.print(i);
                output.print(",");
                output.print(pointX[i]);
                output.print(",");
                output.print(pointY[i]);
                output.print(",");
                output.println(pointZ[i]);
              }
              else if ( units == "in" ) {
                output.print(i);
                output.print(",");
                output.print(pointX[i] / 25.4);
                output.print(",");
                output.print(pointY[i] / 25.4);
                output.print(",");
                output.println(pointZ[i] / 25.4);
              }

          }
       }
    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();
  //units text
  fill(255);
  textSize(16);
  textFont(f);
  text(units, 140,185);
  
  String dataPointsMM = (tipX + ", " + tipY + ", " + tipZ);
  String dataPointsIN = (nf(tipX/25.4,0,4) + ", " + nf(tipY/25.4,0,4) + ", " + nf(tipZ/25.4,0,4)); //nf is for rounding to 3 decimals
  
  //tip location
  
  if (units == "mm" ) {
    text("  X,       Y,        Z", 50, 210);
    text(dataPointsMM, 50, 240);
    }
  else if ( units == "in" ) {
    text("  X,          Y,            Z", 50, 210);
    text(dataPointsIN, 50, 240);
    }
  
  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;
 
    case(4):  // Change Units from MM to inches.
    if (units == "mm" ){
      units = "in";
    } else if ( units == "in" ){
      units = "mm";
    }
    println("Change units to: " + units);
    break;
    
    case(5): //turn beep on/off
    if (beep_state == 1 ){
      beep_state = 0;
      println ("beeper on.");
    } else {
      beep_state = 1;
      println ("beeper on.");
    }
    break;
    

    
    case(6): //ARM HIDE
    if (arm_hide == 1 ){
      arm_hide = 0;
      println ("Show Arm.");
    } else {
       arm_hide = 1;
      println ("Hide Arm.");
    }
    break;
    
    case(7): //CLEAR ALL POINTS!!!!!
    println("REMOVING A LOT OF POINTS");
    if (k > 0 ) {
      for (int i = 1; i <= k; i++){
      removePoint();
      }
    }
    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);
    sphere(15);
    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);
}