#include <ros/ros.h>
#include "datalink/abstractobject.h"
#include "datalink/graspobject.h"
#include "datalink/hand.h"
#include "datalink/humanspace.h"
#include "datalink/joint.h"
#include "datalink/obstacle.h"
#include "datalink/robot.h"
#include "datalink/robotfpe.h"
#include "datalink/world.h"
#include "datalink/worldobject.h"
#include "datalink/quaternion.h"
#include <vector>
#include <iostream>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <cstdio>
#include <tf2/LinearMath/Quaternion.h>
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/PointStamped.h"
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>

int main(int argc, char** argv) {

    ros::init(argc, argv, "Application_Model");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // setup the planning environment
    static const std::string PLANNING_GROUP = "panda_arm";
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    ros::Duration(3.0).sleep();

    move_group.setStartStateToCurrentState();

    geometry_msgs::Pose another_pose;
    another_pose.orientation.w = 1.0;
    another_pose.position.x = 0.4;
    another_pose.position.y = -0.4;
    another_pose.position.z = 0.9;
    move_group.setPoseTarget(another_pose);

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");

    // Move the (simulated) robot in gazebo
    move_group.move();

    if(another_pose.position.z==0.9)
        ROS_INFO(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
    else
        ROS_INFO("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");

    ros::shutdown();

    abstractobject o1;
    graspobject o2;
    hand o3;
    humanspace o4;

    joint o5a;
    joint o5b;
    joint o5c;
    joint o5d;
    joint o5e;
    joint o5f;
    joint o5g;

    obstacle o6;
    robot o7;
    robotfpe o8;
    world o9;
    worldobject o10;

    o7.jo.push_back(o5a);
    o7.jo.push_back(o5b);
    o7.jo.push_back(o5c);
    o7.jo.push_back(o5d);
    o7.jo.push_back(o5e);
    o7.jo.push_back(o5f);
    o7.jo.push_back(o5g);

    o9.c->robname="Robot FPE";
    o9.c->of=true;
    o9.c->moving=false;
    o9.c->grasppos=true;
    o9.c->planninggrp= "Panda Teaching";

    o8.h->grippos= {12.1, 14.3, 18.2};
    o3.grippos={1.1,2.3,2.1};

    o9.a->humannum = 1;
    o9.b1->name="Cube";
    o9.b1->size={5.2, 7.5, 9.1};
    o9.b1->p1= {13.2, 14.1, 17.5};
    o9.b1->x= 2.3;
    o9.b1->y = 1.2;
    o9.b1->z = 0.2;
    o9.b1->w = 0.1;
    o9.b2->name="Ball";
    o9.b2->size={1.3, 2.1, 3.2};
    o9.b2->p1= {12.5, 16.4, 17.3};
    o9.b2->x= 1.1;
    o9.b2->y = 1.4;
    o9.b2->z = 1.6;
    o9.b2->w = 1.9;

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    ros::Rate rate(10);
    int i=0;
    int ho=0;
    while (node_handle.ok()) {
        {
            geometry_msgs::TransformStamped transformStamped[7];

            try { transformStamped[++i] = tfBuffer.lookupTransform("world", o7.jo[++ho].name, ros::Time(0));
            } catch (tf2::TransformException &ex) {
                ROS_WARN("%s", ex.what());
                ros::Duration(0.1).sleep();
                continue;
            }
            if(ho==7)
                ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");
            else
                ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = "<<ho);

            if(o8.h==nullptr)
                ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION");

            if(o9.a== nullptr)
                ROS_ERROR("ERROR IN POINTER WORLD HUMANSPACE AGGREGATION");

            if(o9.c== nullptr)
                ROS_ERROR("ERROR IN ROBOT CLASS AGGREGATION");

            if(o9.b1== nullptr)
                ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");

            if(o9.b2== nullptr)
                ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
        }
        rate.sleep();
    }


    return 0;
}