#include "moveMech.h"


#define SWAP(A,B,T){T=A;A=B;B=T;}
#define printTr(A){\
	Rotation _temp;\
	tr2rot(A,_temp);\
	fprintf(stderr,"%s:\n",#A);\
	fprintf(stderr,"Rot: %3.3f\tTrans:X:%2.3f, Y:%2.3f\n\n",_temp[0],A[0][2],A[1][2]);\
}
#define truncate(_temp){\
   if(_temp<EPS && _temp>-EPS){\
      _temp = 0;\
   }\
}

static Mat CUR,GOAL;
static Motor motor;
static int dt;
static Vector trans={{0,0},{0,0}};
static Rotation rot = {0,0};



static void MtxToReducedREForm(float m[2][3])
{
   int lead;
   int rix, iix,temp;
   int rowCount = 2;
   int colCount = 3;
   float tempFloat;
   lead = 0;
   for(rix = 0; rix< rowCount;rix++){
      for(iix=0;iix<colCount;iix++){
         truncate((m[rix][iix]));
      }
   }
   for (rix=0; rix<rowCount; rix++) {
      if (lead >= colCount)
         return;
      iix = rix;
      while (0 == m[iix][lead]) {
         iix++;
         if (iix == rowCount) {
            iix = rix;
            lead++;
         if (lead == colCount)
            return;
         }
      }
      for(temp=0;temp<colCount;temp++){//MtxSwapRows(m, iix, rix );
         SWAP(m[iix][temp],m[rix][temp],tempFloat);
      }

      tempFloat = m[rix][lead];
      for(temp=0;temp<3&&m[rix][lead]!=0 ;temp++){ //MtxNormalizeRow(m, rix, lead );
         m[rix][temp] /= tempFloat;
      }
      for (iix=0; iix<rowCount; iix++) {
         if ( iix != rix ) {
            tempFloat = m[iix][lead];
            for(temp=0;temp<colCount;temp++){//MtxMulAndAddRows(m,iix, rix, -lv) ;
               m[iix][temp] += m[rix][temp]*(-tempFloat);
            }
         }
      }
      lead++;
   }
}
//Normalizes the Motor speeds to be no bigger then '1'
int normalizeMotor(Motor m){
      int i;
      float max = 1;
      for(i=0;i<4;i++){
               if(f_abs(m[i])>max){
                     max = f_abs(m[i]);
               }
      }
      if(max ==1){
               return 1;
      }
      for(i=0;i<4;i++){
               m[i] /= max;
      }
      return 0;
}
//roation,translation 2 transformation matrix, 
//calculates the transform matrix from the provided rotation and translation
static void rotTran2tr(Rotation rot,Vector trans, Mat A){
      A[0][0] = cos(rot[0]*pi/180);
      A[0][1] = -sin(rot[0]*pi/180);
      A[0][2] = trans[0][0];
      A[1][0] = sin(rot[0]*pi/180);
      A[1][1] = cos(rot[0]*pi/180);
      A[1][2] = trans[1][0];
      A[2][0] = 0;
      A[2][1] = 0;
      A[2][2] = 1;
}
//Transform 2 roation, caluclates the rotation angle from provided transform matrix
static void tr2rot(Mat A, Rotation rot){
      rot[0] = atan2(A[1][0],A[0][0])*180/pi;
}
//matrix multiplcation
static void mat_mult(Mat a, Mat b,Mat c){
   int   i, j , k;

   for(i = 0; i < 3; i++)  {
      for(j = 0; j < 3; j++){
         c[i][j] = 0.0 ;
         for(k = 0; k < 3; k++){
            c[i][j] += a[i][k] * b[k][j];
         }
      }
   }
}

//matrix inversion
//NOTE THIS IS ONLY FOR 3x3 matrix transforms. THis will not work for generalized 3x3 matrixes
static void mat_inv3x3( Mat a , Mat b ){
   float det;
   Mat adj;
   int i,j ;

   adj[0][0] =  (a[1][1]);
   adj[0][1] = -(a[0][1]);
   adj[0][2] =  (a[0][1] * a[1][2]) - (a[0][2] * a[1][1]) ;

   adj[1][0] = -(a[1][0]);
   adj[1][1] =  (a[0][0]);
   adj[1][2] =  (a[0][2] * a[1][0]) - (a[0][0] * a[1][2]) ;

   adj[2][0] =  0;
   adj[2][1] =  0;
   adj[2][2] =   (a[0][0] * a[1][1]) - (a[0][1] * a[1][0]) ;

   det = a[0][0] * adj[0][0] + a[0][1] * adj[1][0] + a[0][2] * adj[2][0];
   if ( det == 0.0 ){
		for(i = 0; i < 3; i++)
			for(j = 0; j < 3; j++)
				b[i][j] = 0;
	}

   for(i = 0; i < 3; i++)
      for(j = 0; j < 3; j++)
		 b[i][j] = adj[i][j] / det ;
}
/*
* Takes current motor speed and caluclates new motor relative speed
*/
static void getMotor(Mat CUR,Mat GOAL,Motor m){
   Mat NEED,invCUR;
   float A,B,C,D;
   Rotation rot = {0,0};
   mat_inv3x3(CUR,invCUR);
   mat_mult(invCUR,GOAL,NEED);	//NEED = inv(CUR)*GOAL;

   tr2rot(NEED,rot);
   rot[0] *= (pi/180)*RADIUS;
   B = (2*rot[0]);
   C = SQRT2*NEED[0][2];
   D = SQRT2*NEED[1][2];
   A = ((NEED[0][2]+NEED[1][2]) + -rot[0])/2;

   /*& Solving for this system without row reductions
   * M = [-1,1,-1,1,B;
   *      1,-1,-1,1,C;
   *      1,1,1,1,D;
   *      1,0,0,0, A];
   *  MS = rref(M)*/

   m[FL] = A;
   m[BR] = (C+D-2*A)/2;
   m[FR] = (B+D-2*m[BR])/2;
   m[BL] = D-A-m[BR]-m[FR];
   normalizeMotor(m);
}
/*
 * Takes motor speeds in m and calculates how it moved in trans and rot
 */
void fwd(Motor m, Vector trans, Rotation rot, uint16_t dt){
   float Vsxp,Vsyp;
   float rotTemp = rot[0]*pi/180;
   Vsxp = (((WHEELR)/4)*         ( m[FL]+m[FR]+m[BL]+m[BR]));
   Vsyp = (((WHEELR)/4)*         (-m[FL]+m[FR]+m[BL]-m[BR]));
   rot[1] = (((WHEELR)/(4*(W+L))*  (-m[FL]+m[FR]-m[BL]+m[BR])))* 180/pi;
   float A[2][3] = {{cos(rotTemp+pi/2),sin(rotTemp+pi/2),Vsxp},
                                    {-sin(rotTemp+pi/2),cos(rotTemp+pi/2),Vsyp}};
   //A = rref(A);
   MtxToReducedREForm(A);
   trans[0][1]  = A  [0][2];
   trans[1][1]  = A  [1][2];
   trans[0][0] += trans[0][1]/dt;
   trans[1][0] += trans[1][1]/dt;
   rot[0] += rot[1]/dt;
}
/*
* Checks if we are close enough to call ourselfs reached the point we wanted to
*/
static int checkEqual(Mat A, Mat B){

   Rotation rA,rB;
   tr2rot(A,rA);
   tr2rot(B,rB);
   if(rA[0] +EPS < rB[0] || rA[0] - EPS > rB[0]){
      return 0;
   }
   if(A[0][2] +EPS < B[0][2] || A[0][2] - EPS > B[0][2]){
      return 0;
   }
   if(A[1][2] +EPS < B[1][2] || A[1][2] - EPS > B[1][2]){
      return 0;
   }
   return 1;
}
/*Initilizing procedure
*/
void initMech(int deltaT){
	dt = deltaT;
	trans[0][0] = 0;
	trans[1][0] = 0;
	rot[0] = 0;
	rotTran2tr(rot,trans,GOAL);
	trans[0][0] = 0;
	trans[1][0] = 0;
	rot[0] = 0;
	rotTran2tr(rot,trans,CUR);
	getMotor(CUR,GOAL,motor);
	tr2rot(CUR,rot);
}
/*
* Seting a new goal to move twords, t is the translation 
*/
void setGoal(Vector t,Rotation rottemp){
	rotTran2tr(rottemp,t,GOAL);
}
/*
* Given where we are how do we need to move the motors to get to our goal
*/
void getNewSpeed(volatile Motor curSpeed,volatile Motor goalSpeed){
	fwd(curSpeed,trans,rot,dt);
	rotTran2tr(rot,trans,CUR);
	getMotor(CUR,GOAL,goalSpeed);
}
/*
* Where are we?
*/
void getPos(Vector xy, Rotation angle){
	xy[0][0] = trans[0][0];
	xy[0][1] = trans[0][1];
	xy[1][0] = trans[1][0];
	xy[1][1] = trans[1][1];
	angle[0] = rot[0];
	angle[1] = rot[1];
}