#include <Position/PositionCoordinatesSemantics.h>
#include <Orientation/OrientationCoordinatesSemantics.h>
#include <Pose/PoseCoordinatesSemantics.h>
#include <LinearVelocity/LinearVelocityCoordinatesSemantics.h>
#include <AngularVelocity/AngularVelocityCoordinatesSemantics.h>
#include <Twist/TwistCoordinatesSemantics.h>
#include <Force/ForceCoordinatesSemantics.h>
#include <Torque/TorqueCoordinatesSemantics.h>
#include <Wrench/WrenchCoordinatesSemantics.h>

using namespace geometric_semantics;

int main (int argc, const char* argv[])
{
    // Here goes the code of our second application

    // Creating the geometric relations coordinates semantics
    PositionCoordinatesSemantics position("a","C","b","D","r");
    OrientationCoordinatesSemantics orientation("e","C","f","D","r");
    PoseCoordinatesSemantics pose("a","e","C","b","f","D","r");

    LinearVelocityCoordinatesSemantics linearVelocity("a","C","D","r");
    AngularVelocityCoordinatesSemantics angularVelocity("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();
    LinearVelocityCoordinatesSemantics linearVelocityInv = linearVelocity.inverse();
    AngularVelocityCoordinatesSemantics angularVelocityInv = angularVelocity.inverse();
    TwistCoordinatesSemantics twistInv = twist.inverse();
    TorqueCoordinatesSemantics torqueInv = torque.inverse();
    ForceCoordinatesSemantics forceInv = force.inverse();
    WrenchCoordinatesSemantics wrenchInv = wrench.inverse();

    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 << "     " << linearVelocityInv << " is the inverse of " << linearVelocity << std::endl;
    std::cout << "     " << angularVelocityInv << " is the inverse of " << angularVelocity << 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);
    LinearVelocityCoordinatesSemantics linearVelocityComp = compose(linearVelocity,linearVelocityInv);
    AngularVelocityCoordinatesSemantics angularVelocityComp = compose(angularVelocity,angularVelocityInv);
    TwistCoordinatesSemantics twistComp = compose(twist,twistInv);
    TorqueCoordinatesSemantics torqueComp = compose(torque,torqueInv);
    ForceCoordinatesSemantics forceComp = compose(force,forceInv);
    WrenchCoordinatesSemantics wrenchComp = compose(wrench,wrenchInv);

    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 << "     " << linearVelocityComp << " is the composition of " << linearVelocity  << " and " << linearVelocityInv << std::endl;
    std::cout << "     " << angularVelocityComp << " is the composition of " << angularVelocity  << " and " << angularVelocityInv << 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;


}