/***************************************************************
* PrRobot.cpp
*
* This is an implementation of the robot interface.
*
****************************************************************/

/*
* modification history
*----------------------
*
* 11/11/97: K.C. Chang: created.
*/

#include "PrRobot.h"
#include "PrJointState.h"
#include "PrLink.h"

#include <stdio.h>
#include <stdlib.h>

PrRobot::PrRobot(int noj,char *paramFilename,int dynamicsType,int fileType)
: noj_(noj), dynamicsType_(dynamicsType)
{
   ee_ = NULL;
   fs_ = NULL;

   if (fileType == PR_ROBOT_FILETYPE_GLOBAL)
      flag_ = InitializeGlobal(paramFilename);
   else
      flag_ = InitializeDH(paramFilename);
}

PrRobot::~PrRobot()
{
   delete base_;
   delete object_;
   for(int i=0;i<noj_;i++)
      delete links_[i];
   if (ee_ != NULL || fs_ != NULL)
   {
      delete links_[noj_];
      delete links_[noj_+1];
   }
   delete[] links_;
   delete[] tau_;
   delete jointState_;
}

Float *PrRobot::jointAcceleration()
{
   return jointState_->ddq();
}

void PrRobot::display() const
{
   for(int i=0;i<noj_;i++)
   {
      links_[i]->display();
   }
}

int PrRobot::InitializeDH(char *paramFilename)
{
   PrRobotDynamicsParamDH *param = new PrRobotDynamicsParamDH[noj_+2];
   int fs_ee;
   PrVector3 gravity;
   if (ReadDynamicsParamDH(paramFilename,gravity,param,&fs_ee) != PR_ROBOT_OK)
      return PR_ROBOT_ERROR;

   links_ = new PrLink *[noj_+2];

   int i;
   for(i=0;i<noj_+fs_ee;i++)
   {
      links_[i] = new PrLink(param[i].type);
      links_[i]->id(param[i].id);

      if (param[i].id == PR_ROBOT_FS_ID)
         fs_ = links_[i];
      else if (param[i].id == PR_ROBOT_EE_ID)
         ee_ = links_[i];

      links_[i]->homePosition(param[i].DH[0],param[i].DH[1],
         param[i].DH[2],param[i].DH[3]);
      links_[i]->mass(param[i].mass);
      links_[i]->centerOfMass(param[i].com);
      links_[i]->inertia(param[i].Ixx,param[i].Iyy,
         param[i].Izz + param[i].Imotor);
      if (i<noj_)
      {
         links_[i]->friction(param[i].viscousFriction,
            param[i].dynamicCoulombFriction,
            param[i].staticCoulombFriction);
         links_[i]->qMin(param[i].qMin);
         links_[i]->qMax(param[i].qMax);
      }
      else
      {
         links_[i]->offset(param[i].offset);
         //	      links_[i]->offset().display("offset");
         //	      printf("\n");
      }
   }
   delete[] param;
   base_ = new PrLink();
   base_->child(links_[0]);
   base_->A()[0] = -gravity;
   links_[0]->parent(base_);
   for(i=1;i<noj_;i++)
   {
      links_[i-1]->child(links_[i]);
      links_[i]->parent(links_[i-1]);
   }
   object_ = new PrLink();
   object_->parent(links_[noj_-1]);
   links_[noj_-1]->child(object_);

   links_[noj_-1]->mass(links_[noj_-1]->mass()+ee_->mass());
   links_[noj_-1]->mass(links_[noj_-1]->mass()+fs_->mass());

   PrMatrix6 XIXt;

   ee_->state(0.0);
   ee_->initInertiaMatrix();
   ee_->similarityXform6(ee_->I(),XIXt);
   links_[noj_-1]->inertia()[0] += XIXt[3][0][0];
   links_[noj_-1]->inertia()[1] += XIXt[3][1][1];
   links_[noj_-1]->inertia()[2] += XIXt[3][2][2];

   fs_->state(0.0);
   fs_->initInertiaMatrix();
   fs_->similarityXform6(fs_->I(),XIXt);
   links_[noj_-1]->inertia()[0] += XIXt[3][0][0];
   links_[noj_-1]->inertia()[1] += XIXt[3][1][1];
   links_[noj_-1]->inertia()[2] += XIXt[3][2][2];

   ee_->parent(links_[noj_-1]);
   fs_->parent(links_[noj_-1]);

   //    ee_->localTransform().translation() += ee_->offset();
   //    fs_->localTransform().translation() += fs_->offset();

   //    ee_->homePosition(ee_->localTransform());
   //    fs_->homePosition(fs_->localTransform());

   //    ee_->localTransform().display("ee");
   //    fs_->localTransform().display("fs");

   tau_ = new Float[noj_];
   for(i=0;i<noj_;i++)
      tau_[i]=0.0;
   jointState_ = new PrJointState(noj_);

   return PR_ROBOT_OK;
}

int PrRobot::ReadDynamicsParamDH(char *filename, PrVector3 &gravity,
                                 PrRobotDynamicsParamDH *param,int *fs_ee)
{
   FILE *fd = fopen(filename,"r"); 
   if (fd == NULL)
   {
      printf("Error: couldn't open file(%s)\n",filename);
      return PR_ROBOT_ERROR;
   }
#ifdef PR_DOUBLE_PRECISION
   // Float == double
   fscanf(fd,"%d",fs_ee);
   fscanf(fd,"%lf %lf %lf",&gravity[0],&gravity[1],&gravity[2]);

   for(int i=0;i<noj_+(*fs_ee);i++)
   {
      fscanf(fd,"%d",&param[i].id);
      fscanf(fd,"%d",&param[i].type);
      fscanf(fd,"%lf %lf %lf %lf",param[i].DH,param[i].DH+1,
         param[i].DH+2,param[i].DH+3);
      fscanf(fd,"%lf",&param[i].mass);
      fscanf(fd,"%lf %lf %lf",&param[i].com[0],&param[i].com[1],
         &param[i].com[2]);
      fscanf(fd,"%lf %lf %lf",&param[i].Ixx,&param[i].Iyy,&param[i].Izz);
      if (i<noj_)
      {
         fscanf(fd,"%lf",&param[i].Imotor);
         fscanf(fd,"%lf",&param[i].viscousFriction);
         fscanf(fd,"%lf",&param[i].dynamicCoulombFriction);
         fscanf(fd,"%lf",&param[i].staticCoulombFriction);
         fscanf(fd,"%lf",&param[i].qMin);
         fscanf(fd,"%lf",&param[i].qMax);
      }
      else
      {
         fscanf(fd,"%lf %lf %lf",&param[i].offset[0],&param[i].offset[1],
            &param[i].offset[2]);
      }
      printf("[%d] min = %lf, max = %lf\n",i,param[i].qMin,param[i].qMax);
   }
#else // PR_DOUBLE_PRECISION
   fscanf(fd,"%d",fs_ee);
   fscanf(fd,"%f %f %f",&gravity[0],&gravity[1],&gravity[2]);

   for(int i=0;i<noj_+(*fs_ee);i++)
   {
      printf("joint%d\n",i);
      fscanf(fd,"%d",&param[i].id);
      fscanf(fd,"%d",&param[i].type);
      fscanf(fd,"%f %f %f %f",param[i].DH,param[i].DH+1,
         param[i].DH+2,param[i].DH+3);
      fscanf(fd,"%f",&param[i].mass);
      fscanf(fd,"%f %f %f",&param[i].com[0],&param[i].com[1],
         &param[i].com[2]);
      fscanf(fd,"%f %f %f",&param[i].Ixx,&param[i].Iyy,&param[i].Izz);
      if (i<noj_)
      {
         fscanf(fd,"%f",&param[i].Imotor);
         fscanf(fd,"%f",&param[i].viscousFriction);
         fscanf(fd,"%f",&param[i].dynamicCoulombFriction);
         fscanf(fd,"%f",&param[i].staticCoulombFriction);
         fscanf(fd,"%f",&param[i].qMin);
         fscanf(fd,"%f",&param[i].qMax);
      }
      else
      {
         fscanf(fd,"%lf %lf %lf",&param[i].offset[0],&param[i].offset[1],
            &param[i].offset[2]);
      }
      printf("[%d] min = %f, max = %f\n",i,param[i].qMin,param[i].qMax);
   }
#endif // PR_DOUBLE_PRECISION

   fclose(fd);
   return PR_ROBOT_OK;
}

int PrRobot::ReadDynamicsParamGlobal(char *filename, PrVector3 &gravity, PrTransform &globalT, PrRobotDynamicsParamGlobal *param)
{
   FILE *fd = fopen(filename,"r"); 
   if (fd == NULL)
   {
      printf("Error: couldn't open file(%s)\n",filename);
      return PR_ROBOT_ERROR;
   }

#ifdef PR_DOUBLE_PRECISION
   printf("double precesion\n");
   fscanf(fd,"%lf %lf %lf",&gravity[0],&gravity[1],&gravity[2]);

   Float data[4];
   fscanf(fd,"%lf %lf %lf %lf",data,data+1,data+2,data+3);
   globalT.rotation().values(PrVector3(data[0],data[1],data[2]),data[3]);

   fscanf(fd,"%lf %lf %lf",data,data+1,data+2);
   globalT.translation().values(data[0],data[1],data[2]);

   for(int i=0;i<noj_;i++)
   {
      fscanf(fd,"%d",&param[i].id);
      fscanf(fd,"%d",&param[i].type);
      fscanf(fd,"%d",&param[i].axis);

      fscanf(fd,"%lf %lf %lf %lf",data,data+1,data+2,data+3);
      param[i].homeT.rotation().values(PrVector3(data[0],data[1],data[2]),data[3]);

      fscanf(fd,"%lf %lf %lf",data,data+1,data+2);
      param[i].homeT.translation().values(data[0],data[1],data[2]);

      fscanf(fd,"%lf %lf %lf",data,data+1,data+2);
      param[i].com.values(data[0],data[1],data[2]);

      fscanf(fd,"%lf",&param[i].mass);
      fscanf(fd,"%lf %lf %lf",data,data+1,data+2);
      param[i].inertia.values(data[0],data[1],data[2]);

      fscanf(fd,"%lf",&param[i].qMin);
      fscanf(fd,"%lf",&param[i].qMax);
      printf("%d: %f<q<%f\n",i,param[i].qMin,param[i].qMax);
   }
#else // PR_DOUBLE_PRECISION
   fscanf(fd,"%f %f %f",&gravity[0],&gravity[1],&gravity[2]);

   Float data[4];
   fscanf(fd,"%f %f %f %f",data,data+1,data+2,data+3);
   globalT.rotation().values(PrVector3(data[0],data[1],data[2]),data[3]);

   fscanf(fd,"%f %f %f",data,data+1,data+2);
   globalT.translation().values(data[0],data[1],data[2]);

   for(int i=0;i<noj_;i++)
   {
      fscanf(fd,"%d",&param[i].id);
      fscanf(fd,"%d",&param[i].type);
      fscanf(fd,"%d",&param[i].axis);

      fscanf(fd,"%f %f %f %f",data,data+1,data+2,data+3);
      param[i].homeT.rotation().values(PrVector3(data[0],data[1],data[2]),data[3]);

      fscanf(fd,"%f %f %f",data,data+1,data+2);
      param[i].homeT.translation().values(data[0],data[1],data[2]);

      fscanf(fd,"%f %f %f",data,data+1,data+2);
      param[i].com.values(data[0],data[1],data[2]);

      fscanf(fd,"%f",&param[i].mass);
      fscanf(fd,"%f %f %f",data,data+1,data+2);
      param[i].inertia.values(data[0],data[1],data[2]);

      fscanf(fd,"%f",&param[i].qMin);
      fscanf(fd,"%f",&param[i].qMax);
      printf("%d: %f<q<%f\n",i,param[i].qMin,param[i].qMax);
   }
#endif // PR_DOUBLE_PRECISION

   fclose(fd);
   return PR_ROBOT_OK;
}

int PrRobot::InitializeGlobal(char *paramFilename)
{
   PrRobotDynamicsParamGlobal *param = new PrRobotDynamicsParamGlobal[noj_];
   PrVector3 gravity;
   PrTransform globalT;
   if (ReadDynamicsParamGlobal(paramFilename,gravity,globalT,param) != PR_ROBOT_OK)
      return PR_ROBOT_ERROR;

   base_ = new PrLink();
   base_->homePosition(globalT);
   base_->state(0.0);
   base_->A()[0] = (~base_->globalTransform().rotation())*(-gravity);

   links_ = new PrLink *[noj_];

   int i;
   for(i=0;i<noj_;i++)
   {
      links_[i] = new PrLink(param[i].type,param[i].axis);
      links_[i]->id(param[i].id);

      links_[i]->homePosition(param[i].homeT);
      links_[i]->centerOfMass(param[i].com);
      links_[i]->mass(param[i].mass);
      links_[i]->inertia(param[i].inertia[0],param[i].inertia[1],param[i].inertia[2]);
      links_[i]->qMin(param[i].qMin);
      links_[i]->qMax(param[i].qMax);
   }
   delete[] param;

   base_->child(links_[0]);
   links_[0]->parent(base_);

   for(i=1;i<noj_;i++)
   {
      links_[i-1]->child(links_[i]);
      links_[i]->parent(links_[i-1]);
   }

   object_ = new PrLink();
   object_->parent(links_[noj_-1]);
   object_->state(0.0);

   tau_ = new Float[noj_];
   for(i=0;i<noj_;i++)
      tau_[i]=0.0;
   jointState_ = new PrJointState(noj_+1); //gripper

   updateTransform();

   return PR_ROBOT_OK;
}

void PrRobot::updateTransform()
{
   for(int i=0;i<noj_;i++)
      links_[i]->state(jointState_->q()[i]);
   object_->state(0.0);
}

void PrRobot::invDynamics()
{
   //    if (1 || dynamicsType_ == PR_ROBOT_INV_DYNAMICS)
   {
      int i;
      for(i=0;i<noj_;i++)
      {
         links_[i]->state(jointState_->q()[i],
            jointState_->dq()[i],
            PR_LINK_INV_DYNAMICS);
      }
      object_->state(0.0);
      for(i=0;i<noj_;i++)
      {
         links_[i]->invDynamics(jointState_->ddq()[i]);
      }

      for(i=noj_-1;i>=0;i--)
      {
         tau_[i]=links_[i]->jointForce();
      }
   }
}	  

void PrRobot::fwdDynamics()
{
   //if (dynamicsType_ == PR_ROBOT_FWD_DYNAMICS)
   {
      int i;
      for(i=0;i<noj_;i++)
      {
         links_[i]->state(jointState_->q()[i],
            jointState_->dq()[i],
            PR_LINK_FWD_DYNAMICS);
      }
      object_->state(0.0);
      for(i=noj_-1;i>=0;i--)
      {
         links_[i]->fwdDynamics(tau_[i]);
      }

      for(i=0;i<noj_;i++)
      {
         jointState_->ddq()[i]=links_[i]->jointAcceleration();
      }
   }
}

// L_0 = large
// L_e = eXn L_n eXn^T
// ^0L_e = R L_e R^T
void PrRobot::localOSIM(PrMatrix6 &Lambda)
{
   // L_0 = large
   for(int j=0;j<3;j++)
   {
      base_->Lambda().elementAt(j,j) = PR_ROBOT_MASS_LARGE;
      base_->Lambda().elementAt(j+3,j+3) = PR_ROBOT_INERTIA_LARGE;
   }

   for(int i=0;i<noj_;i++)
   {
      links_[i]->articulatedOSIM();
   }
   Lambda = links_[noj_-1]->Lambda();
}

// L_0 = large
// L_e = eXn L_n eXn^T
// ^0L_e = R L_e R^T 
void PrRobot::globalOSIM(PrMatrix6 &Lambda)
{
   // L_0 = large
   for(int j=0;j<3;j++)
   {
      base_->Lambda().elementAt(j,j) = PR_ROBOT_MASS_LARGE;
      base_->Lambda().elementAt(j+3,j+3) = PR_ROBOT_INERTIA_LARGE;
   }

   for(int i=0;i<noj_;i++)
   {
      links_[i]->articulatedOSIM();
   }

   // L_e = eXn L_n eXn^T
   PrMatrix6 &localLambda = tmpM61_;
   object_->similarityXform6inv(object_->parent()->Lambda(),localLambda);

   // ^0L_e = R L_e R^T 
   SimilarityRform6(localLambda,Lambda);
}

// ^0L_e = [R 0; 0 R][L0 L1; L2 L3][Rt 0; 0 Rt]
// = [R L0  R L1; R L2  R L3][Rt 0; 0 Rt]
// = [R L0 Rt  R L1 Rt; R L2 Rt  R L3 Rt]
// = [X0 X1; X2 X3]
// X0 = R L0 Rt
// X1 = R L1 Rt
// X2 = X1^T  since I2 = I1^T
// X3 = R L3 Rt
void PrRobot::SimilarityRform6(const PrMatrix6 &localLambda,PrMatrix6 &globalLambda)
{
   PrMatrix3 &R = tmpM31_;
   PrMatrix3 &Rt = tmpM32_;
   PrMatrix3 &RL = tmpM33_;

   object_->globalTransform().rotation().matrix(R);
   R.transpose(Rt);

   R.multiply(localLambda[0],RL);
   RL.multiply(Rt,globalLambda[0]);
   R.multiply(localLambda[1],RL);
   RL.multiply(Rt,globalLambda[1]);
   globalLambda[1].transpose(globalLambda[2]);
   R.multiply(localLambda[3],RL);
   RL.multiply(Rt,globalLambda[3]);
}


