//////////////////////////////////////////////////////////////////////////////
//
//       Title     : exercise10.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 10 of the OSCAR Guide
//
//////////////////////////////////////////////////////////////////////////////
#define DHFile "DataFiles/Titan2.dh"

#include "InverseKinematics/IKJForSix.h"
#include "ManualController/SpaceballMC.h"
#include "Communications/RoboTalk.h"

int main()
{
	unsigned int i; //counter
	
  //Set up Graphics Communication Information
	const unsigned int noTags = 7;
	static float tagValues[noTags];
	char* 	TagNames[] = {"t21_joint_1","t21_joint_2","t21_joint_3",
												"t21_joint_4","t21_joint_5","t21_joint_6",
												"End_Effector"};
	  
  if(Connect("TitanII_Mitch", SELF) != NO_ERROR){
    cout << "Error Connecting" << endl;
  		return 0;
	}

  //Set up the robot data
  RRVector initialJoints(6);
  initialJoints[0] = 0.0;	  initialJoints[1] = 60.0;
  initialJoints[2] = -60.0;	initialJoints[3] = 10.0;
  initialJoints[4] = 40.0;  initialJoints[5] = 10.0;
	initialJoints *= DEGTORAD;
  RRVector joints(initialJoints);
  
	//create the objects that will hold the command changes in EEF position
  RRHandPose       handPose(RRHandPose::FIXED_XYZ);
  RRXform          handXform;
	RRVector3        toolPoint(0, 0, 6.68);
  
  //Determine the initial hand position from the FK object.
  RRFKPosition fk(DHFile);
	fk.SetToolPoint(toolPoint);
  (fk.GetHandPose(joints))->ToHandPose(handPose);
	
	//Create the Manual Controller object
	//(spaceball application must be running!)
  RRSpaceballMC sb;
	RRVector buttons(10);
	RRVector deltas(6);
	RRHandPose temp;
	for(i=0;i<3;i++){deltas.at(i) = 0.0025; deltas.at(i+3) = .00025;}
	if(sb.Initialize() != 1){
		DisplayError("SpaceBall failed to initialize.");
		return -1;
	}
	sb.SetCurrentHand(handPose);
	sb.SetDeltaScalerOn();
	sb.SetDeltaScaling(deltas);
	
	//Create the inverse kinematic objects
  RRIKJForSix ik(DHFile, joints);
	ik.SetToolPoint(toolPoint);
	ik.SetErrorTolerance( 0.0001 , 10000.0 );
	ik.SetMinimumMOM(4000);
  
	int kFlag = 0;
	while(kFlag != -4)
  {
		sb.GetPacketFromMC();
		sb.GetDeltas(temp);
		sb.GetCurrentButtonStatus(buttons);
		if(buttons.at(8)){
			cout << "A BUTTON WAS PRESSED." << endl;
			kFlag = -4;
		}

		if(kFlag != -4){
			//spaceball frame differs from base frame
			handPose.at(0) += temp.at(2);
			handPose.at(1) += temp.at(0);
			handPose.at(2) += temp.at(1);
			handPose.at(3) +- temp.at(4);
			handPose.at(4) += temp.at(5);
			handPose.at(5) += temp.at(3);
      handXform = handPose.ToTransform( );

			cout << ik.GetJointPosition(handXform, joints) << endl;

			(fk.GetHandPose(joints))->ToHandPose(handPose);
	
			for(i=0;i<6;i++) tagValues[i]=joints.at(i)*RADTODEG;
			SetTagValues(TagNames, tagValues, noTags);
		}//end of if(kFlag !=-4)
  }//end of while(kFlag != -4)

  return 0;
}//end of main