//////////////////////////////////////////////////////////////////////////////
//
//       Title           : KinematicsTut2.cpp
//       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: KinematicsTut2.cpp,v $
//       Revision 1.4  2006/08/25 19:21:41  jknoll
//       updated to current OSCAR
//
//       Revision 1.3  2005/01/11 17:40:59  pmarch
//       no message
//
//       Revision 1.2  2003/10/03 16:13:39  pmarch
//       no message
//
//       Revision 1.1  2003/08/14 16:52:20  pmarch
//       no message
//
//
//////////////////////////////////////////////////////////////////////////////
#include "ForwardKinematics/FKPosition.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";


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(void)
{
  //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, TitanIII, is the name of the manipulator in the XML file.
  FKPosition fk( *(GetManipulator(robots,"TitanIII")->GetDHParameters()) );
	
  //Create pointers to access the transformation data.
  const Xform*   EEFXform;
  const Xform*   globalXforms;	
  const Xform*   localXforms;

  HandPose			 EEFPose;

  //Create the Vector using fk.GetDOF() to make
  //sure the Vector is the correct size.
  Vector joints(fk.GetDOF());

  //Fill joints
  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 *= DegToRad;

  //Determine the Hand Position for the joint angles
  EEFXform = fk.GetHandPose(joints);

  //Set the EE position from the Xform
  EEFPose.Set(*EEFXform);
  cout << "The Hand Pose is: \n" << EEFPose << endl;

  //Set the Tool point for the robot.
  Vector3	toolPoint(3,4,5);
  fk.SetTool(toolPoint);

  //Calculate and display the new handpose.
  EEFXform = fk.GetHandPose(joints);
  EEFPose.Set(*EEFXform);
  cout << "The Tool Pose is: \n" << EEFPose << endl;

  //Collect transformation data for all links
  globalXforms = fk.GetAllGlobalTransformations(joints);
  localXforms  = fk.GetAllLocalTransformations(joints);

  //Output selected transformations
  cout << "T01*T12*T23 is: \n" << localXforms[0]*localXforms[1]*localXforms[2] << endl;
  cout << "T03 is: \n" << globalXforms[2] << endl;

  return 0;
}