#include <ros/ros.h>
#include "datalink/abstractobject.h"
#include "datalink/graspobject.h"
#include "datalink/hand.h"
#include "datalink/humanspace.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 "datalink/joint.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 "datalink/joint.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 <std_msgs/UInt32.h>
#include "std_msgs/String.h"
#include <sstream>
//use global var gvar
uint32_t gvar;
void pub(const std_msgs::UInt32::ConstPtr& msg)
{
    ROS_INFO("I heard: [%i]", msg->data);
    gvar= msg->data;
}

    int main(int argc, char **argv) {
    ros::init(argc, argv, "ROSNODE1");
    ros::NodeHandle node_handle("namespace_name");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ROS_INFO("HELLO, WORLD");

    world world1;
    auto robot1 = new robotfpe;
    world1.c = robot1;
    world1.humans = new humanspace;
    world1.humans->humannum = 1;

    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 robot1;
    robotfpe o8;
    world o9;
    worldobject o10;

    robot1->jo.push_back(o5a);
    robot1->jo.push_back(o5b);
    robot1->jo.push_back(o5c);
    robot1->jo.push_back(o5d);
    robot1->jo.push_back(o5e);
    robot1->jo.push_back(o5f);
    robot1->jo.push_back(o5g);

    int yx = 0;
    robot1->jo[++yx].name = "panda_link1";
    robot1->jo[++yx].name = "panda_link2";
    robot1->jo[++yx].name = "panda_link3";
    robot1->jo[++yx].name = "panda_link4";
    robot1->jo[++yx].name = "panda_link5";
    robot1->jo[++yx].name = "panda_link6";
    robot1->jo[++yx].name = "panda_link7";

    robot1->robname = "Robot FPE";
    robot1->of = true;
    robot1->moving = false;
    robot1->grasppos = true;
    robot1->planninggrp = "Panda Teaching";
    robot1->h = new hand{};
    robot1->h->grippos = {12.1, 14.3, 18.2};

        auto *obstacle1 = new obstacle{};
        obstacle1->name = "Cube";
        obstacle1->size = {5.2, 7.5, 9.1};
        obstacle1->p1 = {13.2, 14.1, 17.5};
        obstacle1->x = 2.3;
        obstacle1->y = 1.2;
        obstacle1->z = 0.2;
        obstacle1->w = 0.1;
    /*
    o9.objects->name = "Cube";
    o9.objects->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;
*/
        auto *graspobject1 = new graspobject;
        graspobject1->name = "Ball";
        graspobject1->size = {1.3, 2.1, 3.2};
        graspobject1->p1 = {12.5, 16.4, 17.3};
        graspobject1->x = 1.1;
        graspobject1->y = 1.4;
        graspobject1->z = 1.6;
        graspobject1->w = 1.9;
   /* 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;
*/
    ros::Subscriber sub = node_handle.subscribe("updateDectectedHumans", 1000, pub);
    ros::spin();

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);
    ros::Rate rate(10);

    while (node_handle.ok()) {
        //for (const auto &topic : ROSNODE1::topics)
        {
            geometry_msgs::TransformStamped transformStamped1;
            geometry_msgs::TransformStamped transformStamped2;
            geometry_msgs::TransformStamped transformStamped3;
            geometry_msgs::TransformStamped transformStamped4;
            geometry_msgs::TransformStamped transformStamped5;
            geometry_msgs::TransformStamped transformStamped6;
            geometry_msgs::TransformStamped transformStamped7;

            try {
                transformStamped1 = tfBuffer.lookupTransform("world", robot1->jo[1].name, ros::Time(0));
                transformStamped2 = tfBuffer.lookupTransform("world", robot1->jo[2].name, ros::Time(0));
                transformStamped3 = tfBuffer.lookupTransform("world", robot1->jo[3].name, ros::Time(0));
                transformStamped4 = tfBuffer.lookupTransform("world", robot1->jo[4].name, ros::Time(0));
                transformStamped5 = tfBuffer.lookupTransform("world", robot1->jo[5].name, ros::Time(0));
                transformStamped6 = tfBuffer.lookupTransform("world", robot1->jo[6].name, ros::Time(0));
                transformStamped7 = tfBuffer.lookupTransform("world", robot1->jo[7].name, ros::Time(0));
                o9.humans->humannum =gvar;
            } catch (tf2::TransformException &ex) {
                ROS_WARN("%s", ex.what());
                ros::Duration(0.1).sleep();
                continue;
            }

            robot1->jo[1].p1[0] = 0.0;
            robot1->jo[1].p1[1] = 0.0;
            robot1->jo[1].p1[2] = 0.0;
            robot1->jo[2].p1[0] = 0.0;
            robot1->jo[2].p1[1] = 0.0;
            robot1->jo[2].p1[2] = 0.0;
            robot1->jo[3].p1[0] = 0.0;
            robot1->jo[3].p1[1] = 0.0;
            robot1->jo[3].p1[2] = 0.0;
            robot1->jo[4].p1[0] = 0.0;
            robot1->jo[4].p1[1] = 0.0;
            robot1->jo[4].p1[2] = 0.0;
            robot1->jo[5].p1[0] = 0.0;
            robot1->jo[5].p1[1] = 0.0;
            robot1->jo[5].p1[2] = 0.0;
            robot1->jo[6].p1[0] = 0.0;
            robot1->jo[6].p1[1] = 0.0;
            robot1->jo[6].p1[2] = 0.0;
            robot1->jo[7].p1[0] = 0.0;
            robot1->jo[7].p1[1] = 0.0;
            robot1->jo[7].p1[2] = 0.0;

            robot1->jo[1].p1[0] = transformStamped1.transform.translation.x;
            robot1->jo[1].p1[1] = transformStamped1.transform.translation.y;
            robot1->jo[1].p1[2] = transformStamped1.transform.translation.z;
            ROS_INFO_STREAM(
                    "panda_link1 Position is " << " x = " << robot1->jo[1].p1[0] << ", y = " << robot1->jo[1].p1[1]
                                               << ", z = " << robot1->jo[1].p1[2]);

            robot1->jo[2].p1[0] = transformStamped2.transform.translation.x;
            robot1->jo[2].p1[1] = transformStamped2.transform.translation.y;
            robot1->jo[2].p1[2] = transformStamped2.transform.translation.z;
            ROS_INFO_STREAM(
                    "panda_link2 Position is " << " x = " << robot1->jo[2].p1[0] << ", y = " << robot1->jo[2].p1[1]
                                               << ", z = " << robot1->jo[2].p1[2]);

            robot1->jo[3].p1[0] = transformStamped3.transform.translation.x;
            robot1->jo[3].p1[1] = transformStamped3.transform.translation.y;
            robot1->jo[3].p1[2] = transformStamped3.transform.translation.z;
            ROS_INFO_STREAM(
                    "panda_link3 Position is " << " x = " << robot1->jo[3].p1[0] << ", y = " << robot1->jo[3].p1[1]
                                               << ", z = " << robot1->jo[3].p1[2]);

            robot1->jo[4].p1[0] = transformStamped4.transform.translation.x;
            robot1->jo[4].p1[1] = transformStamped4.transform.translation.y;
            robot1->jo[4].p1[2] = transformStamped4.transform.translation.z;
            ROS_INFO_STREAM(
                    "panda_link4 Position is " << " x = " << robot1->jo[4].p1[0] << ", y = " << robot1->jo[4].p1[1]
                                               << ", z = " << robot1->jo[4].p1[2]);

            robot1->jo[5].p1[0] = transformStamped5.transform.translation.x;
            robot1->jo[5].p1[1] = transformStamped5.transform.translation.y;
            robot1->jo[5].p1[2] = transformStamped5.transform.translation.z;
            ROS_INFO_STREAM(
                    "panda_link5 Position is " << " x = " << robot1->jo[5].p1[0] << ", y = " << robot1->jo[5].p1[1]
                                               << ", z = " << robot1->jo[5].p1[2]);

            robot1->jo[6].p1[0] = transformStamped6.transform.translation.x;
            robot1->jo[6].p1[1] = transformStamped6.transform.translation.y;
            robot1->jo[6].p1[2] = transformStamped6.transform.translation.z;
            ROS_INFO_STREAM(
                    "panda_link6 Position is " << " x = " << robot1->jo[6].p1[0] << ", y = " << robot1->jo[6].p1[1]
                                               << ", z = " << robot1->jo[6].p1[2]);

            robot1->jo[7].p1[0] = transformStamped7.transform.translation.x;
            robot1->jo[7].p1[1] = transformStamped7.transform.translation.y;
            robot1->jo[7].p1[2] = transformStamped7.transform.translation.z;
            ROS_INFO_STREAM(
                    "panda_link7 Position is " << " x = " << robot1->jo[7].p1[0] << ", y = " << robot1->jo[7].p1[1]
                                               << ", z = " << robot1->jo[7].p1[2]);

            robot1->jo[1].or2.w = 0.0;
            robot1->jo[1].or2.x = 0.0;
            robot1->jo[1].or2.y = 0.0;
            robot1->jo[1].or2.z = 0.0;
            robot1->jo[2].or2.w = 0.0;
            robot1->jo[2].or2.x = 0.0;
            robot1->jo[2].or2.y = 0.0;
            robot1->jo[2].or2.z = 0.0;
            robot1->jo[3].or2.w = 0.0;
            robot1->jo[3].or2.x = 0.0;
            robot1->jo[3].or2.y = 0.0;
            robot1->jo[3].or2.z = 0.0;
            robot1->jo[4].or2.w = 0.0;
            robot1->jo[4].or2.x = 0.0;
            robot1->jo[4].or2.y = 0.0;
            robot1->jo[4].or2.z = 0.0;
            robot1->jo[5].or2.w = 0.0;
            robot1->jo[5].or2.x = 0.0;
            robot1->jo[5].or2.y = 0.0;
            robot1->jo[5].or2.z = 0.0;
            robot1->jo[6].or2.w = 0.0;
            robot1->jo[6].or2.x = 0.0;
            robot1->jo[6].or2.y = 0.0;
            robot1->jo[6].or2.z = 0.0;
            robot1->jo[7].or2.w = 0.0;
            robot1->jo[7].or2.x = 0.0;
            robot1->jo[7].or2.y = 0.0;
            robot1->jo[7].or2.z = 0.0;

            robot1->jo[1].or2.w = transformStamped1.transform.rotation.w;
            robot1->jo[1].or2.x = transformStamped1.transform.rotation.x;
            robot1->jo[1].or2.y = transformStamped1.transform.rotation.y;
            robot1->jo[1].or2.z = transformStamped1.transform.rotation.z;
            ROS_INFO_STREAM(
                    "panda_link1 Orientation is " << " w = " << robot1->jo[1].or2.w << ", x = " << robot1->jo[1].or2.x
                                                  << ", y = " << robot1->jo[1].or2.y << ", z = " << robot1->jo[1].or2.z);

            robot1->jo[2].or2.w = transformStamped2.transform.rotation.w;
            robot1->jo[2].or2.x = transformStamped2.transform.rotation.x;
            robot1->jo[2].or2.y = transformStamped2.transform.rotation.y;
            robot1->jo[2].or2.z = transformStamped2.transform.rotation.z;
            ROS_INFO_STREAM(
                    "panda_link2 Orientation is " << " w = " << robot1->jo[2].or2.w << ", x = " << robot1->jo[2].or2.x
                                                  << ", y = " << robot1->jo[2].or2.y << ", z = " << robot1->jo[2].or2.z);

            robot1->jo[3].or2.w = transformStamped3.transform.rotation.w;
            robot1->jo[3].or2.x = transformStamped3.transform.rotation.x;
            robot1->jo[3].or2.y = transformStamped3.transform.rotation.y;
            robot1->jo[3].or2.z = transformStamped3.transform.rotation.z;
            ROS_INFO_STREAM(
                    "panda_link3 Orientation is " << " w = " << robot1->jo[3].or2.w << ", x = " << robot1->jo[3].or2.x
                                                  << ", y = " << robot1->jo[3].or2.y << ", z = " << robot1->jo[3].or2.z);

            robot1->jo[4].or2.w = transformStamped4.transform.rotation.w;
            robot1->jo[4].or2.x = transformStamped4.transform.rotation.x;
            robot1->jo[4].or2.y = transformStamped4.transform.rotation.y;
            robot1->jo[4].or2.z = transformStamped4.transform.rotation.z;
            ROS_INFO_STREAM(
                    "panda_link4 Orientation is " << " w = " << robot1->jo[4].or2.w << ", x = " << robot1->jo[4].or2.x
                                                  << ", y = " << robot1->jo[4].or2.y << ", z = " << robot1->jo[4].or2.z);

            robot1->jo[5].or2.w = transformStamped5.transform.rotation.w;
            robot1->jo[5].or2.x = transformStamped5.transform.rotation.x;
            robot1->jo[5].or2.y = transformStamped5.transform.rotation.y;
            robot1->jo[5].or2.z = transformStamped5.transform.rotation.z;
            ROS_INFO_STREAM(
                    "panda_link5 Orientation is " << " w = " << robot1->jo[5].or2.w << ", x = " << robot1->jo[5].or2.x
                                                  << ", y = " << robot1->jo[5].or2.y << ", z = " << robot1->jo[5].or2.z);

            robot1->jo[6].or2.w = transformStamped6.transform.rotation.w;
            robot1->jo[6].or2.x = transformStamped6.transform.rotation.x;
            robot1->jo[6].or2.y = transformStamped6.transform.rotation.y;
            robot1->jo[6].or2.z = transformStamped6.transform.rotation.z;
            ROS_INFO_STREAM(
                    "panda_link6 Orientation is " << " w = " << robot1->jo[6].or2.w << ", x = " << robot1->jo[6].or2.x
                                                  << ", y = " << robot1->jo[6].or2.y << ", z = " << robot1->jo[6].or2.z);

            robot1->jo[7].or2.w = transformStamped7.transform.rotation.w;
            robot1->jo[7].or2.x = transformStamped7.transform.rotation.x;
            robot1->jo[7].or2.y = transformStamped7.transform.rotation.y;
            robot1->jo[7].or2.z = transformStamped7.transform.rotation.z;
            ROS_INFO_STREAM(
                    "panda_link7 Orientation is " << " w = " << robot1->jo[7].or2.w << ", x = " << robot1->jo[7].or2.x
                                                  << ", y = " << robot1->jo[7].or2.y << ", z = " << robot1->jo[7].or2.z);
        }
        rate.sleep();
    }
    return 0;
}