//////////////////////////////////////////////////////////////////////////////
//
//       Title           : KinematicsTut5.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: KinematicsTut5.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:18  pmarch
//       no message
//
//       Revision 1.1  2003/08/14 16:51:39  pmarch
//       no message
//
//
//////////////////////////////////////////////////////////////////////////////
#include "ForwardKinematics/FKJacobian.h"  
#include "InverseKinematics/IKJReconfig.h"					
#include "Controllers/GeneralKeyboard.h"	
#include "RoboWorks/Include/RoboTalk.h"
#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";

// Setting the Tagnames used by RoboWorks
char* 	TagNames[] = {"Puma_J1","Puma_J2","Puma_J3","Puma_J4","Puma_J5","Puma_J6"};


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()
{
  //Set the number of tags and an array to store the tag values.
  const unsigned int noTags = 6;
  float tagValues[noTags];

  //connect to Roboworks
  if(Connect("Puma760Robot", SELF) != NO_ERROR){
    cout << "Error Connecting.  Make sure Animation->From IPC is selected in RoboWorks" << endl;
    return 0;
  }

  //set up the keyboard object and the deltas vector
  GeneralKeyboard keyboard;
  Vector deltas(6);		
  for(unsigned int i = 0; i < 3; i++)
  {
    deltas.at(i) = 0.5;
    deltas.at(i+3) = 0.02;
  }
  
  //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, Puma760, is the name of the manipulator in the XML file.
  FKJacobian fkj( *(GetManipulator(robots,"Puma760")->GetDHParameters()) ); 

  //set tool point
  Vector3 toolPoint(0, 0, 25);
  fkj.SetTool(toolPoint);

  //set up initial joint position for robot
  JointVector initialJoints(6);
  initialJoints[0] = 10;
  initialJoints[1] = -40;
  initialJoints[2] = -50;
  initialJoints[3] = 5;
  initialJoints[4] = -60;
  initialJoints[5] = 0.0;
  initialJoints *= DegToRad;

  //Create HandPose to store initial position.
  HandPose initialHandPose(Orientation::FixedXYZ);

  //Computes the initial hand position given the initial joint positions
  (fkj.GetHandPose(initialJoints))->Get(initialHandPose);

  //Create an IKJacobian object to solve the inverse position solution.  This
  //object takes a set of initialJoints and a pointer to an FKJacobian object
  //as arguments.  The IKJacobian object only needs to know the Jacobian
  //matrix from the FK object to perform its inverse.  Thus, all tool point
  //and base post settings should be done in the FKJacobian object.
  IKJacobian<>   ikj(initialJoints,&fkj);

  //send initial joints to Roboworks Model
  for(i = 0; i < 6; i++) 
    tagValues[i]=initialJoints.at(i)*RadToDeg;
  SetTagValues(TagNames, tagValues, noTags);
    
  bool  inverseResult;

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

  currentJoints = initialJoints;
  previousHand = currentHand = initialHandPose;

  //Run loop until ESC is pressed
  while(kFlag != -4)
	{
    //Update the currentHand object using the deltas.
    kFlag = keyboard.UpdateVector(currentHand, deltas);
    
    //check for response from keyboard
    if(kFlag > -1){
	  
      //Computes the joint positions based on the end effector position.
      //This method will return a true if a solution was reach, and false
      //if the inverse could not be performed for some reason.
      inverseResult = ikj.GetJointPosition(currentHand, currentJoints);
        
      //If the inverse failed, reset the current position to
      //the previous position and display the error
      if(inverseResult == false){
        currentHand = previousHand;
        DisplayError(ikj.GetError());
      }
      previousHand = currentHand;

    //send the new joint values to Roboworks
    for(i = 0; i < 6; i++) 
      tagValues[i]=currentJoints.at(i)*RadToDeg;
    SetTagValues(TagNames, tagValues, noTags);
    }

    //Use GetSolutionProperties() to return the Measure of
    //Manipulability.  This method can also be used to
    //extract error magnitude, number of iterations, the
    //jacobian, and the HandPose.
    double mom = ikj.GetSolutionProperties();
    cout <<"MOM = " << mom << endl;
  }
  return 0;
}