#ifndef _LWR4_H_
#define _LWR4_H_

#include "Vec.h"
#include "Mat.h"
#include "friComm.h"
#include "Robot.h"


class LWR4 : public Robot
{
    public :
        static Robot::VecJoint getJointRange(void);
        static Robot::VecJoint getJointVelocity(void);
        static Robot::VecJoint getJointAcceleration(void);

        static Vec<float, 4> getDhParameter(unsigned int joint);
        
        static Robot::VecJoint      backwardCalc(MatCarthesian pos, struct RobotConfig config );
        static MatCarthesian forwardCalc(Robot::VecJoint, struct RobotConfig config );
};

Robot::VecJoint LWR4::backwardCalc(MatCarthesian M , struct RobotConfig config )
{
    Robot::VecJoint Joints(0.0f);

    //DH-Parameter
    float alp[7] = {-M_PI/2.0f,     M_PI/2.0f, -M_PI/2.0f,     M_PI/2.0f, -M_PI/2.0f,     M_PI/2.0f, 0.0f};
    float d[7]   = {310.4f/1000.0f, 0.0f,      400.1f/1000.0f, 0.0f,      390.0f/1000.0f, 0.0f,      78.0f/1000.0f};
    //Parameter
    int ELBOW  = (config.elbow ? 1.0f : -1.0f );
    int FLIP   = (config.flip  ? 1.0f : -1.0f );
    float J1os = (config.j1os  ? 1.0f : -1.0f )/180.0*M_PI;

    //Calculating M06
    MatCarthesian R67;
    int i = 6;

    R67(0,0) = cos(0);
    R67(0,1) = -sin(0)*cos(alp[i]);
    R67(0,2) = sin(0)*sin(alp[i]);
    R67(0,3) = 0.0f;

    R67(1,0) = sin(0);
    R67(1,1) = cos(0)*cos(alp[i]);
    R67(1,2) = -cos(0)*sin(alp[i]);
    R67(1,3) = 0.0f;

    R67(2,0) = 0.0f;
    R67(2,1) = sin(alp[i]);
    R67(2,2) = cos(alp[i]);
    R67(2,3) = d[i];

    R67(3,0) = 0.0f;
    R67(3,1) = 0.0f;
    R67(3,2) = 0.0f;
    R67(3,3) = 1.0f;

    MatCarthesian M06;
    MatCarthesian Inv;
    Inv = R67.inv();
    M06=M*Inv;

    //Joint 1 including given offset
    Joints(0) = atan2(M06(1,3),M06(0,3))+J1os;

    //Calculating M16
    MatCarthesian R01;
    i = 0;
    R01(0,0) = cos(Joints(i));
    R01(0,1) = -sin(Joints(i))*cos(alp[i]);
    R01(0,2) = sin(Joints(i))*sin(alp[i]);
    R01(0,3) = 0.0f;

    R01(1,0) = sin(Joints(i));
    R01(1,1) = cos(Joints(i))*cos(alp[i]);
    R01(1,2) = -cos(Joints(i))*sin(alp[i]);
    R01(1,3) = 0.0f;

    R01(2,0) = 0.0f;
    R01(2,1) = sin(alp[i]);
    R01(2,2) = cos(alp[i]);
    R01(2,3) = d[i];

    R01(3,0) = 0.0f;
    R01(3,1) = 0.0f;
    R01(3,2) = 0.0f;
    R01(3,3) = 1.0f;

    Inv = R01.inv();
    MatCarthesian M16;
    M16 = Inv*M06;

    //Joint 2
    float m14 = M16(0,3);
    float m24 = M16(1,3);
    float m34 = M16(2,3);
    float d3 = d[2];
    float d5 = d[4];

    float arg1 = (0.5*(ELBOW*m24*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14
        -ELBOW*m24*m24*m24+sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3
        +2*m14*m14*m14*m14*d5*d5+2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14
        -2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24*m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14
        +2*m24*m24*d3*d3*m14*m14-2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34
        +2*m14*m14*d5*d5*d3*d3+2*m24*m24*d5*d5*m14*m14))/(m24*m24+m14*m14)
        +m34*m34+d3*d3-1*d5*d5+m14*m14+m24*m24))/(d3*m14);

    float arg2 = ELBOW*(0.5*(-ELBOW*m34*m34*m24-ELBOW*m24*d3*d3+ELBOW*m24*d5*d5-ELBOW*m24*m14*m14-ELBOW*m24*m24*m24
        +sqrt(-2*m24*m24*m34*m34*m14*m14-1*m14*m14*d3*d3*d3*d3+2*m14*m14*m14*m14*d5*d5
        +2*m14*m14*m14*m14*d3*d3-1*m14*m14*d5*d5*d5*d5-1*m14*m14*m14*m14*m14*m14-2*m24*m24*m14*m14*m14*m14-1*m24*m24*m24*m24
        *m14*m14-2*m14*m14*m14*m14*m34*m34-1*m34*m34*m34*m34*m14*m14+2*m24*m24*d3*d3*m14*m14
        -2*m14*m14*d3*d3*m34*m34+2*m14*m14*d5*d5*m34*m34+2*m14*m14*d5*d5*d3*d3
        +2*m24*m24*d5*d5*m14*m14)))/((m24*m24+m14*m14)*d3);

    Joints(1) = atan2(arg1,arg2);

    //Calculating M26
    MatCarthesian R12;
    i = 1;

    R12(0,0) = cos(Joints(i));
    R12(0,1) = -sin(Joints(i))*cos(alp[i]);
    R12(0,2) = sin(Joints(i))*sin(alp[i]);
    R12(0,3) = 0.0f;

    R12(1,0) = sin(Joints(i));
    R12(1,1) = cos(Joints(i))*cos(alp[i]);
    R12(1,2) = -cos(Joints(i))*sin(alp[i]);
    R12(1,3) = 0.0f;

    R12(2,0) = 0.0f;
    R12(2,1) = sin(alp[i]);
    R12(2,2) = cos(alp[i]);
    R12(2,3) = d[i];

    R12(3,0) = 0.0f;
    R12(3,1) = 0.0f;
    R12(3,2) = 0.0f;
    R12(3,3) = 1.0f;

    Mat4f R02;
    R02 = R01*R12;
    Inv = R02.inv();
    Mat4f M26;
    M26 = Inv*M06;

    //Joint 3
    Joints(2) = atan2(ELBOW*M26(1,3),ELBOW*M26(0,3));

    //Calculating M36
    MatCarthesian R23;
    i = 2;

    R23(0,0) = cos(Joints(i));
    R23(0,1) = -sin(Joints(i))*cos(alp[i]);
    R23(0,2) = sin(Joints(i))*sin(alp[i]);
    R23(0,3) = 0.0f;

    R23(1,0) = sin(Joints(i));
    R23(1,1) = cos(Joints(i))*cos(alp[i]);
    R23(1,2) = -cos(Joints(i))*sin(alp[i]);
    R23(1,3) = 0.0f;

    R23(2,0) = 0.0f;
    R23(2,1) = sin(alp[i]);
    R23(2,2) = cos(alp[i]);
    R23(2,3) = d[i];

    R23(3,0) = 0.0f;
    R23(3,1) = 0.0f;
    R23(3,2) = 0.0f;
    R23(3,3) = 1.0f;

    MatCarthesian R03;
    R03 = R01*R12*R23;
    Inv = R03.inv();
    MatCarthesian M36;
    M36 = Inv*M06;

    //Joint 4
    Joints(3) = atan2(M36(0,3),-M36(1,3));

    //Calculating M47
    MatCarthesian R34;
    i = 3;
    R34(0,0) = cos(Joints(i));
    R34(0,1) = -sin(Joints(i))*cos(alp[i]); 
    R34(0,2) = sin(Joints(i))*sin(alp[i]);
    R34(0,3) = 0.0f;

    R34(1,0) = sin(Joints(i));
    R34(1,1) = cos(Joints(i))*cos(alp[i]);
    R34(1,2) = -cos(Joints(i))*sin(alp[i]);
    R34(1,3) = 0.0f;

    R34(2,0) = 0.0f;
    R34(2,1) = sin(alp[i]);
    R34(2,2) = cos(alp[i]);
    R34(2,3) = d[i];

    R34(3,0) = 0.0f;
    R34(3,1) = 0.0f;
    R34(3,2) = 0.0f;
    R34(3,3) = 1.0f;

    MatCarthesian R04;
    R04 = R01*R12*R23*R34;
    Inv = R04.inv();
    MatCarthesian M47;
    M47 = Inv*M;

    //Joint 5
    float th = 0.0001;
    if (abs(M47(1,2))<th && abs(M47(0,2))<th)
        Joints(4) = 0.0f;
    else
        Joints(4) = atan2(FLIP*M47(1,2),FLIP*M47(0,2));

    //Joint 6
    Joints(5) = atan2(FLIP*sqrt(1-M47(2,2)*M47(2,2)),M47(2,2));

    //Joint 7
    if (abs(M47(2,1))<th && abs(M47(2,0))<th)
        Joints(6) = 0.0f;
    else
        Joints(6) = atan2(FLIP*M47(2,1),FLIP*-M47(2,0));

    for (int i = 0; i<7; i++)
    {
        Joints(i) = Joints(i)*180.0/M_PI;
    }

    //Kuka Joints
    //Joints[3] = -Joints[3];
    Joints(1) = -Joints(1);
    Joints(5) = -Joints(5);

    return Joints;
}
#endif