/*
 *_________________________________________________________________________*
 *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
 *      DESCRIPTION: SEE READ-ME                                           *
 *      FILE NAME: body23joint.cpp			                                       *
 *      AUTHORS: See Author List                                           * 
 *      GRANTS: See Grants List                                            *
 *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
 *      LICENSE: Please see License Agreement                              *
 *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
 *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
 *                     Computational Dynamics Lab                          *
 *                     Rensselaer Polytechnic Institute                    *
 *                     110 8th St. Troy NY 12180                           * 
 *      CONTACT:        anderk5@rpi.edu                                    *
 *_________________________________________________________________________*/
 

#include "body23joint.h"
#include "point.h"
#include "matrixfun.h"
#include "body.h"
#include "fastmatrixops.h"
#include "norm.h"
#include "eulerparameters.h"
#include "matrices.h"
#include <iomanip>


Body23Joint::Body23Joint(){
  DimQandU(4,2);
}
Body23Joint::~Body23Joint(){
}

JointType Body23Joint::GetType(){
  return BODY23JOINT;
}

bool Body23Joint::ReadInJointData(std::istream& in){
  return true;
}

void Body23Joint::WriteOutJointData(std::ostream& out){
}

Matrix Body23Joint::GetForward_sP(){
	Vect3 temp = -(point2->position);  // This is vector from joint to CM
	Matrix sP(6,2);
	sP.Zeros();
	
	sP(2,1) =1.0;
	sP(3,2) =1.0;	
	sP(5,2) = temp(1);
	sP(6,1) = -temp(1);	
	return sP;    
}

void Body23Joint::UpdateForward_sP( Matrix& sP){
    // sP is constant, do nothing.
  // linear part is not constant
}

Matrix Body23Joint::GetBackward_sP(){
	cout<<" -----------"<<endl;
	cout<<"Am I here "<<endl;
	cout<<" -----------"<<endl;	  
	Vect3 temp = (point2->position);  // This is vector from CM to joint 
	Matrix sP(6,2);
	sP.Zeros();
	sP(2,1) =1.0;
	sP(3,2) =1.0;
	
	sP(5,2) = temp(1);
	sP(6,1) = -temp(1);
	
	return sP;    
}

void Body23Joint::UpdateBackward_sP( Matrix& sP){  
  // sP is constant, do nothing.
}

void Body23Joint::ComputeLocalTransform(){
	Mat3x3 ko_C_k;
	// Obtain the transformation matrix from euler parameters
	EP_Transformation(q, ko_C_k);
	FastMult(pk_C_ko,ko_C_k,pk_C_k);
	}  


void Body23Joint::ForwardKinematics(){
  Vect3 result1,result2,result3,result4,result5;
  Vect3 pk_w_k;
  
  
  //cout<<"Check in spherical "<<q<<" "<<qdot<<endl;
  EP_Normalize(q);
  

  // orientations
  ComputeForwardTransforms();    
  
  
  //----------------------------------//
  // COMPUTE POSITION VECTOR R12 aka GAMMA    
  
  FastNegMult(pk_C_k,point2->position,result1); // parents basis      
  FastAdd(result1,point1->position,r12);      
  
  // compute position vector r21 
  FastNegMult(k_C_pk,r12,r21); 

  
  //----------------------------------//
  // COMPUTE GLOBAL LOCATION  
  FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1);    
  FastAdd(result1,body1->r,result1);  
  FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2);  
  FastAdd(result1,result2,body2->r);  
    
  ColMatrix temp_u(3);
  qdot_to_u(q, temp_u, qdot);  
  
  temp_u(1)=0.0; 
  u(1) = temp_u(2);
  u(2) = temp_u(3);
  

  //-----------------------------------  
  // angular velocities  
  
  FastAssign(temp_u,pk_w_k);
  FastTMult(pk_C_k,body1->omega_k,result1);        
  FastAdd(result1,pk_w_k,body2->omega_k);
  FastMult(body2->n_C_k,body2->omega_k,body2->omega);  // June 1 checked with Lammps       
  //-----------------------------------  
  
  // compute velocities  
  FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);  
  FastAdd(body1->v_k,result1,result2);
  FastTMult(pk_C_k,result2,result1); // In body basis  
  
  FastCross((body2->GetPoint(1))->position,body2->omega_k,result2);                   
  FastAdd(result1,result2,body2->v_k);    // In body basis   
  FastMult(body2->n_C_k,body2->v_k,body2->v); 
  
  //------------------------------------------
  //Compute the KE   
  Matrix tempke;
  tempke = T(body2->v_k)*(body2->v_k);    
  double ke = 0.0;
  ke = body2->mass*tempke(1,1);    
  FastMult(body2->inertia,body2->omega_k,result1);
  tempke= T(body2->omega_k)*result1;  
  ke = 0.5*ke + 0.5*tempke(1,1);
  body2->KE=ke;
  
  //-----------------------------------  
  // compute state explicit angular acceleration  // Has to be in body basis    
  FastTMult(pk_C_k,body1->alpha_t,result2);  
  FastCross(body2->omega_k,pk_w_k,result1);
  FastAdd(result1,result2,body2->alpha_t); 
   
  //-----------------------------------  
  // compute state explicit acceleration   
  // NEED TO DO THIS COMPLETELY IN BODY BASIS 
  
  FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
  FastCross(body1->omega_k,result1,result2); 
  FastTMult(pk_C_k,result2,result1); 
  
  //FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3);  
  FastCross((body2->GetPoint(1))->position,body2->omega_k,result3);  
  FastCross(body2->omega_k,result3,result2);    
  FastAdd(result1,result2,result3); //wxwxr in body basis  
  
  FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4); 
    
  FastTMult(pk_C_k,result4,result5);  
  FastAssign(result5,result4);
  
  FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2);   
  FastAdd(result2,result4,result1); //alphaxr in body basis  
  
  FastTMult(pk_C_k,body1->a_t,result2);
  FastTripleSum(result3,result1,result2,body2->a_t);     // in body basis  
  
  
  //-----------------------------------    
}

// NOTE: NOT USING BACKWARDKINEMATICS AT PRESENT
void Body23Joint::BackwardKinematics(){
  cout<<"what about here "<<endl;
  Vect3 result1,result2,result3,result4,result5;
  Vect3 k_w_pk;

  // orientations
  ComputeBackwardTransforms();
  

  // compute position vector r21
  //r21 = point2->position - k_C_pk * point1->position;
  FastMult(k_C_pk,point1->position,result1);
  FastSubt(point2->position,result1,r21);
  

  // compute position vector r21
  FastNegMult(pk_C_k,r21,r12);

  // compute global location
  // body1->r = body2->r + body2->n_C_k * r21;
  FastMult(body2->n_C_k,r21,result1);
  FastAdd(body2->r,result1,body1->r);

  // compute qdot (for revolute joint qdot = u)
  // qdot = u
  ColMatrix us(3);
  /*us(1)=0;
  us(2)=u(1);
  us(3)=u(2);*/
  EP_Derivatives(q,u,qdot);

  // angular velocities
 
  FastMult(body2->n_C_k,u,result2);
  FastAdd(body2->omega,result2,body1->omega);
  FastAssign(u,k_w_pk);
  FastMult(pk_C_k,body2->omega_k,result1);
  FastSubt(result1,k_w_pk,body1->omega_k);
  cout<<"The program was here"<<endl;

  // compute velocities
  FastCross(body2->omega_k,r21,result1);
  FastCross(point1->position,k_w_pk,result2);
  FastAdd(body2->v_k,result1,result3);
  FastMult(pk_C_k,result3,result4);
  FastAdd(result2,result4,body1->v_k);
  FastMult(body1->n_C_k,body1->v_k,body1->v);

  // compute state explicit angular acceleration
  FastCross(body1->omega_k,k_w_pk,result1);
  FastMult(pk_C_k,body2->alpha_t,result2);
  FastAdd(result1,result2,body1->alpha_t);
  
  // compute state explicit acceleration
  FastCross(body2->alpha_t,point2->position,result1);
  FastCross(body2->omega_k,point2->position,result2);
  FastCross(body2->omega_k,result2,result3);
  FastTripleSum(body2->a_t,result1,result3,result4);
  FastMult(pk_C_k,result4,result5);

  FastCross(point1->position,body1->alpha_t,result1);
  FastCross(point1->position,body1->omega_k,result2);
  FastCross(body1->omega_k,result2,result3);

  FastTripleSum(result5,result1,result3,body1->a_t);
  
}