//Including the necessary headers
#include <Position/PositionCoordinatesSemantics.h>
#include <Orientation/OrientationCoordinatesSemantics.h>
#include <Pose/PoseCoordinatesSemantics.h>
#include <TranslationalVelocity/TranslationalVelocityCoordinatesSemantics.h>
#include <RotationalVelocity/RotationalVelocityCoordinatesSemantics.h>
#include <Twist/TwistCoordinatesSemantics.h>
#include <Force/ForceCoordinatesSemantics.h>
#include <Torque/TorqueCoordinatesSemantics.h>
#include <Wrench/WrenchCoordinatesSemantics.h>

//Using the geometric_semantics namespace
using namespace geometric_semantics;

// The main program
int main (int argc, const char* argv[])                                                                                                                                                                                                      
{
    // Here comes the code of our first application

    // Creating the geometric relations semantics
    PositionCoordinatesSemantics position("a","C","b","D","r");
    OrientationCoordinatesSemantics orientation("e","C","f","D","r");
    PoseCoordinatesSemantics pose("a","e","C","b","f","D","r");
    
    TranslationalVelocityCoordinatesSemantics translationalVelocity("a","C","D","r");
    RotationalVelocityCoordinatesSemantics rotationalVelocity("C","D","r");
    TwistCoordinatesSemantics twist("a","C","D","r");

    TorqueCoordinatesSemantics torque("a","C","D","r");
    ForceCoordinatesSemantics force("C","D","r");
    WrenchCoordinatesSemantics wrench("a","C","D","r");

    //Doing semantic operations with the geometric relations
    // inverting
    PositionCoordinatesSemantics positionInv = position.inverse();
    OrientationCoordinatesSemantics orientationInv = orientation.inverse();
    PoseCoordinatesSemantics poseInv = pose.inverse();
    TranslationalVelocityCoordinatesSemantics translationalVelocityInv = translationalVelocity.inverse();
    RotationalVelocityCoordinatesSemantics rotationalVelocityInv = rotationalVelocity.inverse();
    TwistCoordinatesSemantics twistInv = twist.inverse();
    TorqueCoordinatesSemantics torqueInv = torque.inverse();
    ForceCoordinatesSemantics forceInv = force.inverse();
    WrenchCoordinatesSemantics wrenchInv = wrench.inverse();

    // print the inverses
    std::cout << "-----------------------------------------" << std::endl;
    std::cout << "Inverses: " << std::endl;
    std::cout << "     " << positionInv << " is the inverse of " << position << std::endl;
    std::cout << "     " << orientationInv << " is the inverse of " << orientation << std::endl;
    std::cout << "     " << poseInv << " is the inverse of " << pose << std::endl;
    std::cout << "     " << translationalVelocityInv << " is the inverse of " << translationalVelocity << std::endl;
    std::cout << "     " << rotationalVelocityInv << " is the inverse of " << rotationalVelocity << std::endl;
    std::cout << "     " << twistInv << " is the inverse of " << twist << std::endl;
    std::cout << "     " << torqueInv << " is the inverse of " << torque << std::endl;
    std::cout << "     " << forceInv << " is the inverse of " << force << std::endl;
    std::cout << "     " << wrenchInv << " is the inverse of " << wrench << std::endl;

    //Composing
    PositionCoordinatesSemantics positionComp = compose(position,positionInv);
    OrientationCoordinatesSemantics orientationComp = compose(orientation,orientationInv);
    PoseCoordinatesSemantics poseComp = compose(pose,poseInv);
    TranslationalVelocityCoordinatesSemantics translationalVelocityComp = compose(translationalVelocity,translationalVelocityInv);
    RotationalVelocityCoordinatesSemantics rotationalVelocityComp = compose(rotationalVelocity,rotationalVelocityInv);
    TwistCoordinatesSemantics twistComp = compose(twist,twistInv);
    TorqueCoordinatesSemantics torqueComp = compose(torque,torqueInv);
    ForceCoordinatesSemantics forceComp = compose(force,forceInv);
    WrenchCoordinatesSemantics wrenchComp = compose(wrench,wrenchInv);

    // print the composed objects
    std::cout << "-----------------------------------------" << std::endl;
    std::cout << "Composed objects: " << std::endl;
    std::cout << "     " << positionComp << " is the composition of " << position << " and " << positionInv << std::endl;
    std::cout << "     " << orientationComp << " is the composition of " << orientation << " and " << orientationInv <<  std::endl;
    std::cout << "     " << poseComp << " is the composition of " << pose <<  " and " << poseInv << std::endl;
    std::cout << "     " << translationalVelocityComp << " is the composition of " << translationalVelocity  << " and " << translationalVelocityInv << std::endl;
    std::cout << "     " << rotationalVelocityComp << " is the composition of " << rotationalVelocity  << " and " << rotationalVelocityInv << std::endl;
    std::cout << "     " << twistComp << " is the composition of " << twist <<  " and " << twistInv << std::endl;
    std::cout << "     " << torqueComp << " is the composition of " << torque <<  " and " << torqueInv << std::endl;
    std::cout << "     " << forceComp << " is the composition of " << force <<  " and " << forceInv << std::endl;
    std::cout << "     " << wrenchComp << " is the composition of " << wrench <<  " and " << wrenchInv << std::endl;

}