/* 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 RobotConstants_h
#define RobotConstants_h

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




struct RobotConstants {
  RobotConstants()
  {}
  
  RobotConstants(vector3d _f_body_to_shoulder,
		 vector3d _f_leg_shoulder_to_knee,
		 vector3d _f_leg_knee_to_ball,
		 vector3d _h_body_to_shoulder,
		 vector3d _h_leg_shoulder_to_knee,
		 vector3d _h_leg_knee_to_ball,
		 vector3d _body_to_neck,
		 vector3d _neck_to_camera,
		 vector3d _neck_to_head,
		 vector3d _head_to_camera,
		 double _rotator_min,
		 double _rotator_max,
		 double _shoulder_min,
		 double _shoulder_max,
		 double _knee_min,
		 double _knee_max,
		 double _rotator_kmin,
		 double _rotator_kmax,
		 double _shoulder_kmin,
		 double _shoulder_kmax,
		 double _f_knee_kmin,
		 double _h_knee_kmin,
		 double _knee_kmax,
		 double _tail_tilt_min,
		 double _tail_tilt_max,
		 double _tail_pan_min,
		 double _tail_pan_max,
		 double _head_tilt_min,
		 double _head_tilt_max,
		 double _head_pan_min,
		 double _head_pan_max,
		 double _head_roll_min,
		 double _head_roll_max,
		 double _head_tilt2_min,
		 double _head_tilt2_max,
		 double _mouth_min,
		 double _mouth_max) :
    f_body_to_shoulder(_f_body_to_shoulder),
       f_leg_shoulder_to_knee(_f_leg_shoulder_to_knee),
       f_leg_knee_to_ball(_f_leg_knee_to_ball),
       h_body_to_shoulder(_h_body_to_shoulder),
       h_leg_shoulder_to_knee(_h_leg_shoulder_to_knee),
       h_leg_knee_to_ball(_h_leg_knee_to_ball),
       body_to_neck(_body_to_neck),
       neck_to_camera(_neck_to_camera),
       neck_to_head(_neck_to_head),
       head_to_camera(_head_to_camera),
       f_upper(_f_leg_shoulder_to_knee),
       f_lower(_f_leg_knee_to_ball),
       h_upper(_h_leg_shoulder_to_knee),
       h_lower(_h_leg_knee_to_ball),
       rotator_min(_rotator_min),
       rotator_max(_rotator_max),
       shoulder_min(_shoulder_min),
       shoulder_max(_shoulder_max),
       knee_min(_knee_min),
       knee_max(_knee_max),
       rotator_kmin(_rotator_kmin),
       rotator_kmax(_rotator_kmax),
       shoulder_kmin(_shoulder_kmin),
       shoulder_kmax(_shoulder_kmax),
       f_knee_kmin(_f_knee_kmin),
       h_knee_kmin(_h_knee_kmin),
       knee_kmax(_knee_kmax),
       tail_tilt_min(_tail_tilt_min),
       tail_tilt_max(_tail_tilt_max),
       tail_pan_min(_tail_pan_min),
       tail_pan_max(_tail_pan_max),
       head_tilt_min(_head_tilt_min),
       head_tilt_max(_head_tilt_max),
       head_pan_min(_head_pan_min),
       head_pan_max(_head_pan_max),
       head_roll_min(_head_roll_min),
       head_roll_max(_head_roll_max),
       head_tilt2_min(_head_tilt2_min),
       head_tilt2_max(_head_tilt2_max),
       mouth_min(_mouth_min),
       mouth_max(_mouth_max)
  {
    // do nothing in constructor.
  }
  
  vector3d f_body_to_shoulder;
  vector3d f_leg_shoulder_to_knee;
  vector3d f_leg_knee_to_ball;
  vector3d h_body_to_shoulder;
  vector3d h_leg_shoulder_to_knee;
  vector3d h_leg_knee_to_ball;

  vector3d body_to_neck;
  vector3d neck_to_camera;
  vector3d neck_to_head;
  vector3d head_to_camera;

  vector3d f_upper;
  vector3d f_lower;
  vector3d h_upper;
  vector3d h_lower;

  double rotator_min;
  double rotator_max;
  double shoulder_min;
  double shoulder_max;
  double knee_min;
  double knee_max;
  
  double rotator_kmin;
  double rotator_kmax;
  double shoulder_kmin;
  double shoulder_kmax;
  double f_knee_kmin;
  double h_knee_kmin;
  double knee_kmax;

  double tail_tilt_min;
  double tail_tilt_max;
  double tail_pan_min;
  double tail_pan_max;
  
  double head_tilt_min;
  double head_tilt_max;
  double head_pan_min;
  double head_pan_max;
  double head_roll_min;
  double head_roll_max;
  double head_tilt2_min;
  double head_tilt2_max;
  double mouth_min;
  double mouth_max;
};

const RobotConstants ers210_constants = 
RobotConstants(vector3d( 59.50, 59.20,   0.00) /* _f_body_to_shoulder */,
	       vector3d( 12.80,  0.50, -64.00) /* _f_leg_shoulder_to_knee */,
	       vector3d(-18.00,  0.00, -67.23) /* _f_leg_knee_to_ball */,
	       vector3d(-59.50, 59.20,   0.00) /* _h_body_to_shoulder */,
	       vector3d(-12.80,  0.50, -64.00) /* _h_leg_shoulder_to_knee */,
	       vector3d( 18.00,  0.00, -74.87) /* _h_leg_knee_to_ball */,
	       vector3d( 75.00,  0.00,  50.00) /* _body_to_neck */,
	       vector3d( 65.00,  0.00,  48.00) /* _neck_to_camera */,
	       vector3d( 0.00,  0.00,  0.00) /* _neck_to_head */,
	       vector3d( 0.00,  0.00,  0.00) /* _head_to_camera */,
	       RAD(-117.0) /* rotator_min */,
	       RAD( 117.0) /* rotator_max */,
	       RAD( -11.0) /* shoulder_min */,
	       RAD(  97.0) /* shoulder_max */,
	       RAD( -27.0) /* knee_min */,
	       RAD( 147.0) /* knee_max */,
	       RAD(-90.0) /* rotator_kmin */,
	       RAD( 90.0) /* rotator_kmax */,
	       RAD( -11.0) /* shoulder_kmin */,
	       RAD( 90.0) /* shoulder_kmax */,
	       0.2616 /* f_knee_kmin */,
	       0.2316 /* h_knee_kmin */,
	       RAD( 147.0) /* knee_kmax */,
	       RAD(-22) /* tail_tilt_min */,
	       RAD( 22) /* tail_tilt_max */,
	       RAD(-22) /* tail_pan_min */,
	       RAD(22) /* tail_pan_max */,
	       RAD(-88.5) /* head_tilt_min */,
	       RAD( 43.0) /* head_tilt_max */,
	       RAD(-89.6) /* head_pan_min */,
	       RAD( 89.6) /* head_pan_max */,
	       RAD(-29.0) /* head_roll_min */,
	       RAD( 29.0) /* head_roll_max */,
	       0.0 /* head_tilt2_min */,
	       0.0 /* head_tilt2_max */,
	       0.0 /* mouth_min */,
	       0.0 /* mouth_max */);


const RobotConstants ers7_constants = 
RobotConstants(vector3d( 65.00, 62.50,   0.00) /* _f_body_to_shoulder */,
	       vector3d(  9.00,  4.70, -69.50) /* _f_leg_shoulder_to_knee */,
	       vector3d(-13.13,  0.00, -75.77) /* _f_leg_knee_to_ball */,
	       vector3d(-65.00, 62.50,   0.00) /* _h_body_to_shoulder */,
	       vector3d( -9.00,  4.70, -69.50) /* _h_leg_shoulder_to_knee */,
	       vector3d( 19.02,  0.00, -77.10) /* _h_leg_knee_to_ball */,
	       vector3d( 67.50,  0.00,  19.50) /* _body_to_neck */,
	       vector3d( 81.06,  0.00,  65.40) /* _neck_to_camera */,
	       vector3d(  0.00,  0.00,  80.00) /* _neck_to_head */,
	       vector3d( 81.06,  0.00, -14.60) /* _head_to_camera */,
	       RAD(-115.0) /* rotator_min */,
	       RAD( 130.0) /* rotator_max */,
	       RAD( -10.0) /* shoulder_min */,
	       RAD(  88.0) /* shoulder_max */,
	       RAD( -25.0) /* knee_min */,
	       RAD( 122.0) /* knee_max */,
	       RAD(-90.0) /* rotator_kmin */,
	       RAD( 90.0) /* rotator_kmax */,
	       RAD( -10.0) /* shoulder_kmin */,
	       RAD( 90.0) /* shoulder_kmax */,
	       0.2616 /* f_knee_kmin */,
	       0.2316 /* h_knee_kmin */,
	       RAD( 122.0) /* knee_kmax */,
	       RAD(5) /* tail_tilt_min */,
	       RAD(60) /* tail_tilt_max */,
	       RAD(-45) /* tail_pan_min */,
	       RAD(45) /* tail_pan_max */,
	       RAD(-75.0) /* head_tilt_min */,
	       RAD( 0.0) /* head_tilt_max */,
	       RAD(-88.0) /* head_pan_min */,
	       RAD( 88.0) /* head_pan_max */,
	       0.0 /* head_roll_min */,
	       0.0 /* head_roll_max */,
	       RAD(-15.0) /* head_tilt2_min */,
	       RAD(45.0) /* head_tilt2_max */,
	       RAD(-55) /* mouth_min */,
	       RAD(-3) /* mouth_max */);

//====================================================================//

struct RobotConstants2{
  vector3d f_body_to_shoulder;
  vector3d f_leg_shoulder_to_knee;
  vector3d f_leg_knee_to_ball;
  vector3d h_body_to_shoulder;
  vector3d h_leg_shoulder_to_knee;
  vector3d h_leg_knee_to_ball;

  vector3d body_to_neck;
  vector3d neck_to_camera;
  vector3d neck_to_head;
  vector3d head_to_camera;

  vector3d f_upper;
  vector3d f_lower;
  vector3d h_upper;
  vector3d h_lower;

  double rotator_min;
  double rotator_max;
  double shoulder_min;
  double shoulder_max;
  double knee_min;
  double knee_max;
  
  double rotator_kmin;
  double rotator_kmax;
  double shoulder_kmin;
  double shoulder_kmax;
  double f_knee_kmin;
  double h_knee_kmin;
  double knee_kmax;

  double tail_tilt_min;
  double tail_tilt_max;
  double tail_pan_min;
  double tail_pan_max;
  
  double head_tilt_min;
  double head_tilt_max;
  double head_pan_min;
  double head_pan_max;
  double head_roll_min;
  double head_roll_max;
  double head_tilt2_min;
  double head_tilt2_max;
  double mouth_min;
  double mouth_max;
};


static const RobotConstants2 ERS210Constants = {
  f_body_to_shoulder:     vector3d( 59.50, 59.20,   0.00),
  f_leg_shoulder_to_knee: vector3d( 12.80,  0.50, -64.00),
  f_leg_knee_to_ball:     vector3d(-18.00,  0.00, -67.23),
  h_body_to_shoulder:     vector3d(-59.50, 59.20,   0.00),
  h_leg_shoulder_to_knee: vector3d(-12.80,  0.50, -64.00),
  h_leg_knee_to_ball:     vector3d( 18.00,  0.00, -74.87),

  body_to_neck:   vector3d( 75.00,  0.00,  50.00),
  neck_to_camera: vector3d( 65.00,  0.00,  48.00),
  neck_to_head:   vector3d(  0.00,  0.00,   0.00),
  head_to_camera: vector3d(  0.00,  0.00,   0.00),

  // FIXME, next four are duplicates, should be removed
  f_upper: vector3d( 12.80,  0.50, -64.00),
  f_lower: vector3d(-18.00,  0.00, -67.23),
  h_upper: vector3d(-12.80,  0.50, -64.00),
  h_lower: vector3d( 18.00,  0.00, -74.87),

  rotator_min:   RAD(-117.0),
  rotator_max:	 RAD( 117.0),
  shoulder_min:	 RAD( -11.0),
  shoulder_max:	 RAD(  97.0),
  knee_min:	 RAD( -27.0),
  knee_max:	 RAD( 147.0),

  rotator_kmin:  RAD( -90.0),
  rotator_kmax:	 RAD(  90.0),
  shoulder_kmin: RAD( -11.0),
  shoulder_kmax: RAD(  90.0),
  f_knee_kmin:	 0.2616,
  h_knee_kmin:	 0.2316,
  knee_kmax:	 RAD( 147.0),

  tail_tilt_min: RAD( -22.0),
  tail_tilt_max: RAD(  22.0),
  tail_pan_min:	 RAD( -22.0),
  tail_pan_max:	 RAD(  22.0),

  head_tilt_min:  RAD( -88.5),
  head_tilt_max:  RAD(  43.0),
  head_pan_min:	  RAD( -89.6),
  head_pan_max:	  RAD(  89.6),
  head_roll_min:  RAD( -29.0),
  head_roll_max:  RAD(  29.0),
  head_tilt2_min: 0.0,
  head_tilt2_max: 0.0,
  mouth_min:	  0.0,
  mouth_max:	  0.0,
};

static const RobotConstants2 ERS7Constants = {
  f_body_to_shoulder:     vector3d( 65.00, 62.50,   0.00),
  f_leg_shoulder_to_knee: vector3d(  9.00,  4.70, -69.50),
  f_leg_knee_to_ball:     vector3d(  0.00,  0.00, -76.50),
  h_body_to_shoulder:     vector3d(-65.00, 62.50,   0.00),
  h_leg_shoulder_to_knee: vector3d( -9.00,  4.70, -69.50),
  h_leg_knee_to_ball:     vector3d(  0.00,  0.00, -76.50),

  body_to_neck:   vector3d( 67.50,  0.00,  19.50),
  neck_to_camera: vector3d(137.61,  0.00,  41.95),
  neck_to_head:   vector3d( 56.55,  0.00,  56.55),
  head_to_camera: vector3d( 81.06,  0.00, -14.60),

  // FIXME, next four are duplicates, should be removed
  f_upper: vector3d(  9.00,  4.70, -69.50),
  f_lower: vector3d(  0.00,  0.00, -76.50),
  h_upper: vector3d( -9.00,  4.70, -69.50),
  h_lower: vector3d(  0.00,  0.00, -76.50),

  rotator_min:   RAD(-115.0),
  rotator_max:   RAD( 130.0),
  shoulder_min:  RAD( -10.0),
  shoulder_max:  RAD(  88.0),
  knee_min:      RAD( -25.0),
  knee_max:      RAD( 122.0),
  
  rotator_kmin:  RAD( -90.0),
  rotator_kmax:  RAD(  90.0),
  shoulder_kmin: RAD( -10.0),
  shoulder_kmax: RAD(  90.0),
  f_knee_kmin:   0.2616,
  h_knee_kmin:   0.2316,
  knee_kmax:     RAD( 122.0),

  tail_tilt_min: RAD(   5.0),
  tail_tilt_max: RAD(  60.0),
  tail_pan_min:  RAD( -45.0),
  tail_pan_max:  RAD(  45.0),

  head_tilt_min:  RAD( -75.0),
  head_tilt_max:  RAD(   0.0),
  head_pan_min:	  RAD( -88.0),
  head_pan_max:	  RAD(  88.0),
  head_roll_min:  0.0,
  head_roll_max:  0.0,
  head_tilt2_min: RAD( -15.0),
  head_tilt2_max: RAD(  45.0),
  mouth_min:	  RAD( -55.0),
  mouth_max:	  RAD(  -3.0),
};

#endif
