//////////////////////////////////////////////////////////////////////////////
//
//       Title           : KinematicsTut3cpp
//       Project         : OSCAR
//       Created         : 5/30/1998
//       Author          : Mitch Pryor
//       Platforms       : All
///       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.
//       Access          : Company Confidential
//       Purpose         :
//
//----------------------------------------------------------------------------
//
//       Classes:
//               <none>
//
//       Global Functions:
//               <none>
//
//       Global Variables:
//               <none>
//
//----------------------------------------------------------------------------
//
//       $Revisions$
//
//       $Log: KinematicsTut3.cpp,v $
//       Revision 1.4  2006/08/25 19:21:58  jknoll
//       updated to current OSCAR
//
//       Revision 1.3  2005/01/11 17:41:00  pmarch
//       no message
//
//       Revision 1.2  2003/10/03 16:14:00  pmarch
//       no message
//
//       Revision 1.1  2003/08/14 16:52:10  pmarch
//       no message
//
//
//////////////////////////////////////////////////////////////////////////////
#include "Math/Vector.h"
#include "Controllers/GeneralKeyboard.h"
#include "RoboWorks/Include/RoboTalk.h"	

using namespace OSCAR;
using namespace std;


int main(void)
{
  //Define the variables needed to communicate with Roboworks.  The
  //variable noTags defines the number of tags that will be communicated
  //to Roboworks.  The tagValues array will hold an array of values to
  //be sent to Roboworks.  The TagNames array holds a list of the names
  //of the tags as defined in the Roboworks model.
  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 III", SELF) != NO_ERROR){
    DisplayError("Error Connecting.");
    return 0;
  }

  //Set the initial joint angles
  Vector 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;
	
  //Create the Manual Controller object
  GeneralKeyboard  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 Vector deltas(7, stepValues);
	
  unsigned int kFlag = 0;
	
  cout<<" -- Joint Control of Titan III 6-DOF Manipulator --\n"<<endl;
  cout<<"    Use keys 1-6 for positive motion and q-y for"<<endl;
  cout<<"    motion.\n"<<endl;
  cout<<"    Press ESC to exit.\n";

  //quit when ESC is pressed
  while(kFlag != -4)
  {
    //Use the UpdateVector method to update the joint
    //values.  The UpdateVector() method will automatically
    //use 1-6 for positive motions (as defined in deltas)
    //and the q-y for negative motions.

	kFlag = keyboard.UpdateVector(joints, deltas);

    //Set the tagValues array equal to the updated joints.
    for(int i=0;i<7;i++) tagValues[i]=(float)joints.at(i);

    //Send the new tags to Roboworks.
    if(SetTagValues(TagNames, tagValues, noTags) != NO_ERROR){
      DisplayError("Error writing.");
      return 0;
    }
  }//end of while(kFlag != -4)
  return 0;
}