//////////////////////////////////////////////////////////////////////////////
//
//       Title     : exercise6.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 6 of the OSCAR Guide
//
//////////////////////////////////////////////////////////////////////////////
#include "Math/Vector.h"
#include "Controllers/GeneralKeyboard.h"
#include "Communications/RoboTalk.h"

int main(void)
{
	// These are the control names designated in the RoboWorks .scn file 
	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"};
	
  //Connect to RoboWorks. SELF is defined as this
	//computer. Otherwise use IP address of simulation computer.
  if(Connect("Titan II", SELF) != NO_ERROR){
		DisplayError("Error Connecting.");
		return 0;
	}

	//Set the initial joint angles
	RRVector joints(7);
	joints.at(0) = -5.0; joints.at(1) = 12.0;	joints.at(2) = 10.0;
	joints.at(3) = 32.0; joints.at(4) = 11.0; joints.at(5) = 60.0;
	joints.at(6) = 0.0; //End-Effector
	
	//Create the Manual Controller object
  RRGeneralKeyboard  keyboard;

  //Define the change in joint angles from keyboard input
	double stepValues[] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.05};
  const RRVector deltas(7, stepValues);
	
	unsigned int kFlag = 0;
	while(kFlag != -4)
  {
    kFlag = keyboard.UpdateVector(joints, deltas);

		for(int i=0;i<7;i++) tagValues[i]=joints.at(i);
 		if(SetTagValues(TagNames, tagValues, noTags) != NO_ERROR){
			DisplayError("Error writing.");
			return 0;
		}
  }//end of while(kFlag != -4)
	
	//Disconnect from RoboWorks
  Disconnect();
	return 0;
}