#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>

    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");




        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.jo1->name= "panda_link1";
        o7.jo2->name= "panda_link2";
        o7.jo3->name= "panda_link3";
        o7.jo4->name= "panda_link4";
        o7.jo5->name= "panda_link5";
        o7.jo6->name= "panda_link6";
        o7.jo7->name= "panda_link7";

        //o5a.name = "panda_link1";
        //o5b.name = "panda_link2";
        //o5c.name = "panda_link3";
        //o5d.name = "panda_link4";
        //o5e.name = "panda_link5";
        //o5f.name = "panda_link6";
        //o5g.name = "panda_link7";
        o7.jo1->or2= {1.2,5.3,6.5,7.4};
        o7.jo2->or2= {2.5,3.6,5.8,4.9};
        o7.jo3->or2= {5.1,1.2,9.9,6.8};
        o7.jo4->or2= {6.3,3.4,8.5,9.6};
        o7.jo5->or2= {8.2,2.6,7.9,5.5};
        o7.jo6->or2= {7.3,4.4,6.7,4.0};
        o7.jo7->or2= {7.3,4.4,6.7,4.0};


        //o5a.or2 = {1.2,5.3,6.5,7.4};
        //o5b.or2 = {2.5,3.6,5.8,4.9};
        //o5c.or2 = {5.1,1.2,9.9,6.8};
        //o5d.or2 = {6.3,3.4,8.5,9.6};
        //o5e.or2 = {8.2,2.6,7.9,5.5};
        //o5f.or2 = {7.3,4.4,6.7,4.0};
        //o5g.or2 = {9.0,5.2,5.5,8.7};

        //o5a.p1 = {1.3, 10.1, 9.4};
        //o5b.p1 = {14.7, 11.3, 6.5};
        //o5c.p1 = {4.9, 13.8, 7.7};
        //o5d.p1 = {2.4, 12.5, 1.6};
        //o5e.p1 = {3.1, 16.2, 5.3};
        //o5f.p1 = {11.5, 15.4, 3.4};
        //o5g.p1 = {5.1, 18.3, 4.2};

        o7.jo1->p1= {1.3, 10.1, 9.4};
        o7.jo2->p1= {14.7, 11.3, 6.5};
        o7.jo3->p1= {4.9, 13.8, 7.7};
        o7.jo4->p1= {2.4, 12.5, 1.6};
        o7.jo5->p1= {3.1, 16.2, 5.3};
        o7.jo6->p1= {11.5, 15.4, 3.4};
        o7.jo7->p1= {5.1, 18.3, 4.2};
        //o5a.p1 = {1.3, 10.1, 9.4};
        //o5b.p1 = {14.7, 11.3, 6.5};
        //o5c.p1 = {4.9, 13.8, 7.7};
        //o5d.p1 = {2.4, 12.5, 1.6};
        //o5e.p1 = {3.1, 16.2, 5.3};
        //o5f.p1 = {11.5, 15.4, 3.4};
        //o5g.p1 = {5.1, 18.3, 4.2};

        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};

        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);

        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", o7.jo1->name, ros::Time(0));
                    transformStamped2 = tfBuffer.lookupTransform("world", o7.jo2->name, ros::Time(0));
                    transformStamped3 = tfBuffer.lookupTransform("world", o7.jo3->name, ros::Time(0));
                    transformStamped4 = tfBuffer.lookupTransform("world", o7.jo4->name, ros::Time(0));
                    transformStamped5 = tfBuffer.lookupTransform("world", o7.jo5->name, ros::Time(0));
                    transformStamped6 = tfBuffer.lookupTransform("world", o7.jo6->name, ros::Time(0));
                    transformStamped7 = tfBuffer.lookupTransform("world", o7.jo7->name, ros::Time(0));
                } catch (tf2::TransformException &ex) {
                    ROS_WARN("%s", ex.what());
                    ros::Duration(0.1).sleep();
                    continue;

                }

                o7.jo1->p1[0]=0.0;
                o7.jo1->p1[1]=0.0;
                o7.jo1->p1[2]=0.0;
                o7.jo2->p1[0]=0.0;
                o7.jo2->p1[1]=0.0;
                o7.jo2->p1[2]=0.0;
                o7.jo3->p1[0]=0.0;
                o7.jo3->p1[1]=0.0;
                o7.jo3->p1[2]=0.0;
                o7.jo4->p1[0]=0.0;
                o7.jo4->p1[1]=0.0;
                o7.jo4->p1[2]=0.0;
                o7.jo5->p1[0]=0.0;
                o7.jo5->p1[1]=0.0;
                o7.jo5->p1[2]=0.0;
                o7.jo6->p1[0]=0.0;
                o7.jo6->p1[1]=0.0;
                o7.jo6->p1[2]=0.0;
                o7.jo7->p1[0]=0.0;
                o7.jo7->p1[1]=0.0;
                o7.jo7->p1[2]=0.0;
               // o5a.p1[0]=0.0;
                // o5a.p1[1]=0.0;
                // o5a.p1[2]=0.0;
                // o5b.p1[0]=0.0;
                //o5b.p1[1]=0.0;
                //               o5b.p1[2]=0.0;//
                //o5c.p1[0]=0.0;
                // o5c.p1[1]=0.0;
                //o5c.p1[2]=0.0;
                //o5d.p1[0]=0.0;
                // o5d.p1[1]=0.0;
                //o5d.p1[2]=0.0;
                //o5e.p1[0]=0.0;
                //o5e.p1[1]=0.0;
                // o5e.p1[2]=0.0;
                // o5f.p1[0]=0.0;
                // o5f.p1[1]=0.0;
                //o5f.p1[2]=0.0;
                //o5g.p1[0]=0.0;
                //o5g.p1[1]=0.0;
                //o5g.p1[2]=0.0;

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

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

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

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

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

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

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

                o7.jo1->or2.w=0.0;
                o7.jo1->or2.x=0.0;
                o7.jo1->or2.y=0.0;
                o7.jo1->or2.z=0.0;
                o7.jo2->or2.w=0.0;
                o7.jo2->or2.x=0.0;
                o7.jo2->or2.y=0.0;
                o7.jo2->or2.z=0.0;
                o7.jo3->or2.w=0.0;
                o7.jo3->or2.x=0.0;
                o7.jo3->or2.y=0.0;
                o7.jo3->or2.z=0.0;
                o7.jo4->or2.w=0.0;
                o7.jo4->or2.x=0.0;
                o7.jo4->or2.y=0.0;
                o7.jo4->or2.z=0.0;
                o7.jo5->or2.w=0.0;
                o7.jo5->or2.x=0.0;
                o7.jo5->or2.y=0.0;
                o7.jo5->or2.z=0.0;
                o7.jo6->or2.w=0.0;
                o7.jo6->or2.x=0.0;
                o7.jo6->or2.y=0.0;
                o7.jo6->or2.z=0.0;
                o7.jo7->or2.w=0.0;
                o7.jo7->or2.x=0.0;
                o7.jo7->or2.y=0.0;
                o7.jo7->or2.z=0.0;
               /*o5a.or2.w=0.0;
                o5a.or2.x=0.0;
                o5a.or2.y=0.0;
                o5a.or2.z=0.0;
                o5b.or2.w=0.0;
                o5b.or2.x=0.0;
                o5b.or2.y=0.0;
                o5b.or2.z=0.0;
                o5c.or2.w=0.0;
                o5c.or2.x=0.0;
                o5c.or2.y=0.0;
                o5c.or2.z=0.0;
                o5d.or2.w=0.0;
                o5d.or2.x=0.0;
                o5d.or2.y=0.0;
                o5d.or2.z=0.0;
                o5e.or2.w=0.0;
                o5e.or2.x=0.0;
                o5e.or2.y=0.0;
                o5e.or2.z=0.0;
                o5f.or2.w=0.0;
                o5f.or2.x=0.0;
                o5f.or2.y=0.0;
                o5f.or2.z=0.0;
                o5g.or2.w=0.0;
                o5g.or2.x=0.0;
                o5g.or2.y=0.0;
                o5g.or2.z=0.0;*/

                o7.jo1->or2.w=transformStamped1.transform.rotation.w;
                o7.jo1->or2.x=transformStamped1.transform.rotation.x;
                o7.jo1->or2.y=transformStamped1.transform.rotation.y;
                o7.jo1->or2.z=transformStamped1.transform.rotation.z;
                ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< o7.jo1->or2.w<<", x = "<<o7.jo1->or2.x<<", y = "<<o7.jo1->or2.y<<", z = "<<o7.jo1->or2.z);

                o7.jo2->or2.w=transformStamped2.transform.rotation.w;
                o7.jo2->or2.x=transformStamped2.transform.rotation.x;
                o7.jo2->or2.y=transformStamped2.transform.rotation.y;
                o7.jo2->or2.z=transformStamped2.transform.rotation.z;
                ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<<  o7.jo2->or2.w<<", x = "<< o7.jo2->or2.x<<", y = "<< o7.jo2->or2.y<<", z = "<< o7.jo2->or2.z);

                o7.jo3->or2.w=transformStamped3.transform.rotation.w;
                o7.jo3->or2.x=transformStamped3.transform.rotation.x;
                o7.jo3->or2.y=transformStamped3.transform.rotation.y;
                o7.jo3->or2.z=transformStamped3.transform.rotation.z;
                ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< o7.jo3->or2.w<<", x = "<<o7.jo3->or2.x<<", y = "<<o7.jo3->or2.y<<", z = "<<o7.jo3->or2.z);

                o7.jo4->or2.w=transformStamped4.transform.rotation.w;
                o7.jo4->or2.x=transformStamped4.transform.rotation.x;
                o7.jo4->or2.y=transformStamped4.transform.rotation.y;
                o7.jo4->or2.z=transformStamped4.transform.rotation.z;
                ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<o7.jo4->or2.w<<", x = "<<o7.jo4->or2.x<<", y = "<<o7.jo4->or2.y<<", z = "<<o7.jo4->or2.z);

                o7.jo5->or2.w=transformStamped5.transform.rotation.w;
                o7.jo5->or2.x=transformStamped5.transform.rotation.x;
                o7.jo5->or2.y=transformStamped5.transform.rotation.y;
                o7.jo5->or2.z=transformStamped5.transform.rotation.z;
                ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< o7.jo5->or2.w<<", x = "<<o7.jo5->or2.x<<", y = "<<o7.jo5->or2.y<<", z = "<<o7.jo5->or2.z);

                o7.jo6->or2.w=transformStamped6.transform.rotation.w;
                o7.jo6->or2.x=transformStamped6.transform.rotation.x;
                o7.jo6->or2.y=transformStamped6.transform.rotation.y;
                o7.jo6->or2.z=transformStamped6.transform.rotation.z;
                ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< o7.jo6->or2.w<<", x = "<<o7.jo6->or2.x<<", y = "<<o7.jo6->or2.y<<", z = "<<o7.jo6->or2.z);

                o7.jo7->or2.w=transformStamped7.transform.rotation.w;
                o7.jo7->or2.x=transformStamped7.transform.rotation.x;
                o7.jo7->or2.y=transformStamped7.transform.rotation.y;
                o7.jo7->or2.z=transformStamped7.transform.rotation.z;
                ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<o7.jo7->or2.w<<", x = "<<o7.jo7->or2.x<<", y = "<<o7.jo7->or2.y<<", z = "<<o7.jo7->or2.z);

              }
            rate.sleep();
        }


return 0;
}