/* 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.
  ========================================================================= */

#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__

#include "../headers/Geometry.h"
#include "../headers/Util.h"
#include "RobotConstants.h"

struct BodyPosition{
  vector3d loc;
  vector3d angle; // Euler angles around x,y,z axis
};

extern char *model;

class Kinematics {
public:
  enum RobotModel { ERS210, ERS7 };
  RobotConstants robot;
  RobotModel model;

private:
  int errors[5]; // 4 legs + head
  int cur_leg ;
  vector3d cur_target; // only set on Linux

public:

  Kinematics()
  {    
    if(strcmp(::model,"ERS210")==0){
      model = ERS210;
      robot = ers210_constants;
    }else{
      model = ERS7;
      robot = ers7_constants;
    }
    clearErrors();
  }

  Kinematics(RobotModel _model) :
    model(_model)
  {
    if(model==ERS210){
      robot = ers210_constants;
    }else{
      robot = ers7_constants;
    }

    clearErrors();
  }

  void clearErrors();
  int *getErrors();
  int getTotalErrors();

  // Calculate the joint angle to send to the robot for a particular
  // motion command (between -100 and 100). Generate the appropriate
  // joint angle for the robot being used (as the 210 and ers7 have
  // different ranges for their tail joint)
  double calcTailPan(double cmd_perc);
  double calcTailTilt(double cmd_perc);
  double calcTailPanNorm(double tail_angle);
  double calcTailTiltNorm(double tail_angle);


  double getTrigAngle(double a, double b, double d,
		      double min,double max,bool add);

  // A better version that reports errors properly, and returns the
  // closest solution within [min,max]
  int getTrigAngle2(double &theta,
		    double a, double b, double d,
		    double min,double max);

  void getLegAngles(double *angles,vector3d target,int leg);
  void getLegAngles(double *angles,vector3d target[4],
		    double body_angle,double body_height);
  void getLegAngles(double *angles,vector3d target[4],BodyPosition &bp);
  
  void getLegAngles(double *angles,vector3d target,BodyPosition &bp,int leg);
  vector3d getLegPos(double *angles,double body_angle,double body_height,int leg);
  
  void getBodyLocation(vector3d &ball,vector3d &toe,const double *ang,int leg);

  // get the tilt,pan,roll angles to point the head towards the target assuming 
  //   the given body_angle/body_height
  void getHeadAngles(double *angles,vector3d target,
		     double body_angle,double body_height);

  // converts the camera relative position "point" to a robot centric (under base of neck) position
  //  using the given head angles (tilt/pan/roll) and body_angle and body_height
  vector3d runForwardModel(double *angles, 
			   double body_angle, double body_height, 
			   vector3d point);

  // gets the location of the camera and basis vectors corresponding to the directions of the camera's
  //  z,-y,x corrdinate axis
  void getHeadPosition(vector3d &location, vector3d &direction, vector3d &up, vector3d &right,
		       double *angles, double body_angle, double body_height);

protected:
  void getHeadAnglesERS7(double *angles,vector3d target,
			   double body_angle,double body_height);
  void getHeadAnglesERS210(double *angles,vector3d target,
			   double body_angle,double body_height);

  vector3d runForwardModel7(double *angles, 
			    double body_angle, double body_height, 
			    vector3d point);
  vector3d runForwardModel210(double *angles, 
			      double body_angle, double body_height, 
			      vector3d point);
};

#endif
// __KINEMATICS_H__