/*
 * DeltaBot
 * v0.1
 * Author: Matt Greensmith
 * Date: 25 May 2011
 *
 * All the kinematics calculations are from the superb write-up at:
 * http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/
 */
 
 /*
  * Additional Part 
  * Impremented by Hiroya Tanaka 
  * Social Fabrication Laboratory Keio Univeristy 
  * http://sfc.sfc.keio.ac.jp/
  * Date 02 Jan 2016
 */

import java.util.regex.Pattern;
import java.util.regex.Matcher;
import java.io.File;
import java.io.FileWriter;
import java.io.BufferedWriter;
import java.io.IOException;
import java.util.StringTokenizer;
import java.awt.event.*;
import java.awt.dnd.*;
import java.awt.datatransfer.*; 
import java.awt.event.KeyEvent;
import javax.swing.JOptionPane;
import javax.swing.*;


import controlP5.*;
import processing.serial.*;

boolean debug = false;

// robot geometry
static float e = 14;     // end effector triangle
static float f = 29;     // base triangle
static float re = 50;    //length of long arm
static float rf = 14;     //length of short arm
 
// trigonometric constants
static float sqrt3 = sqrt(3.0);
static float pi = 3.141592653;    // PI
static float sin120 = sqrt3/2.0;   
static float cos120 = -0.5;        
static float tan60 = sqrt3;
static float sin30 = 0.5;
static float tan30 = 1/sqrt3;

//servo alignment
static int s1offset = -19;
static int s2offset = -14;
static int s3offset = -22;

//envelope boundaries
static float min_x = -20;
static float max_x = 20;
static float min_y = -20;
static float max_y = 20;
static float min_z = -64;
static float max_z = -36;
static float max_t = 80;
static float min_t = -45;

static int precision = 100; //round to 2 decimal places

float xp = 0;
float yp = 0;
float zp =-45;
float t1 = 0;  //servo angle t for 'theta', 1 for servo 1
float t2 = 0;
float t3 = 0;

//backups of previous positions in case we try to make an illegal move
float last_x;
float last_y;
float last_z;
float last_t1;
float last_t2;
float last_t3;

boolean validPosition;
boolean servosEnabled = false;
boolean recordEnabled = false;
boolean playEnabled = false;

//Record Motion
int maxcount = 10000;
float[][] recxyz = new float[3][maxcount];
int recordcount=0; 
int playcount=0;
int endcount=0;

// File IO
String getFile="";  
boolean streaming = false;
String[] gcode;


Serial myPort;
ControlP5 controlP5;
PFont font;
Textfield xtext;
Textfield ytext;
Textfield ztext;
Slider2D xy;
Slider z;
Slider servo1;
Slider servo2;
Slider servo3;

void setup() {
  //open serial connection
  //myPort = new Serial(this, Serial.list()[0], 9600);
  
 
  
  size(300,600);
  frameRate(25);
  controlP5 = new ControlP5(this);  

  controlP5.setAutoInitialization(false);
 
  // x axis text field
  xtext = controlP5.addTextfield("x",210,60,50,20);
  xtext.setId(1);
  xtext.setAutoClear(false);
  
  // y axis text field
  ytext = controlP5.addTextfield("y",210,100,50,20);
  ytext.setId(2);
  ytext.setAutoClear(false);
  
  //z axis text field
  ztext = controlP5.addTextfield("z",210,140,50,20);
  ztext.setId(5);
  ztext.setAutoClear(false);
  
  //x-y axis 2d slider
  xy = controlP5.addSlider2D("x-y-axis",20,200,200,200);
  xy.setBroadcast(false); 
  xy.setId(3);
  xy.setMinX(0);
  xy.setMaxX(40);
  xy.setMinY(0);  //minY is the top of the chart!
  xy.setMaxY(40);
  xy.setArrayValue(new float[] {20, 20});
  xy.setColorValueLabel(200);  //hide the incorrect labels
 
  //z azis slider
  z = controlP5.addSlider("z-axis",-64,-36,-45,230,200,15,200);
  z.setBroadcast(false); 
  z.setId(4);
  z.setSliderMode(Slider.FLEXIBLE);
  z.setColorValueLabel(200); 
  
  //servo sliders
  servo1 = controlP5.addSlider("servo1", -45, 80, 0, 20, 460, 200, 15);
  servo1.setBroadcast(false); 
  servo1.setId(6);
  servo1.setSliderMode(Slider.FLEXIBLE);
  
  servo2 = controlP5.addSlider("servo2", -45, 80, 0, 20, 480, 200, 15);
  servo2.setBroadcast(false); 
  servo2.setId(7);
  servo2.setSliderMode(Slider.FLEXIBLE);
  
  servo3 = controlP5.addSlider("servo3", -45, 80, 0, 20, 500, 200, 15);
  servo3.setBroadcast(false); 
  servo3.setId(8);
  servo3.setSliderMode(Slider.FLEXIBLE);
  
  //G-COde

   controlP5.addButton("GCODE")
     .setPosition(20,530)
     .setSize(100,20)
     ;
     
   controlP5.addButton("PRINT")
     .setPosition(20,560)
     .setSize(100,20)
     ;
     
  
  //enable switch
  controlP5.addToggle("enable", false, 20, 20, 50, 20).setMode(ControlP5.SWITCH);

  //record switch
  controlP5.addToggle("record", false, 20, 80, 50, 20).setMode(ControlP5.SWITCH);

  //play switch
  controlP5.addToggle("play", false, 20, 120, 50, 20).setMode(ControlP5.SWITCH);
 
  updateGuiElements();
  
  

 
}

void draw() {
  background(200);
  
  if (servosEnabled) {
    fill(200);
    text("SERVO POWER OFF", 80, 35); //hide
    fill(#00bb00);
    text("SERVO POWER ON", 80, 35); //show this
  } else {
    fill(200);
    text("SERVO POWER ON", 80, 35); //hide
    fill(#ff0000);
    text("SERVO POWER OFF", 80, 35); //show this
  }
  
  
  if(playEnabled == false){
   if (recordEnabled){
    fill(200);
    text("STOP", 80, 95); //hide
    fill(#00bb00);
    text(recordcount +" / " + maxcount, 80, 95); //show this
    
    //println("recording: " + recordcount + " ..." + xp + " - " + yp + " - " + zp);
    recxyz[0][recordcount]=  xp;
    recxyz[1][recordcount]=  yp;    
    recxyz[2][recordcount]=  zp;
    if(recordcount < maxcount){ recordcount = recordcount + 1; } else { recordcount =0; endcount= maxcount-1; recordEnabled = false; }

    
  } else {
    fill(200);
    text("RECODING", 80, 95); //hide
    fill(#ff0000);
    text("STOP", 80, 95); //show this
    if (recordcount >0) {endcount = recordcount; recordcount=0;}
    
  }
  }
  
    if (recordEnabled == false){      
   if (playEnabled) {
    fill(200);
    text("STOP", 80, 135); //hide
    fill(#00bb00);
    text(playcount + " / " + endcount, 80, 135); //show this
    
    xp=recxyz[0][playcount]; 
    yp=recxyz[1][playcount];     
    zp=recxyz[2][playcount]; 
    //println("playing: " + playcount + " / " + endcount +" ..." + xp + " - " + yp + " - " + zp);
        
    setThetasfromXYZ();
    updateGuiElements();   
    
  if (playcount < endcount) { playcount = playcount + 1; } else { playcount = 0; }
    
  } else{
    fill(200);
    text("PLAYING", 80, 135); //hide
    fill(#ff0000);
    text("STOP", 80, 135); //show this
    playcount=0;
  }
}



  /// G-code process
   if(getFile != "") {
   gcode = loadStrings(getFile);
   println(getFile);
   getFile="";
 }
 

  }

public void controlEvent(ControlEvent theEvent) {

  copyLastData(); //back up our last positions

  switch(theEvent.getController().getId()) {
    case (1): //x position text field has been changed
      xp = Float.valueOf(xtext.getStringValue()).floatValue();
      setThetasfromXYZ();
    break;

    case (2): //y position text field has been changed
      yp = Float.valueOf(ytext.getStringValue()).floatValue();
      setThetasfromXYZ();
    break;
    
    case (5): //z position text field has been changed
      zp = Float.valueOf(ztext.getStringValue()).floatValue();
      setThetasfromXYZ();
    break;
   
    case (3): //xy slider has changed
      xp = (xy.getArrayValue()[0] -20) * -1;       //slider is 0,100, convert to -50,50
      yp = (xy.getArrayValue()[1] -20) * -1;  //slider is 0,100, convert to -50,50, invert
      setThetasfromXYZ();
    break;
    
    case (4): //z slider
      zp = z.getValue();
      setThetasfromXYZ();   
    break;
    
    case (6)://servo1 slider
      t1 = servo1.getValue();
      setXYZfromThetas();
    break;
    
    case (7)://servo2 slider
      t2 = servo2.getValue();
      setXYZfromThetas();
    break;
    
    case (8)://servo3 slider
      t3 = servo3.getValue();
      setXYZfromThetas();
    break;   
 
  }
}

//a new xyz has been proposed, validate the position and calculate new servo angles.
void setThetasfromXYZ() {
  if ( debug ) { println("Entering: setThetasfromXYZ()"); }
  //first bounds-check the input
  if (xp < min_x) { xp = min_x; }
  if (xp > max_x) { xp = max_x; }
  if (yp < min_y) { yp = min_y; }
  if (yp > max_y) { yp = max_y; }
  if (zp < min_z) { zp = min_z; }
  if (zp > max_z) { zp = max_z; }
  
  validPosition = true;
  //set the first angle
  float theta1 = delta_calcAngleYZ(xp, yp, zp);
  if (theta1 != 999) {
    float theta2 = delta_calcAngleYZ(xp*cos120 + yp*sin120, yp*cos120-xp*sin120, zp);  // rotate coords to +120 deg
    if (theta2 != 999) {
      float theta3 = delta_calcAngleYZ(xp*cos120 - yp*sin120, yp*cos120+xp*sin120, zp);  // rotate coords to -120 deg
      if (theta3 != 999) {
        //we succeeded - point exists
        if (theta1 <= max_t && theta2 <= max_t && theta3 <= max_t && theta1 >= min_t && theta2 >= min_t && theta3 >= min_t ) { //bounds check
          t1 = theta1;
          t2 = theta2;
          t3 = theta3;
          SetServos();
        } else {
          validPosition = false;
        }

      } else {
        validPosition = false;
      }
    } else {
      validPosition = false;
    }
  } else {
    validPosition = false;
  }
  
  //uh oh, we failed, revert to our last known good positions
  if ( !validPosition ) {
    xp = last_x;
    yp = last_y;
    zp = last_z;
  }
        
  //finally update the gui elements
  updateGuiElements();
  if ( debug ) { println("Exiting: setThetasfromXYZ()"); }
}

//a new theta position has been proposed, validate the position and calculate new servo angles.
void setXYZfromThetas() {
  if ( debug ) { println("Entering: setXYZfromThetas()"); }  
  validPosition = true;

  float t = (f-e)*tan30/2;
  float dtr = pi/(float)180.0;
 
  float theta1 = dtr*t1;
  float theta2 = dtr*t2;
  float theta3 = dtr*t3;
 
  float y1 = -(t + rf*cos(theta1));
  float z1 = -rf*sin(theta1);
   
  float y2 = (t + rf*cos(theta2))*sin30;
  float x2 = y2*tan60;
  float z2 = -rf*sin(theta2);
   
  float y3 = (t + rf*cos(theta3))*sin30;
  float x3 = -y3*tan60;
  float z3 = -rf*sin(theta3);
   
  float dnm = (y2-y1)*x3-(y3-y1)*x2;
   
  float w1 = y1*y1 + z1*z1;
  float w2 = x2*x2 + y2*y2 + z2*z2;
  float w3 = x3*x3 + y3*y3 + z3*z3;
   
  // x = (a1*z + b1)/dnm
  float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
  float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;
   
  // y = (a2*z + b2)/dnm;
  float a2 = -(z2-z1)*x3+(z3-z1)*x2;
  float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;
   
  // a*z^2 + b*z + c = 0
  float a = a1*a1 + a2*a2 + dnm*dnm;
  float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
  float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);
  
  // discriminant
  float d = b*b - (float)4.0*a*c;
  if (d < 0) { validPosition = false; }

  zp = -(float)0.5*(b+sqrt(d))/a;
  xp = (a1*zp + b1)/dnm;
  yp = (a2*zp + b2)/dnm;

  if (xp >= min_x && xp <= max_x&& yp >= min_y && yp <= max_y && zp >= min_z & zp <= max_z) {  //bounds check
  } else {
    validPosition = false;
  }
    
  if ( validPosition ) {   //we're good, make it so 
    SetServos();  
  } else {                 //we failed, revert to our last known good positions
    xp = last_x;
    yp = last_y;
    zp = last_z;
    t1 = last_t1;
    t2 = last_t2;
    t3 = last_t3;  
  }
    
  //finally update the gui elemnts
  updateGuiElements();
  if ( debug ) { println("Exiting: setXYZfromThetas()"); } 
}

//our stuff has changed, update the gui elements
void updateGuiElements() { 
  if ( debug ) { println("Entering: updateGuiElements()"); } 
  //turn off the event broadcasts to stop recursion
  xtext.setBroadcast(false);
  ytext.setBroadcast(false); 
  ztext.setBroadcast(false); 
  xy.setBroadcast(false); 
  z.setBroadcast(false); 
  servo1.setBroadcast(false);
  servo2.setBroadcast(false);
  servo3.setBroadcast(false);
 
  //apply values to gui elements
  xy.setArrayValue(new float[] {xp * -1 + 20, yp * -1 + 20});
  z.setValue(zp);
  xtext.setText(String.valueOf(Math.floor(xp * precision +.5)/precision));
  ytext.setText(String.valueOf(Math.floor(yp * precision +.5)/precision));
  ztext.setText(String.valueOf(Math.floor(zp * precision +.5)/precision));
  servo1.setValue(t1);
  servo2.setValue(t2);
  servo3.setValue(t3);
  
  //turn event broadcasts back on
  xtext.setBroadcast(true);
  ytext.setBroadcast(true); 
  ztext.setBroadcast(true); 
  xy.setBroadcast(true); 
  z.setBroadcast(true);
  servo1.setBroadcast(true);
  servo2.setBroadcast(true);
  servo3.setBroadcast(true);
  if ( debug ) { println("Exiting: updateGuiElements()"); }
}

//set servo positions
void SetServos() {
  if ( debug ) { println("Entering: SetServos()"); }
  if (servosEnabled) {
    sendMove(1, (int)t1 + 90 + s1offset);  //math is -90 to +90, servo is 0 to 180
    sendMove(2, (int)t2 + 90 + s2offset);  //math is -90 to +90, servo is 0 to 180
    sendMove(3, (int)t3 + 90 + s3offset);  //math is -90 to +90, servo is 0 to 180
  }
  if ( debug ) { println("Exiting: SetServos()"); }
}

//enable or disable servos
void enable(boolean btnEnabled) {
  if ( debug ) { println("Entering: enable() (servos)"); }
  if ( btnEnabled==true ) {
    sendMove(99, 180); //turn servos on
    setThetasfromXYZ(); //set servos to current x,y,z
    servosEnabled = true;
  } 
  else {
    sendMove(99, 0); //turn servos off
    servosEnabled = false;
  }
  if ( debug ) { println("Exiting: enable() (servos)"); }
}

//enable or disable servos
void record(boolean btnEnabled) {

  if ( btnEnabled==true ) {

    recordEnabled = true;
  } 
  else {
   
    recordEnabled = false;
  }

}
//enable or disable servos
void play(boolean btnEnabled) {

  if ( btnEnabled==true ) {

    playEnabled = true;
  } 
  else {
   
    playEnabled = false;
  }

}




//send serial command
void sendMove(int servo, int val) {
  myPort.write(255);
  myPort.write(servo);
  myPort.write(val);
}

//copy current xyz values into our failback aray
void  copyLastData() {
  if ( debug ) { println("Entering: copyLastData()"); }
  last_x = xp;
  last_y = yp;
  last_z = zp;
  last_t1 = t1;
  last_t2 = t2;
  last_t3 = t3;
  if ( debug ) { println("Exiting: copyLastData()"); }  
}

// inverse kinematics
// helper functions, calculates angle theta1 (for YZ-pane)
float delta_calcAngleYZ(float x0, float y0, float z0) {
  float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
  y0 -= 0.5 * 0.57735    * e;    // shift center to edge
  // z = a + b*y
  float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
  float b = (y1-y0)/z0;
  // discriminant
  float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); 
  if (d < 0) return 999; // non-existing point
  float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
  float zj = a + b*yj;
  return 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
} 


void GCODE(int theValue) {

  if (!streaming && getFile==""){
    gcode = null; 
    File file = null; 
    println("Loading file...");
    getFile = getFileName();
  }
  
}

String getFileName(){
  //処理タイミングの設定 
  SwingUtilities.invokeLater(new Runnable() { 
    public void run() {
 try {
   //ファイル選択画面表示 
   JFileChooser fc = new JFileChooser(); 
   int returnVal = fc.showOpenDialog(null);
   //「開く」ボタンが押された場合
   if (returnVal == JFileChooser.APPROVE_OPTION) {
     //選択ファイル取得 
     File file = fc.getSelectedFile();
     //選択ファイルのパス取得 
     getFile = file.getPath(); 
   } 
 }
 //上記以外の場合 
 catch (Exception e) {
   //エラー出力 
   e.printStackTrace(); 
 } 
    } 
  } 
  );
  //選択ファイルパス取得
  return getFile; 
}

void PRINT(int theValue){
  // Cut E and F 


     
    for(int linenum=0; linenum<gcode.length; linenum++){
    //  println(glist);
     String[] glist = splitTokens(gcode[linenum], " ");
     String[] glistout = {};

      if (glist.length > 0){
      if (!(";".equals( str (glist[0].charAt(0))))){
                float xx=0; float yy=0; float zz=0;
      for (int i=0; i<glist.length; i++){
   

   
        if ( "X".equals( str( glist[i].charAt(0))) ) {   
          String x=  glist[i].substring(1);
           xx =  float( glist[i].substring(1) );
           xp = xx; 
           
        }
        if ( "Y".equals( str( glist[i].charAt(0))) )  {   
          String y=  glist[i].substring(1);
           yy=   float( glist[i].substring(1) );
           yp = yy;
        }
        if ( "Z".equals( str( glist[i].charAt(0))) )  {   
          String z =  glist[i].substring(1);
          zz=  float( glist[i].substring(1));
          zp = zz;
      }
      }
      setThetasfromXYZ();
      updateGuiElements(); 
    
      String ggg=join(glistout," "); 
      println("gcode: "+gcode[linenum]+" --> x:"+xx + " y:"+yy+ " z:"+zz);

     //Processing Map fuction to be used 
     //map(value, start1, stop1, start2, stop2)
      
      
      }
      }
      

    }


}