#include <Position/PositionSemantics.h>
#include <Orientation/OrientationSemantics.h>
#include <Pose/PoseSemantics.h>
#include <LinearVelocity/LinearVelocitySemantics.h>
#include <AngularVelocity/AngularVelocitySemantics.h>
#include <Twist/TwistSemantics.h>
#include <Force/ForceSemantics.h>
#include <Torque/TorqueSemantics.h>
#include <Wrench/WrenchSemantics.h>

using namespace geometric_semantics;

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

    // Creating the geometric relations semantics
    PositionSemantics position("a","C","b","D");
    OrientationSemantics orientation("e","C","f","D");
    PoseSemantics pose("a","e","C","b","f","D");

    LinearVelocitySemantics linearVelocity("a","C","D");
    AngularVelocitySemantics angularVelocity("C","D");
    TwistSemantics twist("a","C","D");

    TorqueSemantics torque("a","C","D");
    ForceSemantics force("C","D");
    WrenchSemantics wrench("a","C","D");

    //Doing semantic operations with the geometric relations
    // inverting
    PositionSemantics positionInv = position.inverse();
    OrientationSemantics orientationInv = orientation.inverse();
    PoseSemantics poseInv = pose.inverse();
    LinearVelocitySemantics linearVelocityInv = linearVelocity.inverse();
    AngularVelocitySemantics angularVelocityInv = angularVelocity.inverse();
    TwistSemantics twistInv = twist.inverse();
    TorqueSemantics torqueInv = torque.inverse();
    ForceSemantics forceInv = force.inverse();
    WrenchSemantics 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
    PositionSemantics positionComp = compose(position,positionInv);
    OrientationSemantics orientationComp = compose(orientation,orientationInv);
    PoseSemantics poseComp = compose(pose,poseInv);
    LinearVelocitySemantics linearVelocityComp = compose(linearVelocity,linearVelocityInv);
    AngularVelocitySemantics angularVelocityComp = compose(angularVelocity,angularVelocityInv);
    TwistSemantics twistComp = compose(twist,twistInv);
    TorqueSemantics torqueComp = compose(torque,torqueInv);
    ForceSemantics forceComp = compose(force,forceInv);
    WrenchSemantics 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;
}