#include <iostream>
#include <math.h>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <yaml-cpp/yaml.h>

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

    ros::init(argc, argv, "rviz_human");
    ros::NodeHandle n;
    ros::Rate r(100);
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
            "visualization_marker", 15);


    YAML::Node config = YAML::LoadFile("/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml");

    ROS_INFO("%lf", config["arm"].as<double>());

    tf::TransformListener aruco_0;
    tf::TransformListener aruco_1;
    tf::TransformListener aruco_2;

    // suppose we already have the position of the center marker
    // in the correct frame

    geometry_msgs::Point torso;
    geometry_msgs::Point l_hand;
    geometry_msgs::Point r_hand;
    geometry_msgs::Point base_position_in;
    geometry_msgs::Point base_position_out;
    

    // !!! YAML PARSING WORKS LIKE THIS !!!
    // YAML::Node config = YAML::LoadFile(
            // "/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml");
    // ROS_INFO("%lf", config["arm"].as<double>());

    // Set human to starting postion
    std::vector<double> center = config["cr"].as<std::vector<double> >();
    std::vector<double> l_sh = config["lsh"].as<std::vector<double> >();
    std::vector<double> r_sh = config["rsh"].as<std::vector<double> >();

    double size = config["arm"].as<double>()*2;

    torso.x = center[0];
    torso.y = center[1];
    torso.z = center[2];

    l_hand.x = center[0];
    l_hand.y = center[1]+config["arm"].as<double>();
    l_hand.z = center[2]+l_sh[2];

    r_hand.x = center[0];
    r_hand.y = center[1]-config["arm"].as<double>();
    r_hand.z = center[2]+r_sh[2];

    base_position_in.x = center[0];
    base_position_in.y = center[1];
    base_position_in.z = torso.z-0.75*size;

    base_position_out.x = center[0];
    base_position_out.y = center[1];
    base_position_out.z = torso.z-0.75*size;

    double base_in_r_x = (config["fw"].as<double>() - center[0])*0.33;
    double base_out_r_x = config["fw"].as<double>() - center[0];

    double base_in_r_y = (config["rt"].as<double>() - center[1])*0.33;
    double base_out_r_y = (config["rt"].as<double>() - center[1]);

    

    while (ros::ok())
    {

        // tried to subscribe to tf to recieve marker coordinates
        tf::StampedTransform aruco_0_tf;
        tf::StampedTransform aruco_1_tf;
        tf::StampedTransform aruco_2_tf;

        try {
            aruco_0.lookupTransform("/odom", "/Aruco_0_frame",
                    ros::Time(0), aruco_0_tf);
            ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]);
            ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]);
            ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]);

            aruco_1.lookupTransform("/odom", "/Aruco_1_frame",
                    ros::Time(0), aruco_1_tf);
            ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]);
            ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]);
            ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]);

            aruco_2.lookupTransform("/odom", "/Aruco_2_frame",
                    ros::Time(0), aruco_2_tf);
            ROS_INFO("0_2: %f",aruco_2_tf.getOrigin()[0]);
            ROS_INFO("1_2: %f",aruco_2_tf.getOrigin()[1]);
            ROS_INFO("2_2: %f",aruco_2_tf.getOrigin()[2]);

            // update torso and hand positions
            torso.x = aruco_0_tf.getOrigin()[0];
            torso.y = aruco_0_tf.getOrigin()[1];
            torso.z = aruco_0_tf.getOrigin()[2];

            l_hand.x = aruco_1_tf.getOrigin()[0];
            l_hand.y = aruco_1_tf.getOrigin()[1];
            l_hand.z = aruco_1_tf.getOrigin()[2];

            r_hand.x = aruco_2_tf.getOrigin()[0];
            r_hand.y = aruco_2_tf.getOrigin()[1];
            r_hand.z = aruco_2_tf.getOrigin()[2];

        }
        catch (tf::TransformException ex){
            ROS_INFO("%s",ex.what());
            ros::Duration(1.0).sleep();
        }

        visualization_msgs::Marker body;
        visualization_msgs::Marker head;
        visualization_msgs::Marker l_shoulder;
        visualization_msgs::Marker r_shoulder;
        visualization_msgs::Marker l_joints;
        visualization_msgs::Marker r_joints;
        visualization_msgs::Marker l_arm;
        visualization_msgs::Marker r_arm;
        visualization_msgs::Marker l_legjoints;
        visualization_msgs::Marker r_legjoints;
        visualization_msgs::Marker l_leg;
        visualization_msgs::Marker r_leg;
        visualization_msgs::Marker camera;
        visualization_msgs::Marker base_in;
        visualization_msgs::Marker base_out;

        // Set the frame ID and timestamp

        body.header.frame_id = "/odom";
        body.header.stamp = ros::Time::now();

        head.header.frame_id = "/odom";
        head.header.stamp = ros::Time::now();

        l_shoulder.header.frame_id = "/odom";
        l_shoulder.header.stamp = ros::Time::now();

        r_shoulder.header.frame_id = "/odom";
        r_shoulder.header.stamp = ros::Time::now();

        l_joints.header.frame_id = "/odom";
        l_joints.header.stamp = ros::Time::now();

        r_joints.header.frame_id = "/odom";
        r_joints.header.stamp = ros::Time::now();

        l_arm.header.frame_id = "/odom";
        l_arm.header.stamp = ros::Time::now();

        r_arm.header.frame_id = "/odom";
        r_arm.header.stamp = ros::Time::now();

        l_legjoints.header.frame_id = "/odom";
        l_legjoints.header.stamp = ros::Time::now();

        r_legjoints.header.frame_id = "/odom";
        r_legjoints.header.stamp = ros::Time::now();

        l_leg.header.frame_id = "/odom";
        l_leg.header.stamp = ros::Time::now();

        r_leg.header.frame_id = "/odom";
        r_leg.header.stamp = ros::Time::now();

        camera.header.frame_id = "/odom";
        camera.header.stamp = ros::Time::now();

        base_in.header.frame_id = "/odom";
        base_in.header.stamp = ros::Time::now();

 	base_out.header.frame_id = "/odom";
        base_out.header.stamp = ros::Time::now();


        // set namespace and id

        body.ns = "body";
        body.id = 0;

        head.ns = "head";
        head.id = 1;

        l_shoulder.ns = "l_shoulder";
        l_shoulder.id = 2;

        r_shoulder.ns = "r_shoulder";
        r_shoulder.id = 3;

        l_joints.ns = "l_joints";
        l_joints.id = 4;

        r_joints.ns = "l_joints";
        r_joints.id = 5;

        l_arm.ns = "l_arm";
        l_arm.id = 6;

        r_arm.ns = "r_arm";
        r_arm.id = 7;

        l_legjoints.ns = "l_legjoints";
        l_legjoints.id = 8;

        r_legjoints.ns = "l_legjoints";
        r_legjoints.id = 9;

        l_leg.ns = "l_leg";
        l_leg.id = 10;

        r_leg.ns = "r_leg";
        r_leg.id = 11;

        camera.ns = "camera";
        camera.id = 12;

        base_in.ns = "base_in";
        base_in.id = 13;

	base_out.ns = "base_out";
        base_out.id = 14;

        // set marker shape

        body.type = visualization_msgs::Marker::CUBE;
        head.type = visualization_msgs::Marker::SPHERE;
        l_shoulder.type = visualization_msgs::Marker::CUBE;
        r_shoulder.type = visualization_msgs::Marker::CUBE;
        l_joints.type = visualization_msgs::Marker::POINTS;
        r_joints.type = visualization_msgs::Marker::POINTS;
        l_arm.type = visualization_msgs::Marker::LINE_STRIP;
        r_arm.type = visualization_msgs::Marker::LINE_STRIP;
        l_legjoints.type = visualization_msgs::Marker::POINTS;
        r_legjoints.type = visualization_msgs::Marker::POINTS;
        l_leg.type = visualization_msgs::Marker::LINE_STRIP;
        r_leg.type = visualization_msgs::Marker::LINE_STRIP;
        camera.type = visualization_msgs::Marker::SPHERE;
        base_in.type = visualization_msgs::Marker::SPHERE;
        base_out.type = visualization_msgs::Marker::SPHERE;

        // Set the marker action

        body.action = visualization_msgs::Marker::ADD;
        head.action = visualization_msgs::Marker::ADD;
        l_shoulder.action = visualization_msgs::Marker::ADD;
        r_shoulder.action = visualization_msgs::Marker::ADD;
        l_joints.action = visualization_msgs::Marker::ADD;
        r_joints.action = visualization_msgs::Marker::ADD;
        l_arm.action = visualization_msgs::Marker::ADD;
        r_arm.action = visualization_msgs::Marker::ADD;
        l_legjoints.action = visualization_msgs::Marker::ADD;
        r_legjoints.action = visualization_msgs::Marker::ADD;
        l_leg.action = visualization_msgs::Marker::ADD;
        r_leg.action = visualization_msgs::Marker::ADD;
        camera.action = visualization_msgs::Marker::ADD;
        base_in.action = visualization_msgs::Marker::ADD;
        base_out.action = visualization_msgs::Marker::ADD;

        // Set the pose of the marker

        body.pose.position.x = torso.x;
        body.pose.position.y = torso.y;
        body.pose.position.z = torso.z-0.05*size;
        body.pose.orientation.x = 0.0;
        body.pose.orientation.y = 0.0;
        body.pose.orientation.z = 0.0;
        body.pose.orientation.w = 1.0;

        head.pose.position.x = torso.x;
        head.pose.position.y = torso.y;
        head.pose.position.z = torso.z+0.25*size;
        head.pose.orientation.x = 0.0;
        head.pose.orientation.y = 0.0;
        head.pose.orientation.z = 0.0;
        head.pose.orientation.w = 1.0;

        l_shoulder.pose.position.x = torso.x;
        l_shoulder.pose.position.y = torso.y+l_sh[1]-0.05;
        l_shoulder.pose.position.z = torso.z+l_sh[2];
        l_shoulder.pose.orientation.x = 0.0;
        l_shoulder.pose.orientation.y = 0.0;
        l_shoulder.pose.orientation.z = 0.0;
        l_shoulder.pose.orientation.w = 1.0;

        r_shoulder.pose.position.x = torso.x;
        r_shoulder.pose.position.y = torso.y+r_sh[1]+0.05;
        r_shoulder.pose.position.z = torso.z+r_sh[2];
        r_shoulder.pose.orientation.x = 0.0;
        r_shoulder.pose.orientation.y = 0.0;
        r_shoulder.pose.orientation.z = 0.0;
        r_shoulder.pose.orientation.w = 1.0;

        l_joints.pose.orientation.w = 1.0;
        r_joints.pose.orientation.w = 1.0;
        l_arm.pose.orientation.w = 1.0;
        r_arm.pose.orientation.w = 1.0;

        l_legjoints.pose.orientation.w = 1.0;
        r_legjoints.pose.orientation.w = 1.0;
        l_leg.pose.orientation.w = 1.0;
        r_leg.pose.orientation.w = 1.0;

        camera.pose.position.x = 0;
        camera.pose.position.y = 0;
        camera.pose.position.z = 0.0;
        camera.pose.orientation.x = 0.0;
        camera.pose.orientation.y = 0.0;
        camera.pose.orientation.z = 0.0;
        camera.pose.orientation.w = 0.0;

        base_in.pose.position.x = base_position_in.x;
        base_in.pose.position.y = base_position_in.y;
        base_in.pose.position.z = base_position_in.z;
        base_in.pose.orientation.x = 0.0;
        base_in.pose.orientation.y = 0.0;
        base_in.pose.orientation.z = 0.0;
        base_in.pose.orientation.w = 1.0;

	base_out.pose.position.x = base_position_out.x;
        base_out.pose.position.y = base_position_out.y;
        base_out.pose.position.z = base_position_out.z;
        base_out.pose.orientation.x = 0.0;
        base_out.pose.orientation.y = 0.0;
        base_out.pose.orientation.z = 0.0;
        base_out.pose.orientation.w = 1.0;

        // Set the scale of the marker (in meters)

        body.scale.x = 0.125*size;
        body.scale.y = 0.3*size;
        body.scale.z = 0.4*size;

        head.scale.x = 0.125*size;
        head.scale.y = 0.125*size;
        head.scale.z = 0.15*size;
        
        l_shoulder.scale.x = 0.125;
        l_shoulder.scale.y = 0.125;
        l_shoulder.scale.z = 0.125;

        r_shoulder.scale.x = 0.125;
        r_shoulder.scale.y = 0.125;
        r_shoulder.scale.z = 0.125;

        l_joints.scale.x = 0.1;
        l_joints.scale.y = 0.1;

        r_joints.scale.x = 0.1;
        r_joints.scale.y = 0.1;

        l_arm.scale.x = 0.1;
        l_arm.scale.y = 0.1;

        r_arm.scale.x = 0.1;
        r_arm.scale.y = 0.1;

        l_legjoints.scale.x = 0.1;
        l_legjoints.scale.y = 0.1;

        r_legjoints.scale.x = 0.1;
        r_legjoints.scale.y = 0.1;

        l_leg.scale.x = 0.1;
        l_leg.scale.y = 0.1;

        r_leg.scale.x = 0.1;
        r_leg.scale.y = 0.1;

        camera.scale.x = 0.2;
        camera.scale.y = 0.2;
        camera.scale.z = 0.2;

        base_in.scale.x = 2*base_in_r_x;
        base_in.scale.y = 2*base_in_r_y;
        base_in.scale.z = 0.1;

	base_out.scale.x = 2*base_out_r_x;
        base_out.scale.y = 2*base_out_r_y;
        base_out.scale.z = 0.01;

        // Set the color

        body.color.r = 0.0f;
        body.color.g = 1.0f;
        body.color.b = 0.5f;
        body.color.a = 1.0;

        head.color.r = 0.0f;
        head.color.g = 1.0f;
        head.color.b = 0.0f;
        head.color.a = 1.0;

        l_shoulder.color.r = 0.5f;
        l_shoulder.color.g = 0.2f;
        l_shoulder.color.b = 1.0f;
        l_shoulder.color.a = 1.0;

        r_shoulder.color.r = 0.5f;
        r_shoulder.color.g = 0.2f;
        r_shoulder.color.b = 1.0f;
        r_shoulder.color.a = 1.0;

        l_joints.color.r = 0.5f;
        l_joints.color.g = 0.2f;
        l_joints.color.b = 1.0f;
        l_joints.color.a = 1.0;

        r_joints.color.r = 0.5f;
        r_joints.color.g = 0.2f;
        r_joints.color.b = 1.0f;
        r_joints.color.a = 1.0;

        l_arm.color.r = 0.5f;
        l_arm.color.g = 0.2f;
        l_arm.color.b = 1.0f;
        l_arm.color.a = 1.0;

        r_arm.color.r = 0.5f;
        r_arm.color.g = 0.2f;
        r_arm.color.b = 1.0f;
        r_arm.color.a = 1.0;

        l_legjoints.color.r = 0.5f;
        l_legjoints.color.g = 0.2f;
        l_legjoints.color.b = 1.0f;
        l_legjoints.color.a = 1.0;

        r_legjoints.color.r = 0.5f;
        r_legjoints.color.g = 0.2f;
        r_legjoints.color.b = 1.0f;
        r_legjoints.color.a = 1.0;

        l_leg.color.r = 0.5f;
        l_leg.color.g = 0.2f;
        l_leg.color.b = 1.0f;
        l_leg.color.a = 1.0;

        r_leg.color.r = 0.5f;
        r_leg.color.g = 0.2f;
        r_leg.color.b = 1.0f;
        r_leg.color.a = 1.0;

        camera.color.r = 0.5f;
        camera.color.g = 0.5f;
        camera.color.b = 0.5f;
        camera.color.a = 1.0;

        base_in.color.r = 1.0f;
        base_in.color.g = 0.0f;
        base_in.color.b = 0.0f;
        base_in.color.a = 1.0;

	base_out.color.r = 0.0f;
        base_out.color.g = 1.0f;
        base_out.color.b = 0.5f;
        base_out.color.a = 1.0;

        // set lifetime

        body.lifetime = ros::Duration();
        head.lifetime = ros::Duration();
        l_shoulder.lifetime = ros::Duration();
        r_shoulder.lifetime = ros::Duration();
        l_joints.lifetime = ros::Duration();
        r_joints.lifetime = ros::Duration();
        l_arm.lifetime = ros::Duration();
        r_arm.lifetime = ros::Duration();
        l_legjoints.lifetime = ros::Duration();
        r_legjoints.lifetime = ros::Duration();
        l_leg.lifetime = ros::Duration();
        r_leg.lifetime = ros::Duration();
        camera.lifetime = ros::Duration();
        base_in.lifetime = ros::Duration();
	base_out.lifetime = ros::Duration();

        // create lines
        // create line for arms

        geometry_msgs::Point l_shoulder_point;
        geometry_msgs::Point r_shoulder_point;

        l_shoulder_point.x = torso.x;
        l_shoulder_point.y = torso.y+l_sh[1];
        l_shoulder_point.z = torso.z+l_sh[2];

        r_shoulder_point.x = torso.x;
        r_shoulder_point.y = torso.y+r_sh[1];
        r_shoulder_point.z = torso.z+r_sh[2];

        l_joints.points.push_back(l_shoulder_point);
        l_joints.points.push_back(l_hand);
        l_arm.points.push_back(l_shoulder_point);
        l_arm.points.push_back(l_hand);

        r_joints.points.push_back(r_shoulder_point);
        r_joints.points.push_back(r_hand);
        r_arm.points.push_back(r_shoulder_point);
        r_arm.points.push_back(r_hand);

        // create line for legs

        geometry_msgs::Point l_hip;
        geometry_msgs::Point r_hip;
        geometry_msgs::Point l_knee;
        geometry_msgs::Point r_knee;
        geometry_msgs::Point l_foot;
        geometry_msgs::Point r_foot;

        l_hip.x = torso.x;
        l_hip.y = torso.y+0.1;
        l_hip.z = torso.z-0.25*size;

        r_hip.x = torso.x;
        r_hip.y = torso.y-0.1;
        r_hip.z = torso.z-0.25*size;

        l_knee.x = torso.x;
        l_knee.y = torso.y+0.1;
        l_knee.z = torso.z-0.45*size;

        r_knee.x = torso.x;
        r_knee.y = torso.y-0.1;
        r_knee.z = torso.z-0.45*size;

        l_foot.x = torso.x;
        l_foot.y = torso.y+0.1;
        l_foot.z = torso.z-0.70*size;

        r_foot.x = torso.x;
        r_foot.y = torso.y-0.1;
        r_foot.z = torso.z-0.70*size;

        // push to points and lines

        l_legjoints.points.push_back(l_hip);
        l_legjoints.points.push_back(l_knee);
        l_legjoints.points.push_back(l_foot);

        l_leg.points.push_back(l_hip);
        l_leg.points.push_back(l_knee);
        l_leg.points.push_back(l_foot);

        r_legjoints.points.push_back(r_hip);
        r_legjoints.points.push_back(r_knee);
        r_legjoints.points.push_back(r_foot);

        r_leg.points.push_back(r_hip);
        r_leg.points.push_back(r_knee);
        r_leg.points.push_back(r_foot);


        // Publish the markers

        while (marker_pub.getNumSubscribers() < 1)
        {
            if (!ros::ok())
            {
                return 0;
            }
            ROS_WARN_ONCE("Please create a subscriber to the marker");
            sleep(1);
        }

        marker_pub.publish(body);
        marker_pub.publish(head);
        marker_pub.publish(l_shoulder);
        marker_pub.publish(r_shoulder);
        marker_pub.publish(l_joints);
        marker_pub.publish(r_joints);
        marker_pub.publish(l_arm);
        marker_pub.publish(r_arm);
        marker_pub.publish(l_legjoints);
        marker_pub.publish(r_legjoints);
        marker_pub.publish(l_leg);
        marker_pub.publish(r_leg);
        marker_pub.publish(camera);
        marker_pub.publish(base_in);
	marker_pub.publish(base_out);

        r.sleep();
    }
}
