/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */


#ifdef PLATFORM_LINUX
#include <stdlib.h>
#include <stdio.h>
#endif

#include <math.h>

#include "Kinematics.h"
#include "RobotConstants.h"

#ifdef PLATFORM_APERIOS
#include "../headers/CircBufPacket.h"

extern PacketStream *TextOutputStream;
#endif

//#define DEBUG

/*
  18 : measured
  67.23 = sqrt(69.6^2 - 18^2)
  74.87 = sqrt(77^2 - 18^2)
  0.2616 = asin(18 / 69.6)
  0.2316 = asin(18 / 77)
*/

/* ERS-110
const vector3d f_body_to_shoulder       ( 44.85, 26.50,   0.00);
const vector3d f_leg_shoulder_to_knee   ( 13.00,  5.50, -61.00);
const vector3d f_leg_knee_to_ball       ( -9.50,  1.06, -58.00); // ??

const vector3d h_body_to_shoulder       (-44.85, 26.50,   0.00);
const vector3d h_leg_shoulder_to_knee   (-13.00,  5.50, -61.00);
const vector3d h_leg_knee_to_ball       ( 19.00,  1.06, -69.00); // ??
*/

#ifdef PLATFORM_LINUX
void print(double *angles,vector3d pos)
{
  printf("A(%7.4f,%7.4f,%7.4f) P(%7.2f,%7.2f,%7.2f)\n",
	 angles[0],angles[1],angles[2],
	 pos.x,pos.y,pos.z);
}
#endif


void Kinematics::clearErrors()
{
  for(int i=0; i<4; i++)
    errors[i] = 0;
}

int *Kinematics::getErrors()
{
  return errors;
}

int Kinematics::getTotalErrors()
{
  int e = 0;

  for(int i=0; i<4; i++) 
    e += errors[i];
  
  return e;
}

double Kinematics::calcTailPan(double cmd_perc)
{
  cmd_perc /= 100.0;

  double range = robot.tail_pan_max - robot.tail_pan_min;
  double mid_pt = robot.tail_pan_min + range/2.0;

  double target = mid_pt + cmd_perc*(range/2.0);

  return bound(target, robot.tail_pan_min, robot.tail_pan_max);
}

double Kinematics::calcTailTilt(double cmd_perc)
{
  cmd_perc /= 100.0;

  double range = robot.tail_tilt_max - robot.tail_tilt_min;
  double mid_pt = robot.tail_tilt_min + range/2.0;

  double target = mid_pt + cmd_perc*(range/2.0);

  return bound(target, robot.tail_tilt_min, robot.tail_tilt_max);
}

double Kinematics::calcTailPanNorm(double tail_angle)
{
  double range = robot.tail_pan_max - robot.tail_pan_min;
  double mid_pt = robot.tail_pan_min + range/2.0;
  
  double delta = tail_angle - mid_pt;

  double norm = delta/(range/2.0);

  return bound(norm, -1, 1);
}

double Kinematics::calcTailTiltNorm(double tail_angle)
{
  double range = robot.tail_tilt_max - robot.tail_tilt_min;
  double mid_pt = robot.tail_tilt_min + range/2.0;
  
  double delta = tail_angle - mid_pt;

  double norm = delta/(range/2.0);

  return bound(norm, -1, 1);
}

// Analytic solution to a*sin(x) + b*cos(x) = d
double Kinematics::getTrigAngle(double a, double b, double d,
				double min,double max,bool add)
{
  static const bool perror = false;
  double theta;
  double t,f,c;
  int err;
  
  f = d / sqrt(a*a + b*b);
  err = 0;

  if(fabs(f) > 1.0){
#ifdef PLATFORM_LINUX
    if(perror){
      printf("Out of range (distance=%g) leg=%d\n",f,cur_leg);
    }
#endif
    f = (f > 0.0)? 1.0 : -1.0;
    err = 1;
  }

  t = atan2(a,b);
  c = acos(f);

  theta = add? (t + c) : (t - c);

  if(theta < min){
#ifdef PLATFORM_LINUX
    if(perror){
      printf("Out of range (angle to small) leg=%d\n",cur_leg);
    }
#endif
    errors[cur_leg]++;
    return(min);
  }else if(theta > max){
#ifdef PLATFORM_LINUX
    if(perror){
      printf("Out of range (angle to large) leg=%d\n",cur_leg);
    }
#endif
    errors[cur_leg]++;
    return(max);
  }else{
    errors[cur_leg] += err;
    return(theta);
  }
}

int Kinematics::getTrigAngle2(double &theta,
			      double a, double b, double d,
			      double min,double max)
{
  double t,f,c;
  double r1,r2;
  double theta1,theta2;
  int err = 0;

  f = d / sqrt(a*a + b*b);

  if(fabs(f) > 1.0){
    f = (f > 0.0)? 1.0 : -1.0;
    err = 1;
  }

  t = atan2(a,b);
  c = acos(f);

  // check first solution
  theta1 = t + c;
  r1 = 0.0;
  if(theta1 < min){
    r1 = theta1 - min;
  }else if(theta1 > max){
    r1 = max - theta1;
  }

  if(r1 == 0.0){
    theta = theta1;
    return(err);
  }

  // check second solution
  theta2 = t - c;
  r2 = 0.0;
  if(theta2 < min){
    r2 = theta2 - min;
  }else if(theta2 > max){
    r2 = max - theta2;
  }

  if(r2 == 0.0){
    theta = theta2;
    return(err);
  }

  // no solution, return nearest angle
  if(fabs(r1) < fabs(r2)){
    theta = bound(theta1,min,max);
  }else{
    theta = bound(theta2,min,max);
  }
  err = 2;
  return(err);
}

/*
Angle Limits:
           ===Software====  ==Mechanical===
  Rotator  [-117.0, 117.0]  [-120.0, 120.0]
  Shoulder [ -11.0,  97.0]  [ -14.0, 100.0]
  Knee     [ -27.0, 147.0]  [ -30.0, 150.0]
*/

void Kinematics::getLegAngles(double *angles,vector3d target,int leg)
{
  vector3d targ,pos;
  double knee,shoulder,rotator;
  double a,b,d,dist;
  bool front = leg < 2;

  cur_leg = leg;

#ifdef PLATFORM_LINUX
  cur_target = target;
  //printf("GLA: target=(%g,%g,%g)\n",target.x,target.y,target.z);
#endif

  knee = shoulder = rotator = 0.0;

  if(leg % 2) target.y = -target.y;

  if(front){
    targ = target - robot.f_body_to_shoulder;
    dist = targ.sqlength();

    // Calculate knee angle
    a = -2*(robot.f_upper.x*robot.f_lower.z - robot.f_upper.z*robot.f_lower.x);
    b =  2*(robot.f_upper.x*robot.f_lower.x + robot.f_upper.z*robot.f_lower.z);
    d = (dist - robot.f_upper.sqlength() - robot.f_lower.sqlength() - 2*robot.f_upper.y*robot.f_lower.y);
    knee = getTrigAngle(a,b,d,robot.f_knee_kmin,robot.knee_kmax,true);

    // Calculate shoulder angle
    pos = robot.f_leg_shoulder_to_knee + robot.f_leg_knee_to_ball.rotate_y(-knee);
    shoulder = getTrigAngle(-pos.z,pos.y,targ.y,robot.shoulder_kmin,robot.shoulder_kmax,false);

    // Calculate rotator angle
    // pos = pos.rotate_x(shoulder);
    pos.z = sin(shoulder)*pos.y + cos(shoulder)*pos.z;
    rotator = getTrigAngle(-pos.z,pos.x,targ.x,robot.rotator_min,
			   robot.rotator_max,target.z > 0.0);

#ifdef DEBUG
    // Test
    pos = (robot.f_leg_shoulder_to_knee + robot.f_leg_knee_to_ball.rotate_y(-knee))
           .rotate_x(shoulder).rotate_y(-rotator);
    printf("D(%f,%f,%f)\n",targ.x - pos.x,targ.y - pos.y,targ.z - pos.z);
#endif
  }else{
    targ = target - robot.h_body_to_shoulder;
    dist = targ.sqlength();

    // Calculate knee angle
    a = 2*(robot.h_upper.x*robot.h_lower.z - robot.h_upper.z*robot.h_lower.x);
    b = 2*(robot.h_upper.x*robot.h_lower.x + robot.h_upper.z*robot.h_lower.z);
    d = (dist - robot.h_upper.sqlength() - robot.h_lower.sqlength() - 2*robot.h_upper.y*robot.h_lower.y);
    knee = getTrigAngle(a,b,d,robot.h_knee_kmin,robot.knee_kmax,true);

    // Calculate shoulder angle
    pos = robot.h_leg_shoulder_to_knee + robot.h_leg_knee_to_ball.rotate_y(knee);
    shoulder = getTrigAngle(-pos.z,pos.y,targ.y,robot.shoulder_kmin,robot.shoulder_kmax,false);

    // Calculate rotator angle
    pos.z = sin(shoulder)*pos.y + cos(shoulder)*pos.z;
    rotator = getTrigAngle(-pos.z,-pos.x,-targ.x,
			   robot.rotator_min,robot.rotator_max,target.z > 0.0);

    /*
      if(fabs(theta - rotator) > 0.01){
      printf("ERROR(%f - %f = %f)\n",theta,rotator,theta - rotator);
      }
    */

#ifdef DEBUG
    // Test
    pos = (robot.h_leg_shoulder_to_knee + robot.h_leg_knee_to_ball.rotate_y(knee))
      .rotate_x(shoulder).rotate_y(rotator);
    printf("D(%f,%f,%f)\n",targ.x - pos.x,targ.y - pos.y,targ.z - pos.z);
#endif
  }

  angles[0] = rotator;
  angles[1] = shoulder;
  angles[2] = knee;
}

void Kinematics::getLegAngles(double *angles,vector3d target[4],
			      double body_angle,double body_height)
{
  vector3d p;
  
  for(int i=0; i<4; i++){
    p = target[i];
    p.z -= body_height;
    p = p.rotate_y(-body_angle);
    getLegAngles(angles + 3*i,p,i);
  }
}

void Kinematics::getLegAngles(double *angles,vector3d target[4],BodyPosition &bp)
{
  vector3d p;

  for(int i=0; i<4; i++){
    p = (target[i] - bp.loc).rotate_z(bp.angle.z).rotate_y(-bp.angle.y);
    getLegAngles(angles + 3*i,p,i);
  }
}

void Kinematics::getLegAngles(double *angles,vector3d target,
			      BodyPosition &bp,int leg)
{
  vector3d p;

  p = (target - bp.loc).rotate_z(bp.angle.z).rotate_y(-bp.angle.y);
  getLegAngles(angles,p,leg);
}


vector3d Kinematics::getLegPos(double *angles,double body_angle,double body_height,int leg)
{
  vector3d p;
  bool left = (leg % 2 == 0);
  
  if(leg < 2){
    p =  robot.f_leg_knee_to_ball.rotate_y(-angles[2]);
    p += robot.f_leg_shoulder_to_knee;
    p = p.rotate_x( angles[1]);
    p = p.rotate_y(-angles[0]);
    p += robot.f_body_to_shoulder;    
  }else{
    p =  robot.h_leg_knee_to_ball.rotate_y( angles[2]);
    p += robot.h_leg_shoulder_to_knee;
    p = p.rotate_x( angles[1]);
    p = p.rotate_y( angles[0]);
    p += robot.h_body_to_shoulder;    
  }
  
  if(!left) p.y = -p.y;

  p = p.rotate_y(-body_angle);
  p.z += body_height;

  return p;
}


// project various parts of foot that could touch by given joint angles
void Kinematics::getBodyLocation(vector3d &ball, vector3d &toe,
				 const double *ang,int leg)
{
  if(leg < 2){
    ball = robot.f_body_to_shoulder +
      (robot.f_leg_shoulder_to_knee + robot.f_leg_knee_to_ball.rotate_y(-ang[2]))
      .rotate_x(ang[1]).rotate_y(-ang[0]);
  }else{
    ball = robot.h_body_to_shoulder +
      (robot.h_leg_shoulder_to_knee + robot.h_leg_knee_to_ball.rotate_y( ang[2]))
      .rotate_x(ang[1]).rotate_y( ang[0]);
  }

  // TODO: toes
  toe = ball;

  if(leg % 2){
    ball.y = -ball.y;
    toe.y = -toe.y;
  }
}

void Kinematics::getHeadAnglesERS7(double *angles,vector3d target,
				   double body_angle,double body_height)
{
  double tilt=0,pan=0,tilt2=0;
  vector3d neck, targ;
  double height;

  static const bool debug = false;
  bool found;
  double step_size;

  // for error reporting purposes, the head is leg #4
  cur_leg = 4;

  // translate target so it is relative to base of neck
  neck = robot.body_to_neck.rotate_y(body_angle);
  height = body_height + neck.z;
  target.z -= height;

  tilt = robot.head_tilt_max;
  step_size = (robot.head_tilt_max - robot.head_tilt_min) / 8.0 - 1E-9;

  while(tilt > robot.head_tilt_min){
    targ = target.rotate_y(tilt-body_angle);

    pan = atan2(targ.y,targ.x);
    targ = targ.rotate_z(-pan) - robot.neck_to_head;

    found = (getTrigAngle2(tilt2, -targ.x,targ.z,robot.head_to_camera.z,
			   robot.head_tilt2_min,robot.head_tilt2_max) == 0);

    if(debug) printf("%d: %f %f ->",found,tilt,step_size);

    if(found){
      // stop if good enough
      if(tilt==robot.head_tilt_max || step_size<=RAD(1)) break;

      // found a solution, but the largest tilt one is probably behind
      // us, so back up one step.
      step_size *= 0.5;
      tilt += step_size;
    }else{
      tilt -= step_size;
    }

    if(debug) printf("%f %f\n",tilt,step_size);
  }

  if(debug) printf("tilt=%f\n",tilt);

  angles[0] = tilt;
  angles[1] = pan;
  angles[2] = tilt2;
}

void Kinematics::getHeadAnglesERS210(double *angles,vector3d target,
				     double body_angle,double body_height)
{
  double tilt,pan;
  vector3d neck;
  double height;

  // translate target so it is relative to base of neck
  neck = robot.body_to_neck.rotate_y(body_angle);
  height = body_height + neck.z;
  target.z -= height;

  double target_xz_dist; // distance in robot's xz plane of target

  target_xz_dist=hypot(target.x,target.z);

  // assumes that camera is aligned with base of neck
  // can only see if not too close to neck
  if(target_xz_dist > robot.neck_to_camera.z && target.length() > robot.neck_to_camera.length()) {
    double angle_base_target; // angle from base of neck to target xz
    double angle_base_target_camera; // angle between base and camera from target xz

    angle_base_target = atan2(target.z,target.x);
    angle_base_target_camera = asin(robot.neck_to_camera.z/target_xz_dist);

    tilt = angle_base_target - angle_base_target_camera + body_angle;
    tilt = bound(tilt,robot.head_tilt_min,robot.head_tilt_max);

    double camera_dist_to_target;
    camera_dist_to_target = sqrt(target_xz_dist*target_xz_dist - 
                                 robot.neck_to_camera.z*robot.neck_to_camera.z);

    pan = atan2(target.y,camera_dist_to_target);
    pan = bound(pan,robot.head_pan_min,robot.head_pan_max);
  } else {
    tilt = pan = 0.0;
  }

  angles[0] = tilt;
  angles[1] = pan;
  angles[2] = 0.0; // roll
}

void Kinematics::getHeadAngles(double *angles,vector3d target,
			       double body_angle,double body_height)
{
  if(model == ERS7){
    getHeadAnglesERS7(angles,target,body_angle,body_height);
  }else if(model == ERS210){
    getHeadAnglesERS210(angles,target,body_angle,body_height);
  }else{
#ifdef PLATFORM_APERIOS
    pprintf(TextOutputStream,"ERROR: Robot model not set\n");
#else
    printf("ERROR: Robot model not set\n");
#endif
  }
}

vector3d Kinematics::runForwardModel210(double *angles, double body_angle, 
					double body_height, vector3d point)
{  
  double tilt=0.0,pan=0.0,roll=0.0;
  
  tilt = angles[0];
  pan  = angles[1];
  roll = angles[2];
  
  point = point.rotate_x(roll);
  point += robot.neck_to_camera;
  point = point.rotate_z(pan);
  point = point.rotate_y(-tilt+body_angle);
  
  vector3d neck;
  
  neck = robot.body_to_neck;
  neck = neck.rotate_y(body_angle);
  neck.z += body_height;
  
  point.z += neck.z;
  
  return point;
}

vector3d Kinematics::runForwardModel7(double *angles, double body_angle, 
				      double body_height, vector3d point)
{
  double neck_tilt=0.0, pan=0.0, head_tilt=0.0;

  neck_tilt = angles[0];
  pan       = angles[1];
  head_tilt = angles[2];

  point += robot.head_to_camera;
  point = point.rotate_y(-head_tilt);
  point += robot.neck_to_head;
  point = point.rotate_z(pan);
  point = point.rotate_y(-neck_tilt+body_angle);
  
  vector3d neck;
  
  neck = robot.body_to_neck;
  neck = neck.rotate_y(body_angle);
  neck.z += body_height;
  
  point.z += neck.z;
  
  return point;
}

vector3d Kinematics::runForwardModel(double *angles, double body_angle,
				     double body_height, vector3d point)
{
  if(model == ERS7){
    return runForwardModel7(angles, body_angle, body_height, point);
  }else if(model == ERS210){
    return runForwardModel210(angles, body_angle, body_height, point);
  }else{
#ifdef PLATFORM_APERIOS
    pprintf(TextOutputStream,"ERROR: Robot model not set\n");
#else
    printf("ERROR: Robot model not set\n");
#endif
    return(vector3d(0,0,0));
  }
}

// calculatesg the pose of the camera
// location  = location of camera in robot coordinates relative to point on ground under neck
// direction = unit vector pointing in direction of camera
// up        = unit vector pointing in direction of higher on image
// right     = unit vector pointing in direction of more right on image

void Kinematics::getHeadPosition(vector3d &location, vector3d &direction,
				 vector3d &up, vector3d &right,
				 double *angles, double body_angle,
				 double body_height)
{  
  location = runForwardModel(angles, body_angle, body_height,
			     vector3d(0.0,0.0,0.0));

  vector3d image_x,image_y,image_z;
  
  image_x = runForwardModel(angles, body_angle, body_height,
			    vector3d(0.0,-1.0, 0.0));
  image_y = runForwardModel(angles, body_angle, body_height,
			    vector3d(0.0, 0.0, 1.0));
  image_z = runForwardModel(angles, body_angle, body_height, 
			    vector3d(1.0, 0.0, 0.0));
  
  direction = image_z - location;
  direction = direction.norm();
  
  up = image_y - location;
  up = up.norm();
  
  right = image_x - location;
  right = right.norm();
}