/***************************************************************
 * PrJointIntegrator.cpp
 *
 * This is an implementation of the joint integrator.
 *
 ****************************************************************/
/*
 * modification history
 *----------------------
 *
 * 02/17/98: K.C. Chang: created.
 */

#include "PrJointIntegrator.h"
#include "PrRobot.h"
#include "PrJointState.h"

PrJointIntegrator::PrJointIntegrator( int noj )
{
    noj_ = noj;
    for(int i=0;i<5;i++)
      k_[i] = new PrJointState(noj_);
}

PrJointIntegrator::~PrJointIntegrator()
{
    for(int i=0;i<5;i++)
      delete k_[i];
}

// Euler integration
void PrJointIntegrator::integrateEuler(PrRobot *robot, Float dt)
{
  robot->fwdDynamics();
    
  Float *ddqo = robot->jointState()->ddq(); //robot->jointAcceleration();
  Float *qo = robot->jointState()->q();
  Float *dqo = k_[0]->dq();
    
  for(int i=0;i<robot->jointState()->noj();i++)
    {
      dqo[i]=robot->jointState()->dq()[i];
      robot->jointState()->dq()[i] = dqo[i]+ddqo[i]*dt;  
      robot->jointState()->q()[i] = qo[i]+dqo[i]*dt+0.5*ddqo[i]*dt*dt;  
    }
}

/*
   Fourth-Order Runge-Kutta Methods 
   (for "Numerical Methods for Engineers" by Chapra and Canale, p603)
   
   f(x,y) = d(y)/dx;
   
   yi+1 = yi + (1/6)*[k1 + 2*k2 + 2*k3 + k4]*h;
   
   k1 = f(xi,       yi);
   k2 = f(xi+0.5*h, yi+0.5*h);
   k3 = f(xi+0.5*h, yi+0.5*h*k2);
   k4 = f(xi+    h, yi+    h*k3);
   */

void PrJointIntegrator::integrateRK4(PrRobot *robot, Float dt)
{
  int i;

  if (noj_ != robot->jointState()->noj())
    {
      noj_ = robot->jointState()->noj();
      for(i=0;i<5;i++)
	{
	  delete k_[i];
	  k_[i] = new PrJointState(noj_);
	}
    }

  *k_[0] = *robot->jointState();
  robot->fwdDynamics();
  *k_[1] = *robot->jointState();
  for(i=0;i<robot->jointState()->noj();i++)
    {
      robot->jointState()->dq()[i] = k_[0]->dq()[i] + 0.5*dt*k_[1]->ddq()[i]; 
      robot->jointState()->q()[i] = k_[0]->q()[i] + 0.5*dt*k_[1]->dq()[i]; 
    }
  robot->fwdDynamics();
  *k_[2] = *robot->jointState();
    
  for(i=0;i<robot->jointState()->noj();i++)
    {
      robot->jointState()->dq()[i] = k_[0]->dq()[i] + 0.5*dt*k_[2]->ddq()[i]; 
      robot->jointState()->q()[i] = k_[0]->q()[i] + 0.5*dt*k_[2]->dq()[i]; 
    }
  robot->fwdDynamics();
  *k_[3] = *robot->jointState();
    
    
  for(i=0;i<robot->jointState()->noj();i++)
    {
      robot->jointState()->dq()[i] = k_[0]->dq()[i] + dt*k_[3]->ddq()[i]; 
      robot->jointState()->q()[i] = k_[0]->q()[i] + dt*k_[3]->dq()[i]; 
    }
  robot->fwdDynamics();
  *k_[4] = *robot->jointState();
    
  for(i=0;i<robot->jointState()->noj();i++)
    {
      robot->jointState()->dq()[i] = k_[0]->dq()[i] + (k_[1]->ddq()[i] 
						       + 2*k_[2]->ddq()[i]
						       + 2*k_[3]->ddq()[i] 
						       + k_[4]->ddq()[i])*dt/6;
      robot->jointState()->q()[i] = k_[0]->q()[i] + (k_[1]->dq()[i] 
						     + 2*k_[2]->dq()[i]
						     + 2*k_[3]->dq()[i] 
						     + k_[4]->dq()[i])*dt/6;
    }
}


