import Jama.*; 

/*
Using the Jama libray to take in denavit-Hartenberg parameters for a surgical
robot arm of (nj) joints/links and calculate the closed chain kinematics 
using the Newton-Rhapson solution.  Obstacles can be added or deleted. 

The alternative (there are many actually) is to use a path planning algorithm,
perhaps RRT to generate a path.  The Newton-Rhapson is only faster here because
only minor adjustments need to be made in order to find a configuration which 
fit around obstacles.  However, when faced with a maze-like obstacle a 
Probablistic Roadmap (PRM) will find a solution when there is one or return 
there is no solution, whereas this Newton-Rhapson will continuously hunt for a
solution when one does not exist.  Much work needs to be done to fix this problem.
*/

int nj = 13;              // number of joints
int num_obs = 300;        // number of obstacles
float obssz = 50;         // obstacle size
float obsb = 10;          // obstacle boundary padding        
double t = 0;             // time = 0

int backgroundcolor = 80;


//__________ARRAYS_________________________//
double[][] Rarray = {
  {
    0, -1
  }
  , {
    1, 0
  }
};
double phistart = 0;
double[][] spA_array = new double[2][1];
double[][] spB_array = new double[2][1];
double[][] rA_array = new double[2][1];
double[][] rB_array = new double[2][1];
double[][] r_array = new double[2][1];
double[][] A_array = new double[2][2];
double[][] B_array = new double[2][2];

//double[][] PHI_array = new double[nj*2+1][1];
double[][] PHI_array = new double[nj*2+2][1];
double[][] q_array = new double[(nj-1)*3][1];
double[][] q_array2 = new double[(nj-1)*3][1];
double[][] JAC_array = new double[(nj-1)*2+4][(nj-1)*3];
double[] phi = new double[nj];

//__________MATRICES________________//
Matrix R = new Matrix(Rarray);
Matrix spA[] = new Matrix[nj];
Matrix spB[] = new Matrix[nj];
Matrix rA[] = new Matrix[nj];
Matrix rB[] = new Matrix[nj];
Matrix r[] = new Matrix[nj];
Matrix A[] = new Matrix[nj];
Matrix B[] = new Matrix[nj];
Matrix spAt = new Matrix(2, 1);
Matrix spBt = new Matrix(2, 1);

Matrix PHI = new Matrix(PHI_array);
Matrix JAC = new Matrix(JAC_array);
Matrix PHI_orig = new Matrix(PHI_array);
Matrix JAC_orig = new Matrix(JAC_array);

Matrix q = new Matrix(q_array);

double [][] dh = new double[nj][4];

String[] lines;

PFont font1;
float [][][] obsp = new float[num_obs][4][2];
long frame = 0;
//____________________INITIALIZE/SETUP_____________________________//
void setup() 
{ 
  size(450, 600);
  
  lines = loadStrings("positions.txt");
  for (int i = 0; i < lines.length; i ++)
  {
    String[] pieces = split(lines[i], '\t');
    if (pieces.length == 4) {
      obsp[i][0][0] = float(pieces[0]);
      obsp[i][0][1] = float(pieces[1]);
      obsp[i][2][0] = float(pieces[2]);
      obsp[i][2][1] = float(pieces[3]);
    }
    obsp[i][1][0] = obsp[i][2][0];
    obsp[i][1][1] = obsp[i][0][1];
    obsp[i][3][0] = obsp[i][0][0];
    obsp[i][3][1] = obsp[i][2][1];
  }

  font1 = loadFont("CourierNew36.vlw");
  textAlign(CENTER);
  stroke(255);

  num_obs = 20;
  for (int i = 0; i < num_obs; i ++) {
    
    //if (i >= lines.length) 
    if (i >= 0)
    {
      while(abs(obsp[i][0][0]) <= 40) obsp[i][0][0] = random(width-obssz)-width/2;
      while(abs(obsp[i][0][1]) <= 40) obsp[i][0][1] = random(height-200-obssz)-height/2+100;
      //obsp[i][0][0] = obsp_arrayx[i];
      //obsp[i][0][1] = obsp_arrayy[i];        
      obsp[i][1][0] = obsp[i][0][0]+obssz;
      obsp[i][1][1] = obsp[i][0][1];
      obsp[i][2][0] = obsp[i][0][0];
      obsp[i][2][1] = obsp[i][0][1]+obssz;
      obsp[i][3][0] = obsp[i][0][0]+obssz;
      obsp[i][3][1] = obsp[i][0][1]+obssz;
    }
    
  }

  for (int i = 0; i < nj; i ++)
  {
    dh[i][0] = 40;
    if (i % 2 == 0)    dh[i][3] = 100-i*30; 
    else     dh[i][3] = -dh[i-1][3];
  }

  Matrix dhMat = new Matrix(dh);
  dhMat.print(1, 1);

  for (int i = 0; i < nj; i ++)
  {
    spB_array[0][0] = dh[i][0];
    spB_array[1][0] = 0;
    spA[i] = new Matrix(2, 1);
    spB[i] = new Matrix(2, 1);
    phi[i] = radians((float)dh[i][3]);
    spAt = new Matrix(spA_array);
    spBt = new Matrix(spB_array);
    spA[i].setMatrix(0, 1, 0, 0, spAt);
    spB[i].setMatrix(0, 1, 0, 0, spBt);
  }

  phistart = phi[1];

  for (int i = 1; i < nj; i ++)
  {
    A[i] = getA(phi[i]);
    if (i > 1)  r[i] = A[i].times(spB[i]).plus(r[i-1]);
    else        r[i] = A[i].times(spB[i]);

    q_array[(i-1)*3][0] = r[i].get(0, 0);
    q_array[(i-1)*3+1][0] = r[i].get(1, 0);
    q_array[(i-1)*3+2][0] = phi[i];
  }

  q = new Matrix(q_array);
  for (int i = 0; i < nj; i ++) {
    rA[i] = new Matrix(2, 1);
    rB[i] = new Matrix(2, 1);
  }
  for (int i = 0; i < nj; i ++) rA[i].print(1, 1);
  for (int i = 0; i < nj; i ++) rB[i].print(1, 1);

  smooth();
  frameRate(30);
} 



float[] x = new float[2];
float[] y = new float[2];
//________________________MAIN_LOOP___________________________//
void draw() 
{
  frame++;
  t+=0.1;

  evalConstraint();  // calculate constraint vector
  newtonRhapson();   // qnew = qold * inv(jacobian)*constraint

  check_distances();  // check distance between each joint and object
  check_collision();  // check if any collision and which joint/obstacle

  int countmove = 0;
  while (max (movejoint) !=0 && countmove < 50) 
  {
    println("COLLISION!");

    for (int i = 0; i < nj; i++) {
      int obsInWay = movejoint[i];
      if (movejoint[i] != 0) {
        
        int num_iter = 10;
        for (int k = 1; k < num_iter; k ++) {
          double cAng = q.get((i-1)*3+2,0);
          if (k%2==0) q.set((i-1)*3+2, 0, cAng-k*(180/num_iter)*PI/180);
          else q.set((i-1)*3+2, 0, cAng+k*(180/num_iter)*PI/180);
            
          //q.set((i-1)*3+2, 0, random(360)*PI/180);
        }

        evalConstraint();
        newtonRhapson();

        check_distances();
        check_collision();
      }
      countmove++;
    }
  }

  background(backgroundcolor);
  stroke(255);
  strokeWeight(4);
  pushMatrix();
  translate(width/2, height/2);
  point(0, 0);

  for (int i = 1; i < nj-2; i ++) 
  {
    for (int j = 0; j < 2; j ++) {
      x[j] = (float)r[i+j].get(0, 0);
      y[j] = (float)r[i+j].get(1, 0);
    }
    strokeWeight(10);
    stroke(0, 0, 100);
    line(x[0], y[0], x[1], y[1]);
    
    int szellipse = 25;
    for (int j = 0; j < 2; j ++) {
      strokeWeight(0);
      fill(0);
      if (i!=nj-2 && j!=1) {
      
      ellipse(x[j], y[j], szellipse, szellipse);
      fill(255);
      textFont(font1, 12);
      text(str(i), x[j], y[j]);
      }
    }
  }
  add_obstacles();
  draw_gripper();
  popMatrix();

  stroke(255);
  textFont(font1, 25);
  text("2D Closed Chain Kinematics", width/2, 30);
  
  textFont(font1, 11);
  text("The robot finds a configuration to avoid random obstacles", width/2,50);
  textAlign(LEFT);
  textFont(font1, 10);
  text("Press 'i' to add an obstacle", 10,70);
  text("Press 'd' to delete an obstacle", 10,80);
  textAlign(CENTER);
  if (frame % 5 == 0 ) println("frame: " + frame);
  //print_stuff();
  //text(nj, width/6, 80);
}


//__________CONSTRAINTS_&_JACOBIAN_________________//
double [][] rBset_array = {
  {
    100.
  }
  , {
    0.
  }
};
void evalConstraint() 
{

  for (int i = 1; i < nj; i ++) 
  {
    r[i] = q.getMatrix((i-1)*3, (i-1)*3+1, 0, 0);
    phi[i] = q.get((i-1)*3+2, 0);
  }
  for (int i = 0; i < nj; i ++)
  {
    A[i] = getA(phi[i]);
    B[i] = A[i].times(R);
  }
  for (int i = 1; i < nj; i ++)
  {
    rA[i] = r[i].plus(A[i].times(spA[i]));
    rB[i] = r[i].plus(A[i].times(spB[i]));
  }

  rBset_array[0][0] = (double)(mouseX-width/2);
  rBset_array[1][0] = (double)(mouseY-height/2);

  Matrix rBset = new Matrix(rBset_array);
  
  for (int i = 0; i < nj-1; i ++)
  {
    if (i == 0)
    {
      PHI.setMatrix(i*2, i*2+1, 0, 0, rA[0].minus(rA[1]));

      JAC.setMatrix(i*2, i*2+1, i*3, i*3+1, Matrix.identity(2, 2).uminus());
      JAC.setMatrix(i*2, i*2+1, i*3+2, i*3+2, B[i+1].times(spA[i+1]));
      JAC.setMatrix(i*2+2, i*2+3, i*3, i*3+1, Matrix.identity(2, 2));
      JAC.setMatrix(i*2+2, i*2+3, i*3+2, i*3+2, B[i+1].times(spB[i+1]));
    }
    else 
    {
      PHI.setMatrix(i*2, i*2+1, 0, 0, rB[i].minus(rA[i+1]));

      JAC.setMatrix(i*2, i*2+1, i*3, i*3+1, Matrix.identity(2, 2).uminus());
      JAC.setMatrix(i*2, i*2+1, i*3+2, i*3+2, B[i+1].uminus().times(spA[i+1]));
      JAC.setMatrix(i*2+2, i*2+3, i*3, i*3+1, Matrix.identity(2, 2));
      JAC.setMatrix(i*2+2, i*2+3, i*3+2, i*3+2, B[i+1].times(spB[i+1]));
    }
  }  
  JAC.setMatrix(nj*2, nj*2+1, (nj-2)*3, (nj-2)*3+1, Matrix.identity(2, 2));
  PHI.setMatrix(nj*2, nj*2+1, 0, 0, rB[nj-1].minus(rBset));
}





//________________________NEWTON_RHAPSON________________________________//
double phimax = 0, phitest = 0, tolerance = 0.1;

void newtonRhapson()
{
  evalConstraint();

  phimax = 0;
  for (int i=0;i<nj*2+1;i++) { 
    if (abs((float)PHI.get(i, 0))>phimax) phimax=abs((float)PHI.get(i, 0));
  }
  int counter = 0;  
  while ( phimax > tolerance && counter < 5) 
  {
    Matrix x1 = pinv(JAC).times(PHI);
    q.minusEquals(x1);
    evalConstraint();

    phimax = 0;
    for (int i=0;i<nj*2+1;i++) { 
      if (abs((float)PHI.get(i, 0))>phimax) phimax=abs((float)PHI.get(i, 0));
    }  
    phitest = phimax;
    counter++;
  }
}


float [] vec = new float[nj];
//__________CLOSEST_OBSTACLES__________________//
int [][] clObs= new int[nj][2];
void check_distances() 
{
  float distance;
  int min_j = 0; 
  int closest_obs = 0;

  for (int i = 1; i < nj; i ++) 
  {    
    float min_distance = 1000;
    for (int k = 0; k < num_obs; k ++) 
    {
      for (int j=0; j < 4; j++)
      {
        distance = dist((float)r[i].get(0, 0), (float)r[i].get(1, 0), 
        obsp[k][j][0], obsp[k][j][1]);

        if (distance < min_distance) 
        {
          min_distance = distance; 
          min_j = j;
          closest_obs = k;
        }
      }
    }

    clObs[i][0] = closest_obs;    // closest object
    clObs[i][1] = min_j;          // closest vertex

    // determine the vector direction the joint should rotate
    // angle = tan-1( jointY-closestpoint of closest obstacle) / 
    // (jointX- "  ")
    vec[i] = atan(((float)r[i].get(0, 0)-obsp[clObs[i][0]][clObs[i][1]][1])/
      ((float)r[i].get(1, 0)-obsp[clObs[i][0]][clObs[i][1]][0]));
    //    print(i + " " + vec + "   ");
    if (frame % 2000 == 0) {
      print(i+" "+vec+"  ");
      print(i +  "o:" + closest_obs + "c: " + min_j + "  "); 
      //      //print((float)r[i].get(0, 0) + " y: " + (float)r[i].get(1, 0) + "  "); 
      if (i == nj-1)
        println("");
    }
  }
}




//__________DRAW_OBSTACLES_____________________//
void add_obstacles() 
{
  
  strokeWeight(0); 
  stroke(100);
  for (int i =0; i < num_obs; i ++) 
  {
    fill(0);
    rect(obsp[i][0][0]+obsb,obsp[i][0][1]+obsb,(obsp[i][1][0]-obsp[i][0][0])-obsb,obsp[i][2][1]-obsp[i][0][1]-obsb); 
    fill(205);
    textFont(font1, 15);
    text(i, obsp[i][0][0]+obssz/2+obsb/2, obsp[i][0][1]+obssz*2/3+obsb/4); 
    textFont(font1, 8);
    //for (int k = 0; k < 4; k ++) text(k, obsp[i][k][0]+5, obsp[i][k][1]+8);
  }  
  // uncomment to display line from each joint to closest obstacle corner.
  /*
  for (int i = 1; i < nj-1; i ++) 
  {
    stroke(backgroundcolor-5);
    stroke(0);
    float x = obsp[clObs[i][0]][clObs[i][1]][0];
    float y = obsp[clObs[i][0]][clObs[i][1]][1];
    line((float)r[i].get(0, 0), (float)r[i].get(1, 0), x, y);
  }*/
}




//__________JAMA_PSEUDO_INVERSE__________________//
// The following two functions are from Ahmed Abdelkader's JAMA blog:
// http://the-lost-beauty.blogspot.com/2009/04/moore-penrose-pseudoinverse-in-jama.html
public static double MACHEPS = 2E-16; 
//______________________________________________________________________//
Matrix pinv(Matrix x) {
  if (x.getColumnDimension() > x.getRowDimension())
    return pinv(x.transpose()).transpose();
  if (x.rank() < 1)
    return null;
  if (x.getColumnDimension() > x.getRowDimension())
    return pinv(x.transpose()).transpose();
  SingularValueDecomposition svdX = new SingularValueDecomposition(x);
  double[] singularValues = svdX.getSingularValues();
  double tol = Math.max(x.getColumnDimension(), x.getRowDimension()) * singularValues[0] * MACHEPS;
  double[] singularValueReciprocals = new double[singularValues.length];
  for (int i = 0; i < singularValues.length; i++)
    singularValueReciprocals[i] = Math.abs(singularValues[i]) < tol ? 0 : (1.0 / singularValues[i]);
  double[][] u = svdX.getU().getArray();
  double[][] v = svdX.getV().getArray();
  int min = Math.min(x.getColumnDimension(), u[0].length);
  double[][] inverse = new double[x.getColumnDimension()][x.getRowDimension()];
  for (int i = 0; i < x.getColumnDimension(); i++)
    for (int j = 0; j < u.length; j++)
      for (int k = 0; k < min; k++)
        inverse[i][j] += v[i][k] * singularValueReciprocals[k] * u[j][k];
  return new Matrix(inverse);
}
public static void updateMacheps() {
  MACHEPS = 1;
  do
    MACHEPS /= 2;
  while (1 + MACHEPS / 2 != 1);
}
//_________________________________________________________________//






//___________PRINT___________________//
void print_stuff()
{
  print("r"); 
  for (int i = 0; i < nj; i ++) r[i].print(1, 1);
  print("phi");
  for (int i = 0; i < nj; i ++) print(" " + phi[i]);
  print("rA");
  for (int i = 0; i < nj; i ++) rA[i].print(1, 1);
  print("rB");
  for (int i = 0; i < nj; i ++) rB[i].print(1, 1);

  print("spB");
  for (int i = 0; i < nj; i ++) spB[i].print(1, 1);
  print("spA");
  for (int i = 0; i < nj; i ++) spA[i].print(1, 1);

  print("A");
  for (int i = 0; i < nj; i ++) A[i].print(1, 1);
  print("B");
  for (int i = 0; i < nj; i ++) B[i].print(1, 1);
  JAC.print(1, 1);
  PHI.print(1, 1);
}





//_____________GET_ROTATION_MATRIX________//
Matrix getA(double phi) 
{
  double[][] Ain = {
    {
      cos((float)phi), -sin((float)phi)
      }
    , {
      sin((float)phi), cos((float)phi)
      }
    }; 
    Matrix Aout = new Matrix(Ain);
  return Aout;
}

int [] movejoint = new int[nj];
void check_collision()
{
  for (int i = 1; i < nj-2; i ++)   // for each joint
  {  
    movejoint[i] = 0;
    for (int j = 0 ; j < num_obs; j ++)   // check each obstacle
    {
      for (int k = 0; k < 4; k ++)  // each line
      {
        int coll;

        if (k == 3) 
          coll = collision_detect(r[i].get(0, 0), 
          r[i+1].get(0, 0), r[i].get(1, 0), 
          r[i+1].get(1, 0), (double)obsp[j][0][0], (double)obsp[j][3][0], 
          (double)obsp[j][0][1], (double)obsp[j][3][1]);
        else
          coll = collision_detect(r[i].get(0, 0), 
          r[i+1].get(0, 0), r[i].get(1, 0), 
          r[i+1].get(1, 0), (double)obsp[j][k][0], (double)obsp[j][k+1][0], 
          (double)obsp[j][k][1], (double)obsp[j][k+1][1]);

        if (coll == 1)    movejoint[i] = j+1;
      }
    }
    //print(i + "_" + movejoint[i] + " ");
  }
  //println("");
}

void draw_gripper() 
{
  int x=30, y = 15;
  
  //stroke(0,0,10);
  stroke(255); 
  strokeWeight(10);
  pushMatrix();
  translate((float)r[nj-2].get(0, 0),(float)r[nj-2].get(1, 0));
  rotate((float)phi[nj-3]);
  line(0,0,0,y);
  line(0,0,0,-y);
  line(0,y,x,y);
  line(0,-y,x,-y);
  popMatrix();
}

int collision_detect(double X1, double X2, double Y1, double Y2, 
double X3, double X4, double Y3, double Y4)
{

  double X4_X3 = X4-X3;
  double X1_X3 = X1-X3;
  double X2_X1 = X2-X1;
  double Y4_Y3 = Y4-Y3;
  double Y1_Y3 = Y1-Y3;
  double Y2_Y1 = Y2-Y1;

  double num_a = X4_X3 * Y1_Y3 - Y4_Y3 * X1_X3;
  double num_b = X2_X1 * Y1_Y3 - Y2_Y1 * X1_X3;
  double den = Y4_Y3 * X2_X1 - X4_X3 * Y2_Y1;

  if (den == 0) den = 0.0001; 

  double u_a = num_a / den;
  double u_b = num_b / den;

  double INT_X = X1+X2_X1*u_a;
  double INT_Y = Y1+Y2_Y1*u_a;
  boolean INT_Bbool = (u_a >= 0) & (u_a <= 1) & (u_b >= 0) & (u_b <= 1);

  int INT_B = 1;
  if (INT_Bbool==true) INT_B = 1;
  else INT_B = 0;

  return INT_B;
}


void keyPressed() {
   if (key == 'i' && num_obs < 300) {
      num_obs++;
      while(abs(obsp[num_obs-1][0][0]) <= 40) obsp[num_obs-1][0][0] = random(width-obssz)-width/2;
      while(abs(obsp[num_obs-1][0][1]) <= 40) obsp[num_obs-1][0][1] = random(height-100-obssz)-height/2+100;        
      obsp[num_obs-1][1][0] = obsp[num_obs-1][0][0]+obssz;
      obsp[num_obs-1][1][1] = obsp[num_obs-1][0][1];
      obsp[num_obs-1][2][0] = obsp[num_obs-1][0][0];
      obsp[num_obs-1][2][1] = obsp[num_obs-1][0][1]+obssz;
      obsp[num_obs-1][3][0] = obsp[num_obs-1][0][0]+obssz;
      obsp[num_obs-1][3][1] = obsp[num_obs-1][0][1]+obssz;
   }
   else if (key == 'd' && num_obs > 0)
    num_obs--;
  
}