#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "PrGlobalDefn.h"
#include "PrRobot.h"
#include "PrJointState.h"
#include "PrLink.h"
#include "matrix/PrVector3.h"
#include "matrix/PrMatrix6.h"
#include "matrix/PrTransform.h"

PrRobot *robot;
PrTransform T;
PrMatrix6 Lambda,J;

void UpdateState();
void Frame(PrTransform &globalT);
void Jacobian(PrMatrix6 &J);
void OSIM(PrMatrix6 &Lambda);

static PrMatrix6 Js,A,Lambdas,M;
static PrVector6 G;
static PrMatrix3 R;
static PrVector3 P;

void GetSymbolicEOM();
void GetCosSin(PrVector6 &q);
void GetTransform(PrMatrix3 &R,PrVector3 &P);
void GetJacobian(PrMatrix6 &J);
void GetGravity(PrVector6 &G);
void GetMass(PrMatrix6 &A);
void GetMassFull(PrMatrix6 &M);
void GetLambda(PrMatrix6 &J,PrMatrix6 &A,PrMatrix6 &Lambda);

int main()
{
    robot = new PrRobot(6,"../../../param/puma560.prm",PR_ROBOT_INV_DYNAMICS);

    UpdateState();
    /*
    Frame(T);
    T.display("Frame");
    T.rotation().matrix().display("Rot");
    T.translation().display("Trans");
    printf("\n");
    Jacobian(J);
    J.display("Jacobian");
    */
    OSIM(Lambda);
    Lambda.display("Lambda");

    GetSymbolicEOM();
    /*
    R.display("RotS");
    P.display("TranS");
    printf("\n");
    Js.display("JacobianS");
    */
    Lambdas.display("LambdaS");

    delete robot;
    return 1;
}

void UpdateState()
{
  for(int i=0;i<6;i++)
    robot->jointState()->q()[i] = 0.0;     // wrist lock on
  robot->jointState()->q()[4] = M_PI_2;    // wrist lock off
  //robot->jointState()->q()[2] = M_PI_2;  // elbow lock on
  
  robot->jointState()->q()[0]=0.036830; 
  robot->jointState()->q()[1]=0.167307; 
  robot->jointState()->q()[2]=-0.209244;
  robot->jointState()->q()[3]=-0.046374;
  robot->jointState()->q()[4]=1.116782;
  robot->jointState()->q()[5]=-0.305342;
  
  robot->updateTransform();
}

void Frame(PrTransform &globalT)
{
  globalT = robot->object()->globalTransform();
}

void Jacobian(PrMatrix6 &J)
{
  PrLink *link = robot->base()->child();
  PrVector3 &P = robot->object()->globalTransform().translation();
  for(int j=0;j<6;j++)
    {
      PrVector6 &Jj = link->globalJacobian(P);
      for(int i=0;i<6;i++)
	J.elementAt(i,j) = Jj.elementAt(i);
      link = link->child();
    }
}

void OSIM(PrMatrix6 &Lambda)
{
    robot->globalOSIM(Lambda);
}

/*
 * Constants for Gravity vector(g)
 */
#define g1  -37.2
#define g2  -8.44
#define g3   1.02
#define g4   0.249
#define g5  -0.0282

/*
 * Denavit-Hartenberg Parameters
 */
#define a2  0.4318
#define a3 -0.0203
#define d2  0.2435
#define d3 -0.0934
#define d4  0.4331
#define d7  0.17       /* for EE in frame6 */
#define d23 0.1501   /* d56 = d5 + d6 */

/*
 * Macros for cosines and sines
 */
#define S1  sine[0]
#define C1  cosine[0]
#define S2  sine[1]
#define C2  cosine[1]
#define S3  sine[2]
#define C3  cosine[2]
#define S4  sine[3]
#define C4  cosine[3]
#define S5  sine[4]
#define C5  cosine[4]
#define S6  sine[5]
#define C6  cosine[5]

Float cosine[6];
Float sine[6];
Float S23,C23;

void GetSymbolicEOM()
{
  PrVector6 q;
  for(int i=0;i<6;i++)
    q.elementAt(i) = robot->jointState()->q()[i];
  GetCosSin(q);
  GetTransform(R,P);
  GetJacobian(Js);
  GetGravity(G);
  GetMass(A);
  GetMassFull(M);
  GetLambda(Js,M,Lambdas);
}

void GetCosSin(PrVector6 &q)
{
  for(int i=0;i<6;i++){
    cosine[i] = cos(q.elementAt(i));
    sine[i] = sin(q.elementAt(i));
  }
  S23 = S2*C3 + C2*S3;
  C23 = C2*C3 - S2*S3;
}

void GetTransform(PrMatrix3 &R,PrVector3 &P)
{
  R.elementAt(0,0) = C1*((C23*C4*C5 - S23*S5)*C6 - C23*S4*S6) 
                   - S1*(C4*S6 + S4*C5*C6);
  R.elementAt(1,0) = S1*((C23*C4*C5 - S23*S5)*C6 - C23*S4*S6) 
                   + C1*(C4*S6 + S4*C5*C6);
  R.elementAt(2,0) = -(C23*S5 + S23*C4*C5)*C6 + S23*S4*S6;
  // T(4,1) = 0.0;

  R.elementAt(0,1) = -C1*((C23*C4*C5 - S23*S5)*S6 + C23*S4*C6) 
                   - S1*(C4*C6 - S4*C5*S6);
  R.elementAt(1,1) = -S1*((C23*C4*C5 - S23*S5)*S6 + C23*S4*C6) 
                   + C1*(C4*C6 - S4*C5*S6);
  R.elementAt(2,1) = (C23*S5 + S23*C4*C5)*S6 + S23*S4*C6;
  // T(4,2) = 0.0;

  R.elementAt(0,2) = C1*(C23*C4*S5 + S23*C5) - S1*S4*S5;
  R.elementAt(1,2) = S1*(C23*C4*S5 + S23*C5) + C1*S4*S5;
  R.elementAt(2,2) = C23*C5 - S23*C4*S5;
  // T(4,3) = 0.0;

  P[0] = C1*(d4*S23 + a3*C23 + a2*C2) - S1*d23;
  P[1] = S1*(d4*S23 + a3*C23 + a2*C2) + C1*d23;
  P[2] = d4*C23 - a3*S23 - a2*S2;
  // T(4,4) = 1.0;
}

void GetJacobian(PrMatrix6 &J)
{
  J.elementAt(0,0) = -S1*(d4*S23 + a3*C23 + a2*C2) - C1*d23;
  J.elementAt(0,1) = C1*(d4*C23 - a3*S23 - a2*S2);
  J.elementAt(0,2) = C1*(d4*C23 - a3*S23);
  J.elementAt(0,3) = 0.0;
  J.elementAt(0,4) = 0.0;
  J.elementAt(0,5) = 0.0;

  J.elementAt(1,0) = C1*(d4*S23 + a3*C23 + a2*C2) - S1*d23;
  J.elementAt(1,1) = S1*(d4*C23 - a3*S23 - a2*S2);
  J.elementAt(1,2) = S1*(d4*C23 - a3*S23);
  J.elementAt(1,3) = 0.0;
  J.elementAt(1,4) = 0.0;
  J.elementAt(1,5) = 0.0;

  J.elementAt(2,0) = 0.0;
  J.elementAt(2,1) = -(d4*S23 + a3*C23 + a2*C2);
  J.elementAt(2,2) = -(d4*S23 + a3*C23);
  J.elementAt(2,3) = 0.0;
  J.elementAt(2,4) = 0.0;
  J.elementAt(2,5) = 0.0;

  /* Orientation Jacobian 4-6 */
  /* Coupling Jacobian 1-3 */

  J.elementAt(3,0) = 0.0;
  J.elementAt(3,1) = -S1;
  J.elementAt(3,2) = -S1;
  J.elementAt(3,3) = C1*S23;
  J.elementAt(3,4) = -C1*C23*S4 - S1*C4;
  J.elementAt(3,5) = C1*(C23*C4*S5 + S23*C5) - S1*S4*S5;

  J.elementAt(4,0) = 0.0;
  J.elementAt(4,1) = C1;
  J.elementAt(4,2) = C1;
  J.elementAt(4,3) = S1*S23;
  J.elementAt(4,4) = -S1*C23*S4 + C1*C4;
  J.elementAt(4,5) = S1*(C23*C4*S5 + S23*C5) + C1*S4*S5;

  J.elementAt(5,0) = 1.0;
  J.elementAt(5,1) = 0.0;
  J.elementAt(5,2) = 0.0;
  J.elementAt(5,3) = C23;
  J.elementAt(5,4) = S23*S4;
  J.elementAt(5,5) = C23*C5 - S23*C4*S5;
}

void GetGravity(PrVector6 &G)
{
  G.elementAt(0) = 0.0;
  G.elementAt(1) = g1*C2 + g2*S23 + g3*S2 + g4*C23 + g5*(C23*C4*S5 + S23*C5);
  G.elementAt(2) = g2*S23 + g4*C23 + g5*(C23*C4*S5 + S23*C5);
  G.elementAt(3) = -g5*S23*S4*S5;
  G.elementAt(4) = g5*(C23*S5 + S23*C4*C5);
  G.elementAt(5) = 0.0;
}


void GetMass(PrMatrix6 &A)
{
/* estimation of PUMA */

  A.elementAt(0,0) = 2.57 + 1.38*C2*C2 + 0.30*S23*S23 + 0.744*C2*S23;
  A.elementAt(0,1) = 0.69*S2 -0.134*C23 + 0.0238*C2;
  A.elementAt(0,2) = -0.134*C23 - 0.00397*S23;
  A.elementAt(0,3) = 0.0;
  A.elementAt(0,4) = 0.0;
  A.elementAt(0,5) = 0.0;
  A.elementAt(1,1) = 6.79 + 0.744*S3;
  A.elementAt(1,2) = 0.333 + 0.372*S3 - 0.011*C3;
  A.elementAt(1,3) = 0.0;
  A.elementAt(1,4) = 0.0;
  A.elementAt(1,5) = 0.0;
  A.elementAt(2,2) = 1.16;
  A.elementAt(2,3) = -0.00125*S4*S5;
  A.elementAt(2,4) = 0.00125*C4*C5;
  A.elementAt(2,5) = 0.0;
  A.elementAt(3,3) = 0.20;
  A.elementAt(3,4) = 0.0;
  A.elementAt(3,5) = 0.0;
  A.elementAt(4,4) = 0.18;
  A.elementAt(4,5) = 0.0;
  A.elementAt(5,5) = 0.19;

/* A is symmetric */
  for(int i=0;i<5;i++)
    for(int j=i+1;j<6;j++)
      A.elementAt(j,i) = A.elementAt(i,j);
}


void GetLambda(PrMatrix6 &J,PrMatrix6 &A,PrMatrix6 &Lambda)
{
  PrMatrix6 Jt,Ainv,JAinv,JAinvJt;
  A.inverseSPD(Ainv);
  J.multiply(Ainv,JAinv);
  J.transpose(Jt);
  JAinv.multiply(Jt,JAinvJt);
  JAinvJt.inverse(Lambda);
}

#define M(i,j)   M.elementAt(i-1,j-1)
/*
 * Constants for Kinetic energy matrix(A)
 */
#define I1  1.43
#define I2  1.75
#define I3  1.38
#define I4  6.90e-1
#define I5  3.72e-1
#define I6  3.33e-1
#define I7  2.98e-1
#define I8  -1.34e-1
#define I9  2.38e-2
#define I10  -2.13e-2
#define I11  -1.42e-2
#define I12  -1.10e-2
#define I13  -3.79e-3
#define I14  1.64e-3
#define I15  1.25e-3
#define I16  1.24e-3
#define I17  6.42e-4
#define I18  4.31e-4
#define I19  3.00e-4
#define I20  -2.02e-4
#define I21  -1.00e-4
#define I22  -5.80e-5
#define I23  4.00e-4
#define Im1  1.14
#define Im2  4.71
#define Im3  8.27e-1
#define Im4  2.0e-1
#define Im5  1.79e-1
#define Im6  1.93e-1


void GetMassFull(PrMatrix6 &M) 
{
  register float SS23,SC23,SC4; 
  register float SC5,CC4,SS4,SS5; //SC2

  SC23 = S23 * C23;
  SS23 = S23 * S23; 
  SC4 = S4 * C4; 
  SC5 = S5 * C5; 
  CC4 = C4 * C4; 
  SS4 = S4 * S4; 
  SS5 = S5 * S5; 

/* MaSS Matrix CalCulationS */ 

  M(1,1) = Im1 + I1 + I3*C2*C2 + I7*SS23 + I10*SC23 + I11*S2*C2 
          + I20*(SS5*(SS23*(1+CC4)-1) - 2*S23*C23*C4*S5*C5) 
          + I21*SS23*CC4 + 2*(I5*C2*S23 + I12*C2*C23 	 
          + I15*(( C23*C4*S5 + S23*C5)*S23) 	 
          + I16*C2*(C23*C4*S5 + S23*C5)
       	  + I18*S4*S5 + I22*((C23*C4*S5 + S23*C5)*C23));
  M(1,2) = I4*S2 + I8*C23 + I9*C2 + I13*S23 - I15*C23*S4*S5 
          + I16*S2*S4*S5 + I18*(-C23*C5 + S23*C4*S5) 
          + I19*S23*SC4 + I20*S4*(C23*S5 + S23*C4*C5)*C5 
          + I22*S23*S4*S5;
  M(1,3) = I8*C23 + I13*S23 - I15*C23*S4*S5 + I19*S23*SC4 
          + I18*(-C23*C5 +  S23*C4*S5) + I22*S23*S4*S5 
          + I20*S4*(C23*S5 + S23*C4*C5)*C5;
  M(1,4) = I14*C23 + I15*S23*C4*S5 + I16*C2*C4*S5 
          + I18*C23*S4*S5 - I20*(C23*S5 + S23*C4*C5)*S5 
          + I22*C23*C4*S5;
  M(1,5) = I15*S23*S4*C5 + I16*C2*S4*C5 + I17*S23*S4 
          + I18*(-C23*C4*C5 + S23*S5) + I22*C23*S4*C5;
  M(1,6) = I23*(C23*C5 -  S23*C4*S5);

  M(2,1) = M(1,2);
  M(2,2) = Im2 + I2 + I6 + I20*SS4*SS5 + I21*SS4 
          + 2*(I5*S3 + I12*C3 + I15*C5 
          + I16*(S3*C5 + C3*C4*S5) + I22*C4*S5);
  M(2,3) = I5*S3 + I6 + I12*C3 + I16*(S3*C5 + C3*C4*S5) 
          + I20*SS4*SS5 + I21*SS4 + 2*(I15*C5 + I22*C4*S5);
  M(2,4) = -I15*S4*S5 - I16*S3*S4*S5 + I20*S4*SC5;
  M(2,5) = I15*C4*C5 + I16*(C3*S5 + S3*C4*C5) 
          + I17*C4 + I22*S5;
  M(2,6) = I23*S4*S5;

  M(3,1) = M(1,3);
  M(3,2) = M(2,3);
  M(3,3) = Im3 + I6 + I20*SS4*SS5 + I21*SS4 
          + 2*(I15*C5 + I22*C4*S5);
  M(3,4) = -I15*S4*S5 + I20*S4*SC5;
  M(3,5) = I15*C4*C5 + I17*C4 +I22*S5;
  M(3,6) = I23*S4*S5;

  M(4,1) = M(1,4);
  M(4,2) = M(2,4);
  M(4,3) = M(3,4);
  M(4,4) = Im4 + I14 - I20*SS5;
  M(4,5) = 0.0;
  M(4,6) = I23*C5;

  M(5,1) = M(1,5);
  M(5,2) = M(2,5);
  M(5,3) = M(3,5);
  M(5,4) = M(4,5);
  M(5,5) = Im5 + I17;
  M(5,6) = 0.0;

  M(6,1) = M(1,6);
  M(6,2) = M(2,6);
  M(6,3) = M(3,6);
  M(6,4) = M(4,6);
  M(6,5) = M(5,6);
  M(6,6) = Im6 + I23;
}
