/* cor_link.cc */

#include "cor_link.h"

Barrier *create_rope_constraint( RigidBody *body, ctriple bp, ctriple wp, 
				 double dist ) {
  CExpr e = dist * dist - sqr( BodyPoint( *body, bp ) - wp );
  return new Barrier( e );
}

Barrier *create_rope_constraint( RigidBody *bodA, RigidBody *bodB, ctriple bpa, 
				 ctriple bpb, double dist ) {
  CExpr e = dist * dist - sqr( BodyPoint( *bodA, bpa ) - 
			       BodyPoint( *bodB, bpb ) );
  return new Barrier( e );
}

//==================================================
void Motor::init( RigidBody &bod1, RigidBody &bod2, triple bop1, 
		  triple bop2, double mag = 1, double f = 1, double Phase = 0 ){
  body1 = &bod1;
  body2 = &bod2;
  bp1   = bop1;
  bp2   = bop2;
  magnitude = mag;
  frequency = f;
  phase = Phase;
  
  // Servo model used in Caterpillar robot is a Tower Hobbies TS-55
  //     Torque : 0.30 Nm = 3 x 10^6 dyne-cm (ergs)
  //     Weight : 45.4 g
  // Dimensions : 4.064 x 2.032 x 3.810 cm 
  //      Speed : 0.20 sec/60 degrees => 60rpm
  //      Power : max 1.91 Watts mechanical power output (2pi/60*torque*rpm)
  // 
  //    Density : [2D] 5.5g/cm^2 = 45.4g/(4.064cm x 2.032cm)
}

//==================================================
void Motor::update() {
  // Forces are cumulative in Coriolis!!!
  double now;
  now = bs->current_time;
  //double maxtorque = 3000000.0;  

  // tried attenuating system towards the ends - didn't work.
  //  int midlink = NLINKS/2;
  //  double atten_coeff = 0.1;
  //  double attenuate = 1.0 - (atten_coeff * (midlink - link));
  
  torque = magnitude * sin( now * frequency + phase );
  
  //normalize to max power?
  
  vector j(3, 0.0, 0.0, torque); // X,Y and theta
  exert_force(*body1, j);
  exert_force(*body2, -j);
  
  //  relative damping between links that is porportional to relative omega
  triple rw = body1->omega - body2->omega;
  vector  newd(3, 0.0, 0.0, -0.01 * rw);
  exert_force(*body1, newd);
  exert_force(*body2, -newd);
  
  //  damping porportional to omega of link. Not used.
  //  double damping = links[link].omega * 0.01; // damping coefficient
  //  vector d(3, 0.0, 0.0, damping);
  //  exert_force(*body1, d);
  //  exert_force(*body2, -d);
}

//==================================================
Motor::Motor() { }
Motor::~Motor() { }

//==================================================
void Motor::ignore(Body &b) {
  if (&b == body1 || &b == body2) {
    body1 = body2 = 0;
    active = FALSE;
  }
}

//==================================================
// create angular limits for joints - angle is in radians
void create_angular_constraint(RigidBody2d &b1, RigidBody2d &b2, double angle,
			       BodySystem &bs ) {
  if (angle < 1e-3)
    return; // use a nail instead

  CExpr e1 = (BodyDof(b1,2) - BodyDof(b2, 2)) + angle;  // ccw angle constraint
  Barrier *ccw_limit = new Barrier(e1);                 // insure that e1 >= 0

  CExpr e2 = (BodyDof(b1,2) - BodyDof(b2, 2)) - angle;  // cw angle constraint
  Barrier *cw_limit = new Barrier(-e2);                 // insure that e2 <= 0

  bs.add(*cw_limit);                                    // add to world 
  bs.add(*ccw_limit);
}

void create_attachment( RigidBody &b1, RigidBody &b2, EConstraint *&constr1,
			EConstraint *&constr2, EConstraint *&constr3 ) {
  /* choose three points and constraint them to lie on each other */
  /* choose com as first point */
  CExpr p1 = BodyPoint( b1, b1.com ) - BodyPoint( b2, b2.com );
  
  CExpr p2 = BodyPoint( b1, b1.com + triple( 1, 0, 0 ) ) - 
             BodyPoint( b2, b2.com + triple( 1, 0, 0 ) );
  CExpr p3 = BodyPoint( b1, b1.com + triple( 0, 1, 0 ) ) - 
             BodyPoint( b2, b2.com + triple( 0, 1, 0 ) );
  constr1 = new EConstraint( p1 );
  constr2 = new EConstraint( p2 );
  constr3 = new EConstraint( p3 );
}

void create_axial_link( RigidBody &b1, ctriple s_bp1, ctriple s_bp2, 
			RigidBody &b2, ctriple t_bp1, ctriple t_bp2,
			EConstraint *&constr1, EConstraint *&constr2 ) {
  CExpr p1 = BodyPoint( b1, s_bp1 ) - BodyPoint( b2, t_bp1 );
  CExpr p2 = BodyPoint( b1, s_bp2 ) - BodyPoint( b2, t_bp2 );

  constr1 = new EConstraint( p1 );
  constr2 = new EConstraint( p2 );
}

void create_axial_link( RigidBody &b1, ctriple bp1, ctriple bp2, 
			ctriple wp1, ctriple wp2,
			EConstraint *&constr1, EConstraint *&constr2 ) {
  CExpr p1 = BodyPoint( b1, bp1 ) - wp1;
  CExpr p2 = BodyPoint( b1, bp2 ) - wp2;

  constr1 = new EConstraint( p1 );
  constr2 = new EConstraint( p2 );
}

EConstraint *create_dist_constraint( RigidBody &bodA, ctriple bpA,
				     RigidBody &bodB, ctriple bpB, double dist )
{
  return new EConstraint( sqr( BodyPoint( bodA, bpA ) - BodyPoint( bodB, bpB ) )
			  - ( dist * dist ) );
}

EConstraint *create_dist_constraint( RigidBody& bod, ctriple bp, ctriple wp, 
				     double dist )
{
  return new EConstraint( sqr( BodyPoint( bod, bp ) - wp ) - ( dist * dist ) );
}

Booster::Booster( void ) {
  body = NULL;
  thrust = 0;
}

//Booster::~Booster( void ) {
//}

void Booster::init( RigidBody &Body, double Thrust, ctriple Axis ) {
  body = &Body;
  thrust = Thrust;
  axis = Body.com + normalize( Axis );
}

void Booster::set_axis( ctriple Axis ) {
  axis = body->com + normalize( Axis );
}

void Booster::update( void ) {
  static vector j(6); /* made static to avoid creating new vector every time */
  double now;
  now = bs->current_time;
  
  triple w_axis = ( body->r * axis ) * thrust;
  j(0) = w_axis(0);
  j(1) = w_axis(1);
  j(2) = w_axis(2);
  exert_force( *body, j );
}

void Booster::ignore( Body &B ) {
  if( ( ( Body * )body ) = &B ) body = NULL;
  active = false;
}
