//////////////////////////////////////////////////////////////////////////////
//
//       Title     : exercise7.cpp
//       Project   : OSCAR tutorial
//       Created   : May 30, 1998
//       Author    : Mitch Pryor
//       Platforms : PC
//	 Copyright : Copyright© The University of Texas at Austin, 2002.
//                   All rights reserved.
//                 
//         This software and documentation constitute an unpublished work
//         and contain valuable trade secrets and proprietary information
//         belonging to University.  None of the foregoing material may be
//         copied  or duplicated or disclosed without the express, written
//         permission of University.  UNIVERSITY EXPRESSLY DISCLAIMS ANY
//         AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
//         INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
//         PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
//         THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
//         NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
//         THE SOFTWARE OR DOCUMENTATION.  Under no circumstances shall
//         University be liable for incidental, special, indirect, direct or
//         consequential damages or loss of profits, interruption of business,
//         or related expenses which may arise from use of software or documentation,
//         including but not limited to those resulting from defects in software
//         and/or documentation, or loss or inaccuracy of data of any kind.
//
//       Purpose   : Solution to Exercise 7 of the OSCAR Guide
//
///////////////////////////////////////////////////////////////////////////////
#include <iostream.h>
#include "ForwardKinematics/FKAcceleration.h"

int main( ){
	int i; //counter

	RRFKAcceleration fkAcc("DataFiles/Titan2.dh");
	RRVector         joints(fkAcc.GetDOF());

	const RRXform*      handXform;
	const RRMatrix*			jacobian;
	const RRTensor*     Rot_G;
	const RRTensor*			Trans_G;
	const RRTensor*     Rot_H_link5;
	const RRTensor*			Trans_H_link5;
	const RRTensorP*    Rot_H;
	const RRTensorP*		Trans_H;
	
	for(i=0; i<joints.GetSize(); i++)
		joints[i] = (i*9.6+12.7)*DEGTORAD;
	
	//Determine any and all necessary forward data
	handXform  = fkAcc.GetHandPose(joints);
	jacobian = fkAcc.GetJacobian(joints);
  Rot_G = fkAcc.GetRotationalGfunctions(joints);
  Trans_G = fkAcc.GetTranslationalGfunctions();
  Rot_H_link5 = fkAcc.GetRotationalHfunctions(5,joints);
  Trans_H_link5 = fkAcc.GetTranslationalHfunctions(5);
  Rot_H = fkAcc.GetRotationalHfunctions();
  Trans_H = fkAcc.GetTranslationalHfunctions();

	cout << "The joint variables are: \n" << joints << endl;
	cout << "The hand tranformation matrix: \n" << *handXform << endl;
	cout << "Jacobian: \n" << *jacobian << endl;
  cout << "Rotational G functions: \n" << *Rot_G << endl;
  cout << "Translational G functions: \n" << *Trans_G << endl;
  cout << "Rotational H function for joint 5: \n" << *Rot_H_link5 << endl;
  cout << "Translational H function for joint 5: \n" << *Trans_H_link5 << endl;
	
	/* commented for brevity of output to screen
  for(i=0; i<joints.GetSize(); i++)
    cout << "Rotational H function for joint " << i+1 << "\n" << *Rot_H[i] << endl;
  for(i=0; i<joints.GetSize(); i++)
    cout << "Translational H function for joint " << i+1 << "\n" << *Trans_H[i] << endl;
  */
	return 0;
}//end of main