//////////////////////////////////////////////////////////////////////////////
//
//       Title     : exercise5.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 5 of the OSCAR Guide
//
//////////////////////////////////////////////////////////////////////////////
#include <iostream.h>
#include "ForwardKinematics/FKPosition.h"

int main(void)
{
	//Create the Forward Kinematics Object
	RRFKPosition     fk("DataFiles/Titan2.dh");
	
	//Create the matrices to hold the transformation data
	const RRXform* EEFXform;
	const RRXform* globalXforms = new RRXform[fk.GetDOF()];	
	const RRXform* localXforms  = new RRXform[fk.GetDOF()];
	RRHandPose			 EEFPose;

	//Set the joint angles
	RRVector joints(fk.GetDOF());
	joints.at(0) = -5.0; joints.at(1) = 12.0;
	joints.at(2) = 10.0; joints.at(3) = 32.0;
	joints.at(5) = 11.0; joints.at(6) = 60.0;
	joints *= DEGTORAD;

	//Determine the Hand Position for the joint angles
	EEFXform = fk.GetHandPose(joints);
	EEFXform->ToHandPose(EEFPose);
	cout << "The Hand Pose is: \n" << EEFPose << endl;

	//Adjust the toolPoint
	RRVector3				 toolPoint(3,4,5);
	fk.SetToolPoint(toolPoint);

	//Determine the new Hand Postion 
	EEFXform = fk.GetHandPose(joints);
	EEFXform->ToHandPose(EEFPose);
	cout << "The Tool Pose is: \n" << EEFPose << endl;

	//Collect transformation data for all links
	globalXforms = fk.GetAllGlobalTransformations(joints);
	localXforms  = fk.GetAllLocalTransformations(joints);

	//Output selected transformations
	cout << "T01*T12*T23 is: \n" << localXforms[0]*localXforms[1]*localXforms[2] << endl;
	cout << "T03 is: \n" << globalXforms[2] << endl;

	delete globalXforms;
	delete localXforms;
	return 0;
}