//////////////////////////////////////////////////////////////////////////////
//
//       Title           : KinematicsTut6.cpp
//       Project         : OSCAR
//       Created         : 7/23/2003
//       Author          : Peter S. March
//       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: KinematicsTut6.cpp,v $
//       Revision 1.4  2006/08/25 19:21:59  jknoll
//       updated to current OSCAR
//
//       Revision 1.3  2005/01/11 17:41:01  pmarch
//       no message
//
//       Revision 1.2  2003/10/03 16:14:27  pmarch
//       no message
//
//       Revision 1.1  2003/08/14 16:51:24  pmarch
//       no message
//
//
//////////////////////////////////////////////////////////////////////////////
#include "ForwardKinematics/FKJacobian.h"   // provides generalized forward position solution
#include "InverseKinematics/IKJReconfig.h"  // provides a closed form puma inverse
#include "Controllers/GeneralKeyboard.h"    // class to poll the keyboard for input
#include "RoboWorks/Include/RoboTalk.h"     // communicate with RoboWorks puma model
#include "FileData/WorkcellData.h"          // used to store and retrieve DH and other data in XML files

using namespace OSCAR;
using namespace std;

String scemafilepath = "c:\\oscar\\Schemas\\ManipulatorModelingTypes.xsd";
String xmlfilepath = "c:\\oscar Tutorials\\Kinematics Tutorials\\DataFiles\\KinematicsTutorial.xml";

//tag names for RoboWorks
char* 	TagNames[] = {"planar4R_1","planar4R_2","planar4R_3", "planar4R_4"};


OSCAR::ManipulatorData* GetManipulator( vector<ManipulatorData*> manips, string name ) 
{ 
	for( unsigned int i=0; i<manips.size(); i++) {
      if( manips[i]->GetName()==name ) {
        return manips[i];
      }
    }
	OSCARError newerror(777, "NO DATA FOUND");
    newerror.SetCustomDescription( "GetManipulator -> Manipulator named '"+name+"' not found in XML." );
    DisplayError( newerror );
    system("PAUSE");
    throw runtime_error( string("GetManipulator -> Manipulator named '"+name+"' not found in XML.").c_str() );
}


int main()
{
  //more variables for Roboworks use
  const unsigned int noTags = 4;
  float tagValues[noTags];

  //Connecting up with RoboWorks
  if(Connect("Planar4R", SELF) != NO_ERROR){
    cout << "Error Connecting" << endl;
    return 0;
  }

  //setup the keyboard controller and the delta vector
  GeneralKeyboard keyboard; //defines the keyboard as the controller
  Vector deltas(6);		// setup a vector to supply the delta motion commands 
 
  for(unsigned int i = 0; i < 3; i++)
  {
    deltas.at(i) = .4;
    deltas.at(i+3) = .04;
  }
   
  // Since the robot is planar,
  // set the z, roll, and pitch deltas
  // to 0.0
  deltas.at(2) = 0.0;  // z
  deltas.at(3) = 0.0;  // roll
  deltas.at(4) = 0.0;  // pitch

  //All data is stored in an XML file.  
  //This data can be DH parameters, position/velocity/acceleration limits, and 
  //obstacle data for multiple manipulators.

  //The XML file must be validated with schema at schemafilepath
  OSCARError error(noError);
  error = XMLData::SetSchemaFile(scemafilepath);
  if(error != OSCAR::noError){
    OSCAR::DisplayError(error);
    system("PAUSE");
    return 0; }

  //Create WorkcellData object to read and store data from xml file at xmlfilepath
  WorkcellData XMLDataFile(xmlfilepath, error);
  if(error != OSCAR::noError){
    OSCAR::DisplayError(error); 
	system("PAUSE");
    return 0; }
  XMLDataFile.LoadData();

  vector<ManipulatorData*> robots = XMLDataFile.Manipulators;

  //Create a Forward Kinematics object by using the DH paramaters stored in the XML file.
  //The name, Planar4R, is the name of the manipulator in the XML file.
  FKJacobian fkj( *(GetManipulator(robots,"Planar4R")->GetDHParameters()) );  

  //Set up the tool point for the robot
  fkj.SetTool( *(GetManipulator(robots,"Planar4R")->GetToolPlatePose()) );

  //set up the initialJoints Vector
  JointVector initialJoints(fkj.GetDOF());
  initialJoints[0] = 30.0;
  initialJoints[1] = 65.0;
  initialJoints[2] = -105.0;
  initialJoints[3] = 55.0;

  initialJoints *= DegToRad;

  //Send initial joint values to Roboworks
  for(i = 0; i < noTags; i++) 
    tagValues[i]=initialJoints.at(i)*RadToDeg;
  SetTagValues(TagNames, tagValues, noTags);

  //create an HandPose object and calculate the
  //initial hand position
  HandPose initialHandPose(Orientation::FixedXYZ);
  (fkj.GetHandPose(initialJoints))->Get(initialHandPose);

  //Create an IKJReconfig object by supplying an initialJoint vector
  //and a pointer to an FKJacobian object
  IKJReconfig<>   ikj(initialJoints,&fkj);  
  ikj.SetMinimumMOT(10);

  //set up variables to be used inside loop
  bool  inverseResult;

  int kFlag = 0;
  JointVector   currentJoints(fkj.GetDOF());
  HandPose previousHand(Orientation::FixedXYZ), currentHand(Orientation::FixedXYZ);

  currentJoints = initialJoints;
  previousHand = currentHand = initialHandPose;

  //Set the Active/Inactive inputs and outputs
  //These outputs will always be inactive because it
  //is a planar robot
  currentHand.SetCoordinateStatus(2,Inactive);
  currentHand.SetCoordinateStatus(3,Inactive);
  currentHand.SetCoordinateStatus(4,Inactive);

  //the following line locks the first joint
  //comment it out to unlock to first joint
  //currentJoints.SetCoordinateStatus(0,Inactive);

  //the following line disregards the orientation of the EE
  //comment it out to use orientation in the inverse
  //currentHand.SetCoordinateStatus(5,Inactive);

  cout<<" -- Inverse Control of Planar 4R 3-DOF Manipulator --\n"<<endl;
  cout<<"    Use keys for x, y, and yaw control of end-effector."<<endl;
  cout<<"      For +/- x direction use 1 and q."<<endl;
  cout<<"      For +/- y direction use 2 and w."<<endl;
  cout<<"      For +/- z rotation use 6 and y.\n"<<endl;
  cout<<"    Use keys a and z for Self-motion about joint 2.\n"<<endl;
  cout<<"    Press ESC to exit.\n\n";
  cout<<"any key to begin...";
  getch();

  bool selfMotion=false;
  while(kFlag != -4)
	{
    kFlag = keyboard.UpdateVector(currentHand, deltas);
    if(kFlag == -4)
      cout << "ESC WAS PRESSED." << endl;

    //Self Motion
	if(keyboard.GetKeyState('a') == true){
      currentJoints.at(1) += .2;
      selfMotion = true;
      ikj.SetJointPosition(currentJoints);
    }
     
    //Self Motion
    if(keyboard.GetKeyState('z') == true){
      currentJoints.at(1) -= .2;
      selfMotion = true;
      ikj.SetJointPosition(currentJoints);
    }


    // the end effector command was changed
    if((kFlag > -1) || (selfMotion==true)){
	  
      //Computes the joint positions based on the end effector position
      inverseResult = ikj.GetJointPosition(currentHand, currentJoints);
      
	  //Check to make sure inverse calculation was successful
      if(inverseResult == false){
        currentHand = previousHand;
        DisplayError(ikj.GetError());
      }
      previousHand = currentHand;

    //send current position to roboworks
    for(i = 0; i < fkj.GetDOF(); i++) 
      tagValues[i]=currentJoints.at(i)*RadToDeg;
    SetTagValues(TagNames, tagValues, noTags);

    selfMotion=false;
    }

    //Calculate and display the Measure of Manipulability
    double mom = ikj.GetSolutionProperties();
    cout <<"MOM = " << mom << endl;
  }
  return 0;
}