/***************************************************************
 * 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]);
}


