//Including the necessary headers
#include <Position/Position.h>
#include <Orientation/Orientation.h>
#include <Pose/Pose.h>
#include <TranslationalVelocity/TranslationalVelocity.h>
#include <RotationalVelocity/RotationalVelocity.h>
#include <Twist/Twist.h>
#include <Force/Force.h>
#include <Torque/Torque.h>
#include <Wrench/Wrench.h>

#include <Position/PositionCoordinatesKDL.h>
#include <Orientation/OrientationCoordinatesKDL.h>
#include <Pose/PoseCoordinatesKDL.h>
#include <TranslationalVelocity/TranslationalVelocityCoordinatesKDL.h>
#include <RotationalVelocity/RotationalVelocityCoordinatesKDL.h>
#include <Twist/TwistCoordinatesKDL.h>
#include <Force/ForceCoordinatesKDL.h>
#include <Torque/TorqueCoordinatesKDL.h>
#include <Wrench/WrenchCoordinatesKDL.h>

#include <kdl/frames.hpp>
#include <kdl/frames_io.hpp>

//Using the geometric_semantics and KDL namespace
using namespace geometric_semantics;
using namespace KDL;

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

    // Creating the geometric relations 

    // a Position with a KDL::Vector
    Vector coordinatesPosition(1,2,3);
    Position<Vector> position("a","C","b","D","r",coordinatesPosition);

    // an Orientation with KDL::Rotation
    Rotation coordinatesOrientation=Rotation::EulerZYX(M_PI/4,0,0);
    Orientation<Rotation> orientation("e","C","f","D","f",coordinatesOrientation);

    // a Pose with a KDL::Frame
    KDL::Frame coordinatesPose(coordinatesOrientation,coordinatesPosition);
    Pose<KDL::Frame> pose1("a","e","C","b","f","D","f",coordinatesPose);

    // a Pose as aggregation of a Position and a Orientation
    Pose<Vector,Rotation> pose2(position,orientation);

    // a TranslationalVelocity with a KDL::Vector
    Vector coordinatesTranslationalVelocity(1,2,3);
    TranslationalVelocity<Vector> translationalVelocity("a","C","D","r",coordinatesTranslationalVelocity);

    // a RotationalVelocity with a KDL::Vector
    Vector coordinatesRotationalVelocity(1,2,3);
    RotationalVelocity<Vector> rotationalVelocity("C","D","r",coordinatesRotationalVelocity);

    // a Twist with a KDL::Twist
    KDL::Twist coordinatesTwist(coordinatesTranslationalVelocity,coordinatesRotationalVelocity);
    geometric_semantics::Twist<KDL::Twist> twist1("a","C","D","r",coordinatesTwist);

    // a Twist of a TranslationalVelocity and a RotationalVelocity
    geometric_semantics::Twist<Vector,Vector> twist2(translationalVelocity,rotationalVelocity);

    // a Torque with a KDL::Vector
    Vector coordinatesTorque(1,2,3);
    Torque<Vector> torque("a","C","D","r",coordinatesTorque);

    // a Force with a KDL::Vector
    Vector coordinatesForce(1,2,3);
    Force<Vector> force("C","D","r",coordinatesForce);

    // a Wrench with a KDL::Wrench
    KDL::Wrench coordinatesWrench(coordinatesForce,coordinatesTorque);
    geometric_semantics::Wrench<KDL::Wrench> wrench1("a","C","D","r",coordinatesWrench);

    // a Wrench of a Force and a Torque
    geometric_semantics::Wrench<KDL::Vector,KDL::Vector> wrench2(torque,force);

    //Doing operations with the geometric relations
    // inverting
    Position<Vector> positionInv = position.inverse();
    Orientation<Rotation> orientationInv = orientation.inverse();
    Pose<KDL::Frame> pose1Inv = pose1.inverse();
    Pose<Vector,Rotation> pose2Inv = pose2.inverse();
    TranslationalVelocity<Vector> translationalVelocityInv = translationalVelocity.inverse();
    RotationalVelocity<Vector> rotationalVelocityInv = rotationalVelocity.inverse();
    geometric_semantics::Twist<KDL::Twist> twist1Inv = twist1.inverse();
    geometric_semantics::Twist<Vector,Vector> twist2Inv = twist2.inverse();
    Torque<Vector> torqueInv = torque.inverse();
    Force<Vector> forceInv = force.inverse();
    geometric_semantics::Wrench<KDL::Wrench> wrench1Inv = wrench1.inverse();
    geometric_semantics::Wrench<Vector,Vector> wrench2Inv = wrench2.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 << "     " << pose1Inv << " is the inverse of " << pose1 << std::endl;
    std::cout << "     " << pose2Inv << " is the inverse of " << pose2 << std::endl;
    std::cout << "     " << translationalVelocityInv << " is the inverse of " << translationalVelocity << std::endl;
    std::cout << "     " << rotationalVelocityInv << " is the inverse of " << rotationalVelocity << std::endl;
    std::cout << "     " << twist1Inv << " is the inverse of " << twist1 << std::endl;
    std::cout << "     " << twist2Inv << " is the inverse of " << twist2 << std::endl;
    std::cout << "     " << torqueInv << " is the inverse of " << torque << std::endl;
    std::cout << "     " << forceInv << " is the inverse of " << force << std::endl;
    std::cout << "     " << wrench1Inv << " is the inverse of " << wrench1 << std::endl;
    std::cout << "     " << wrench2Inv << " is the inverse of " << wrench2 << std::endl;

    //Composing
    Position<Vector> positionComp = compose(position,positionInv);
    Orientation<Rotation> orientationComp = compose(orientation,orientationInv);
    Pose<KDL::Frame> pose1Comp = compose(pose1,pose1Inv);
    Pose<Vector,Rotation> pose2Comp = compose(pose2,pose2Inv);
    TranslationalVelocity<Vector> translationalVelocityComp = compose(translationalVelocity,translationalVelocityInv);
    RotationalVelocity<Vector> rotationalVelocityComp = compose(rotationalVelocity,rotationalVelocityInv);
    geometric_semantics::Twist<KDL::Twist> twist1Comp = compose(twist1,twist1Inv);
    geometric_semantics::Twist<Vector,Vector> twist2Comp = compose(twist2,twist2Inv);
    Torque<Vector> torqueComp = compose(torque,torqueInv);
    Force<Vector> forceComp = compose(force,forceInv);
    geometric_semantics::Wrench<KDL::Wrench> wrench1Comp = compose(wrench1,wrench1Inv);
    geometric_semantics::Wrench<Vector,Vector> wrench2Comp = compose(wrench2,wrench2Inv);

    // 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 << "     " << pose1Comp << " is the composition of " << pose1 <<  " and " << pose1Inv << std::endl;
    std::cout << "     " << pose2Comp << " is the composition of " << pose2 <<  " and " << pose2Inv << 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 << "     " << twist1Comp << " is the composition of " << twist1 <<  " and " << twist1Inv << std::endl;
    std::cout << "     " << twist2Comp << " is the composition of " << twist2 <<  " and " << twist2Inv << 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 << "     " << wrench1Comp << " is the composition of " << wrench1 <<  " and " << wrench2Inv << std::endl;
    std::cout << "     " << wrench2Comp << " is the composition of " << wrench1 <<  " and " << wrench2Inv << std::endl;

}