#include <ignition/msgs.hh>
#include <ignition/transport/Node.hh>
#include <ignition/msgs/annotated_axis_aligned_2d_box_v.pb.h>

#include <iostream>
#include <cmath>
#include <chrono>
#include <thread>

// Settings
const std::string world_name = "empty";
const std::string model_name = "blue_cone";

const int timeout_ms = 10;

double x = 0;
double y = 0;
double z = 0;

double roll = 0;
double pitch = 0;
double yaw = 0;

// init semaphore
pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t bbox_cond = PTHREAD_COND_INITIALIZER;
bool pos_changed = true;

void callback_bbox(const ignition::msgs::AnnotatedAxisAligned2DBox_V &_msg)
{
    if (pos_changed)
    {
        pthread_mutex_lock(&lock);
        for (const auto & gz_box : _msg.annotated_box()) {
            if (gz_box.label() == 5) {
                auto box = gz_box.box();
                auto min_corner = box.min_corner();
                auto max_corner = box.max_corner();

                // write to csv file
                std::ofstream outfile;
                outfile.open("cone_pos_bbox.csv", std::ios_base::app);
                outfile << x << "," << y << "," << z << "," << roll << "," << pitch << "," << yaw << "," << min_corner.x() << "," << min_corner.y() << "," << max_corner.x() << "," << max_corner.y() << "\n";
                outfile.close();
            }
        }
        pos_changed = false;
        pthread_cond_signal (&bbox_cond);
        pthread_mutex_unlock(&lock);
    }
    
  

}


int main()
{
    // Connect to the Ignition Transport node
    ignition::transport::Node node;

    // Set service details
    const std::string service = "/world/" + world_name + "/set_pose";
    const std::string reqtype = "ignition.msgs.Pose";
    const std::string reptype = "ignition.msgs.Boolean";

    int min_circle_radius = 1;
    int max_circle_radius = 40;
    int min_angle = 60;
    int max_angle = 210;

    // listen for bounding box message
    std::string bbox_topic = "/box";
    node.Subscribe(bbox_topic, callback_bbox); 

    for (int i = 0; i < 1000; i++)
    {   
        std::cout << "Iteration: " << i << std::endl;
        while (pos_changed)
            pthread_cond_wait (&bbox_cond, &lock);
        // random float between 1 and 40
        float cur_circle_radius = min_circle_radius + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max_circle_radius - min_circle_radius)));
        // random float between min_angle and max_angle
        float random_degree = min_angle + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(max_angle - min_angle)));
        float random_radians = random_degree * M_PI / 180;
        
        x = cur_circle_radius * cos(random_radians);
        y = cur_circle_radius * sin(random_radians);
        z = 0;

        // rotate cone randomly around z axis
        yaw = 0;
        pitch = 0;
        roll = (double)rand() / RAND_MAX * 2 * M_PI;

        // create request message
        ignition::msgs::Vector3d vector3d_msg;
        vector3d_msg.set_x(x);
        vector3d_msg.set_y(y);
        vector3d_msg.set_z(z);

        ignition::msgs::Quaternion quat_msg;
        quat_msg.set_w(cos(yaw / 2) * cos(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
        quat_msg.set_x(sin(yaw / 2) * cos(pitch / 2) * cos(roll / 2) - cos(yaw / 2) * sin(pitch / 2) * sin(roll / 2));
        quat_msg.set_y(cos(yaw / 2) * sin(pitch / 2) * cos(roll / 2) + sin(yaw / 2) * cos(pitch / 2) * sin(roll / 2));
        quat_msg.set_z(cos(yaw / 2) * cos(pitch / 2) * sin(roll / 2) - sin(yaw / 2) * sin(pitch / 2) * cos(roll / 2));

        ignition::msgs::Pose pose_msg;
        pose_msg.set_name(model_name);
        pose_msg.mutable_position()->CopyFrom(vector3d_msg);
        pose_msg.mutable_orientation()->CopyFrom(quat_msg);
        bool result = false;
        ignition::msgs::Boolean result_msg;

        // submit request (blocking)
        while(!result){
            result = node.Request(service, pose_msg, timeout_ms, result_msg, result);

        if (!result)
        {
            std::cout << "Update failed" << std::endl;
        }
        }
        pos_changed = true;
        pthread_mutex_unlock(&lock);
    }

    

    return 0;
}
