#include <math.h>


#include "Utilities.h"
#include "../Common/Common.h"
#include "../Globals.h"
#include "../LOCWM/Matrix.h"

Utilities::Utilities() {
}

void Utilities::RawCommand(LocomotionCommand lc) {
  // automagically fiddle priorities based on locomotion type.
  int p = 0;
  if (lc.motionType != LocomotionCommand::TP_WALK && lc.motionType != LocomotionCommand::TP_DONOTHING && lc.motionType != LocomotionCommand::TP_WALKWITHOUTFRONT && lc.motionType != LocomotionCommand::TP_WALKTURNKICK) p = 1;
  else if (lc.motionType == -1) p=-1;
  lcq_.Enqueue(p,lc);
}

void Utilities::Step(int type, double frontStrideLength, double backStrideLength, double turn, double strafe, double stepFrequency) {
  LocomotionCommand lc;
  lc.Set(type,frontStrideLength,backStrideLength,turn,strafe,stepFrequency);
  RawCommand(lc);
}

void Utilities::Kick(int type, bool useHead) {
 LocomotionCommand lc;
 lc.SetKick(type, useHead);
 //changed to RawCommand()
 //lcq_.Enqueue(1, lc);
 RawCommand(lc);
}

void Utilities::Kick(int type, bool useHead, double angle) {
 LocomotionCommand lc;
 lc.SetKick(type, useHead);
 lc.turn = angle;
 lc.odomTurn = 0;
 //changed to RawCommand()
 //lcq_.Enqueue(1, lc);
 RawCommand(lc);
}

void Utilities::Head_Pan(double left, double right, double tilt, double speed, bool reset) {
  HeadPoints p1(tilt,tilt,(left+right)/2,left,(int)speed);
  HeadPoints p2(tilt, tilt, left, (left+right)/2,(int)speed);
  HeadPoints p3(tilt, tilt, (left+right)/2, right,(int)speed);
  HeadPoints p4(tilt,tilt,right,(left+right)/2,(int)speed);

  HeadPoints points[4];
  points[0] = p1;
  points[1] = p2;
  points[2] = p3;
  points[3] = p4;

  Head_MoveThroughPoints(points, 4, reset);
}

void Utilities::Head_MoveTo(double tilt, double pan, double roll) {
  HeadCommand hc;
  hc.Set(tilt,pan,roll,false);
  lcq_.SetHeadCommand(hc);
}

void Utilities::Head_FollowObject(int cx, int cy) {
#ifdef ERS_7

  HeadCommand hc;

  double currentTiltBig = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT1]);
  double currentTiltSmall = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT2]);
  double currentPan  = MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]);

///
   double sinHeadPan = sin(currentPan);
   double cosHeadPan = cos(currentPan);
   double sinHeadTiltBig = sin(currentTiltBig);
   double cosHeadTiltBig = cos(currentTiltBig);
   double sinHeadTiltSmall = sin(currentTiltSmall);
   double cosHeadTiltSmall = cos(currentTiltSmall);

   double effectiveRoll = (asin(sinHeadPan*-1*sinHeadTiltBig)); 
///
  int xErrorOriginal = -(cx-(currentImageWidth_/2));
  int yErrorOriginal = -(cy-(currentImageHeight_/2));

  int xErrorRotated = (int) (cos(effectiveRoll)*xErrorOriginal - sin(effectiveRoll)*yErrorOriginal);
  int yErrorRotated = (int) (sin(effectiveRoll)*xErrorOriginal + cos(effectiveRoll)*yErrorOriginal);


  // Matrix row,column
  Matrix pixelErrors = Matrix(2,1);
  double radsPerPixelHoriz = FOVx/(double)currentImageWidth_;
  double radsPerPixelVert = FOVy/(double)currentImageHeight_;
  pixelErrors[0][0] = xErrorRotated*radsPerPixelHoriz;
  pixelErrors[1][0] = yErrorRotated*radsPerPixelVert;
/*
working, but doesn't lock big tilt where we want it ...
  Matrix X = Matrix(2,3);
  X[0][0] = 0;
  X[1][0] = 1;
  X[0][1] = sinHeadPan;
  X[1][1] = cosHeadPan;
  X[0][2] = 1;
  X[1][2] = 0;

  Matrix W = X.transp() * Invert22(X * X.transp()) * pixelErrors;

*/
  double smallTiltMotor = 0.0; 
  double panMotor = 0.0;
  double bigTiltMotor = 0.0; 
/*
  double desiredTiltBig = DEG_TO_RAD(-30.0);
  double changeTiltBig = desiredTiltBig - currentTiltBig;

  Matrix Xbt = Matrix(2,1);
  Xbt[0][0] = sinHeadPan;
  Xbt[1][0] = cosHeadPan;
  Matrix Xpt = Matrix(2,2);
  Xpt[0][0] = 0;
  Xpt[1][0] = 1;
  Xpt[0][1] = 1;
  Xpt[1][1] = 0;
  Matrix XptInv = Invert22(Xpt);
  Matrix W = XptInv * (pixelErrors - (Xbt * changeTiltBig));

  panMotor = RAD_TO_DEG(currentPan+W[1][0]);
  smallTiltMotor = RAD_TO_DEG(currentTiltSmall+W[0][0]);
  bigTiltMotor = RAD_TO_DEG(currentTiltBig+changeTiltBig);

  if (smallTiltMotor < -14.0) {
    Matrix X = Matrix(2,3);
    X[0][0] = 0;
    X[1][0] = 1;
    X[0][1] = sinHeadPan;
    X[1][1] = cosHeadPan;
    X[0][2] = 1;
    X[1][2] = 0;

    Matrix W2 = X.transp() * Invert22(X * X.transp()) * pixelErrors;

    smallTiltMotor = RAD_TO_DEG(currentTiltSmall+W[0][0]);
    bigTiltMotor = RAD_TO_DEG(currentTiltBig+W[1][0]);
    panMotor = RAD_TO_DEG(currentPan+W[2][0]);
  }
*/

  if (ABS(currentPan) > DEG_TO_RAD(35.0)) {
    // big pan, use anything !
    Matrix X = Matrix(2,3);
    X[0][0] = 0;
    X[1][0] = 1;
    X[0][1] = sinHeadPan;
    X[1][1] = cosHeadPan;
    X[0][2] = 1;
    X[1][2] = 0;

    Matrix W2 = X.transp() * Invert22(X * X.transp()) * pixelErrors;

    smallTiltMotor = RAD_TO_DEG(currentTiltSmall+W2[0][0]);
    bigTiltMotor = RAD_TO_DEG(currentTiltBig+W2[1][0]);
    panMotor = RAD_TO_DEG(currentPan+W2[2][0]);
  } else {
    // LOCK small tilt, use big tilt
    double desiredTiltSmall = DEG_TO_RAD(15.0);
    double changeTiltSmall = desiredTiltSmall - currentTiltSmall;

    Matrix Xst = Matrix(2,1);
    Xst[0][0] = 0;
    Xst[1][0] = 1;

    Matrix Xpt = Matrix(2,2);
    Xpt[0][0] = sinHeadPan;
    Xpt[1][0] = cosHeadPan;
    Xpt[0][1] = 1;
    Xpt[1][1] = 0;
    Matrix XptInv = Invert22(Xpt);
    Matrix W = XptInv * (pixelErrors - (Xst * changeTiltSmall));

    bigTiltMotor = RAD_TO_DEG(currentTiltBig+W[0][0]);
    panMotor = RAD_TO_DEG(currentPan+W[1][0]);
    smallTiltMotor = RAD_TO_DEG(currentTiltSmall+changeTiltSmall);

    // big tilt is getting low. LOCK big tilt here and start using small tilt
    if (bigTiltMotor < -37) {
      double desiredTiltBig = DEG_TO_RAD(-37.0);
      double changeTiltBig = desiredTiltBig - currentTiltBig;

      Matrix Xbt = Matrix(2,1);
      Xbt[0][0] = sinHeadPan;
      Xbt[1][0] = cosHeadPan;
      Matrix Xpt = Matrix(2,2);
      Xpt[0][0] = 0;
      Xpt[1][0] = 1;
      Xpt[0][1] = 1;
      Xpt[1][1] = 0;
      Matrix XptInv = Invert22(Xpt);
      Matrix W2 = XptInv * (pixelErrors - (Xbt * changeTiltBig));

      panMotor = RAD_TO_DEG(currentPan+W2[1][0]);
      smallTiltMotor = RAD_TO_DEG(currentTiltSmall+W2[0][0]);
      bigTiltMotor = RAD_TO_DEG(currentTiltBig+changeTiltBig);
    }
  }

//  double desiredTilt = RAD_TO_DEG(currentTilt) + 0.8*RAD_TO_DEG(atan((currentImageHeight_/2.0-newCy)/(currentImageHeight_/(2.0*tan(FOVy/2.0)))));
//  double desiredPan  = RAD_TO_DEG(currentPan)  + 0.8*RAD_TO_DEG(atan((currentImageWidth_ /2.0-newCx)/(currentImageWidth_ /(2.0*tan(FOVx/2.0)))));
  
/*  double bigTiltMotor = desiredTilt-20.0;
  double smallTiltMotor = 20.0;
  if (bigTiltMotor < -37) {  //was 37, then 33
    smallTiltMotor = desiredTilt+37.0;
    bigTiltMotor = -37.0;
  } else if (bigTiltMotor > -10) {
    smallTiltMotor = desiredTilt+10;
    bigTiltMotor = -10;
  }
*/
  hc.Set(bigTiltMotor,panMotor,smallTiltMotor,false); // FIXME interp is ON
  lcq_.SetHeadCommand(hc);
/*
  HeadCommand hc;
  double hTiltSmall = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT_SMALL]);
  double hTiltBig = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT_BIG]);
  double hPan  = MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]);

  double x1,y1,z1;

  x1 = vo_ball_->distance_ * (currentImageWidth_/2.0-cx)/(currentImageWidth_/(2.0*tan(FOVx/2.0)));
  y1 = vo_ball_->distance_ * (currentImageHeight_/2.0-cy)/(currentImageHeight_/(2.0*tan(FOVy/2.0)));
  z1 = vo_ball_->distance_;


  double x4 = x1;
  double y4 = y1;
  double z4 = z1 + HEADLENGTH;
  double x45 = x4;
  double y45 = y4 * cos(hTiltSmall) - z4 * sin(hTiltSmall);
  double z45 = y4 * sin(hTiltSmall) + z4 * cos(hTiltSmall);
    // robot head pan - fixed in the y direction
  double x5 = x45 * cos(hPan) + z45 * sin(hPan);
  double y5 = y45;
  double z5 = -x45 * sin(hPan) + z45 * cos(hPan);
    // robot neck length and distance from ground to neck
    double x6 = x5;
    double y6 = y5 + NECKLENGTH;
    double z6 = z5;

    // rotation for head tilt - fixed in the x direction
    double x7 = x6;
    double y7 = y6 * cos(hTiltBig) - z6 * sin(hTiltBig);
    double z7 = y6 * sin(hTiltBig) + z6 * cos(hTiltBig);


    if (z7 < NECKLENGTH) {
        z7 = NECKLENGTH;
    }

    double desiredTilt = RAD_TO_DEG(atan(y7/z7)-asin(NECKLENGTH/sqrt(z7*z7 + y7*y7)));

    double x8 = x7;
    
    double z8 = z7*cos(DEG_TO_RAD(-desiredTilt))-y7*sin(DEG_TO_RAD(-desiredTilt));
    
    double desiredPan = RAD_TO_DEG(atan(x8/z8));

  double bigTiltMotor = desiredTilt-15.0;
  double smallTiltMotor = 15.0;
  if (bigTiltMotor < -40) {
    smallTiltMotor = desiredTilt+40.0;
    bigTiltMotor = -40.0;
  }

  hc.Set(bigTiltMotor,desiredPan,smallTiltMotor,false);
  lcq_.SetHeadCommand(hc);
*/

#endif
#ifdef ERS_210

  HeadCommand hc;
  double currentTilt = MICRO_TO_DEG(sensorValues_[S_HEAD_TILT]);
  double currentPan  = MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]);

  double desiredTilt = currentTilt + RAD_TO_DEG(atan((currentImageHeight_/2.0-cy)/(currentImageHeight_/(2.0*tan(FOVy/2.0)))));
  double desiredPan  = currentPan  + RAD_TO_DEG(atan((currentImageWidth_ /2.0-cx)/(currentImageWidth_ /(2.0*tan(FOVx/2.0)))));
/*

  HeadCommand hc;
  double hTilt = MICRO_TO_RAD(sensorValues_[S_HEAD_TILT]);
  double hPan  = MICRO_TO_RAD(sensorValues_[S_HEAD_PAN]);


    double x1,y1,z1,x2,y2,z2,x3,y3,z3,x4,y4,z4,x5,z5;

    x1 = vo_ball_->distance_ * (currentImageWidth_/2.0-cx)/(currentImageWidth_/(2.0*tan(FOVx/2.0)));
    y1 = vo_ball_->distance_ * (currentImageHeight_/2.0-cy)/(currentImageHeight_/(2.0*tan(FOVy/2.0)));
    z1 = vo_ball_->distance_;

    x2 = x1;
    y2 = y1 + NECKLENGTH;
    z2 = z1 + HEADLENGTH;

    x3 = x2*cos((-hPan))-z2*sin((-hPan));
    y3 = y2;
    z3 = x2*sin((-hPan))+z2*cos((-hPan));

    x4 = x3;
    y4 = z3*sin((hTilt))+y3*cos((hTilt));
    z4 = z3*cos((hTilt))-y3*sin((hTilt));

    if (z4 < NECKLENGTH) {
        z4 = NECKLENGTH;
    }

    double desiredTilt = RAD_TO_DEG(atan(y4/z4)-asin(NECKLENGTH/sqrt(z4*z4 + y4*y4)));

    x5 = x4;
    
    z5 = z4*cos(DEG_TO_RAD(-desiredTilt))-y4*sin(DEG_TO_RAD(-desiredTilt));
    
    double desiredPan = RAD_TO_DEG(atan(x5/z5));

*/
  hc.Set(desiredTilt,desiredPan,0.0,false);
  lcq_.SetHeadCommand(hc);
#endif
}

void Utilities::Head_PanForBall(bool reset, bool right, int totalFrames) {
  int mult = 1;
  if (right) mult = -1;
  double tFrames = (double)totalFrames;
  HeadPoints p1(-18,-18,-55,-55,0.0,0.0,(int)(4.0/70.0*tFrames));
  HeadPoints p2(-18,-18,-55,-55,0.0,-60*mult,(int)(4.0/70.0*tFrames));
  HeadPoints p3(-18,-18,-55,-45,-60*mult,60*mult,(int)(16.0/70.0*tFrames));
  HeadPoints p4(-18,0,-45,15,60*mult,80*mult,(int)(5.0/70.0*tFrames));
  HeadPoints p5(0,0,15,15,80*mult,-80*mult,(int)(30.0/70.0*tFrames));
  HeadPoints p6(0,-18,15,-45,-80*mult,-60*mult,(int)(6.0/70.0*tFrames));
  HeadPoints p7(-18,-18,-45,-55,-60*mult,0,(int)(5.0/70.0*tFrames));

/*
  HeadPoints p1(0, 0, 15, 15, -80*mult, 80*mult, (int)(30.0/70.0*tFrames));
  HeadPoints p2(0, -10, 15, -45, 80*mult, 60*mult, (int)(8.0/70.0*tFrames));
  HeadPoints p3(-10, -10, -45, -55, 60*mult, 0*mult, (int)(12.0/70.0*tFrames));
  HeadPoints p4(-10, -10, -55, -45, 0*mult, -60*mult, (int)(12.0/70.0*tFrames)); 
  HeadPoints p5(-10, 0, -45, 15, -60*mult, -80*mult, (int)(8.0/70.0*tFrames)); */
  HeadPoints points[7];
  points[0] = p1;
  points[1] = p2;
  points[2] = p3;
  points[3] = p4;
  points[4] = p5;
  points[5] = p6;
  points[6] = p7;

  Head_MoveThroughPointsNoClosest(points, 7, reset);
}

// This might not work anymore .. it is crap anyway
bool Utilities::Head_FlickLook(bool left, bool restart) {
  HeadPoints p0(30, 30, 0, 0, 10);
  HeadPoints p1(30, 30, 0, -80, 30);
  HeadPoints p2(30, 30, -80, 80, 60);
  HeadPoints p3(30, 30, 80, 0, 30);
  HeadPoints points[4];
  points[0] = p0;
  points[1] = p1;
  points[2] = p2;
  points[3] = p3;
  return Head_MoveThroughPoints(points, 4, restart);
} 

bool Utilities::Head_MoveThroughPoints(HeadPoints points[], int arraySize, bool reset) {
  static int index = 0; // The current array value we are using
  static int currFrame = 0; // The current frame for the current index
  if (reset) { // Set the position to the close point in the array

    int bestIndex = -1;
    double bestDistance = -1;
    for (int i=0; i< arraySize; i++) {
#ifdef ERS_7
      double tempDistance = pow((MICRO_TO_DEG(sensorValues_[S_HEAD_TILT1]) - points[i].sTilt1), 2) + 
                            pow((MICRO_TO_DEG(sensorValues_[S_HEAD_TILT2]) - points[i].sTilt2), 2) + 
                            pow((MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]) - points[i].sPan), 2);
#endif
#ifdef ERS_210
      double tempDistance = pow((MICRO_TO_DEG(sensorValues_[S_HEAD_TILT]) - points[i].sTilt), 2) + 
                            pow((MICRO_TO_DEG(sensorValues_[S_HEAD_PAN]) - points[i].sPan), 2);
#endif

      if ((tempDistance < bestDistance) || bestIndex == -1) {
        bestIndex = i;
        bestDistance = tempDistance;
      }
    }
    index = bestIndex;
    currFrame = 0;
  }  
  if (index < 0 || index > arraySize-1) {
    cout << "Utilities::Head_MoveThroughPoints: critical error. Breaking out." << endl << flush;
    return false;
  }

    // Calculate the points to move the head to
  HeadPoints currPoints = points[index];
  if (currPoints.frames == 0) {
    cout << "Utilities::Head_MoveThroughPoints: divide by zero. Breaking out." << endl << flush;
    return false;
  }
    // Calculate the amount to increment the head by
  double incTilt1 = (currPoints.eTilt1 - currPoints.sTilt1) / currPoints.frames;
  double incTilt2 = (currPoints.eTilt2 - currPoints.sTilt2) / currPoints.frames;
  double incPan = (currPoints.ePan - currPoints.sPan) / currPoints.frames;
     // Get the tilt and pan values
  double tilt1 = currPoints.sTilt1 +  (incTilt1 * currFrame);
  double tilt2 = currPoints.sTilt2 +  (incTilt2 * currFrame);
  double pan = currPoints.sPan + (incPan * currFrame);
    // Move the head  
#ifdef ERS_7
  Head_MoveTo(tilt1, pan, tilt2);
#endif
#ifdef ERS_210
  Head_MoveTo(tilt2, pan, 0);
#endif
  currFrame++;
  if (currFrame >= currPoints.frames) {
    currFrame = 0;
    index++;
    if (index >= arraySize) {
      index = 0;
      return true;
    }  
  }
  return false;
}

bool Utilities::Head_MoveThroughPointsNoClosest(HeadPoints points[], int arraySize, bool reset) {
  static int index = 0; // The current array value we are using
  static int currFrame = 0; // The current frame for the current index
  if (reset) { // Set the position to the zero point.
    index = 0;
    currFrame = 0;
  }  

    // Calculate the points to move the head to
  HeadPoints currPoints = points[index];
  if (currPoints.frames == 0) {
    cout << "Utilities::Head_MoveThroughPoints: divide by zero. Breaking out." << endl << flush;
    return false;
  }
    // Calculate the amount to increment the head by