/*!
 * @file fk.cpp
 * @brief forward kinema for refhard1
 *
 * @date 2003.11.3
 *
 * Copyright 2003 Takashi Suehiro
 */
// #include <stdio.h>
// #include <iostream.h>
#include "fk.h"
/*!
 * @class FK
 * @brief class for forward kinema for refhard1
 *
 */

//! 
FK::FK() {
  int i,j,k;
  for(i=0;i<3;i++) {
    for(j=0;j<3;j++) {			//mod.start 2010.1.15 by FSI
      a_mat[6][i][j] = 0.0;		//a_mat[7][i][j] = 0.0;
    }
    a_mat[6][i][i] = 1.0;		//a_mat[7][i][i] = 1.0;
    v_tool[i] = 0.0;			
  }
};
//! 
FK::~FK() {};
//! 
int FK::set_tool(FRAME * frm) {
  m_ass(frm -> mtr, a_mat[6]);		//m_ass(frm -> mtr, a_mat[7]);
  v_ass(frm -> vtr, v_tool);
  return 0;
}
//! 
int FK::calc_fk(REAL *th) {
  int i,j,k;
  mc(th[0],a_mat[0]);					//mc(th[5],a_mat[0]);
  mb(th[1],a_mat[1]);					//	:
  mb(th[2],a_mat[2]);
  mc(th[3],a_mat[3]);
  mb(th[4],a_mat[4]);					//	:
  mc(th[5],a_mat[5]);					//mc(th[6],a_mat[6]);
  m_ass(a_mat[0],link_mat[0]);
  for(i = 0; i<6 ;i++)					//for(i = 0; i<7 ;i++)
    m_mul(link_mat[i],a_mat[i+1],link_mat[i+1]);
  m_vec(2,link_mat[0],joint_vec[0]);			//m_vec(2,link_mat[0],joint_vec[0]);
  m_vec(1,link_mat[1],joint_vec[1]);			//m_vec(1,link_mat[1],joint_vec[1]);
  m_vec(2,link_mat[1],dir_vtr);	//to make "ua"		//m_vec(2,link_mat[2],joint_vec[2]);
  m_vec(1,link_mat[2],joint_vec[2]);			//m_vec(1,link_mat[3],joint_vec[3]);
  m_vec(2,link_mat[3],joint_vec[3]);			//m_vec(2,link_mat[4],joint_vec[4]);
  m_vec(1,link_mat[4],joint_vec[4]);			//m_vec(1,link_mat[5],joint_vec[5]);
  m_vec(2,link_mat[5],joint_vec[5]);			//m_vec(2,link_mat[6],joint_vec[6]);

  sv_mul(LS,dir_vtr,ua);				//  sv_mul(LS,joint_vec[2],ua);
  sv_mul(LE,joint_vec[3],fa);				//sv_mul(LE,joint_vec[4],fa);
  sv_mul(LW,joint_vec[5],fr);				//sv_mul(LW,joint_vec[6],fr);
  mv_mul(link_mat[5],v_tool,ef);			//mv_mul(link_mat[6],v_tool,ef);

  link_pos[0][0] = 0.0;
  link_pos[0][1] = 0.0;
  link_pos[0][2] = LB;
  v_ass(link_pos[0],link_pos[1]);
  v_add(link_pos[1],ua,link_pos[2]);
  v_add(link_pos[2],fa,link_pos[3]);			//v_ass(link_pos[2],link_pos[3]);
  v_ass(link_pos[3],link_pos[4]);			//v_add(link_pos[3],fa,link_pos[4]);
  v_add(link_pos[4],fr,link_pos[5]);			//v_ass(link_pos[4],link_pos[5]);
  v_add(link_pos[5],ef,link_pos[6]);			//v_add(link_pos[5],fr,link_pos[6]);
							//v_add(link_pos[6],ef,link_pos[7]);

  v_ass(ef,joint_arm[5]);				//v_ass(ef,joint_arm[6]);
  v_add(joint_arm[5],fr,joint_arm[4]);			//v_add(joint_arm[6],fr,joint_arm[5]);
  v_ass(joint_arm[4],joint_arm[3]);			//v_ass(joint_arm[5],joint_arm[4]);
  v_add(joint_arm[3],fa,joint_arm[2]);			//v_add(joint_arm[4],fa,joint_arm[3]);
  v_add(joint_arm[2],ua,joint_arm[1]);			//v_ass(joint_arm[3],joint_arm[2]);
  v_ass(joint_arm[1],joint_arm[0]);			//v_add(joint_arm[2],ua,joint_arm[1]);
							//v_ass(joint_arm[1],joint_arm[0]);
  return 0;
}
//! 
int FK::link_frame(int idx, FRAME * frm) {
  m_ass(link_mat[idx], frm -> mtr);
  v_ass(link_pos[idx], frm -> vtr);
  return 0;
}
//! 
int FK::jacob(Jacob jcb) {

  int i,j;

  VECTOR vtmp1;

  for(j=0;j<6;j++) {					//for(j=0;j<7;j++) {
    v_cross(joint_vec[j],joint_arm[j],vtmp1);		//mod.end 2010.1.15 by FSI
    for(i=0;i<3;i++) {
      jcb[i][j] = vtmp1[i];
      jcb[i+3][j] = joint_vec[j][i];
    }
  }
  return 0;
}
