#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;
const double update_rate_hz = 100.0;

// Offsets
double pose_offset_x = 0.0;
double pose_offset_y = 0.0;
double pose_offset_z = 0.17;

double x = pose_offset_x;
double y = pose_offset_y;
double z = pose_offset_z;

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

void callback_bbox(const ignition::msgs::AnnotatedAxisAligned2DBox_V &_msg)
{
    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();
            // save bbox data and cone pose to csv file
            std::cout << "Bounding box: " << min_corner.x() << ", " << min_corner.y() << ", " << max_corner.x() << ", " << max_corner.y() << std::endl;

            // save cone pose to csv file
            std::cout << "Cone pose: " << x << ", " << y << ", " << z << std::endl;

            // 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();
            
        }
  }

}

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

    // Configure the update loop
    auto start_time = std::chrono::steady_clock::now();
    double update_period_s = 1.0 / update_rate_hz;
    auto last_update_time = start_time;


    // move cone in circles around the origin and rotate the cone randomly
    // after setting the position, wait a short duration and listen for a bounding box message
    // write current cone position and orientation and bounding box to csv file
    // repeat

    int circle_resolution = 100;
    int circles = 100;
    int min_circle_radius = 1;
    int max_circle_radius = 40;

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

    for (int i = 0; i < circles; i++)
    {
        for (int j = 0; j < circle_resolution; j++)
        {
            // move cone in rotation around origin with sin and cos
            double cur_circle_radius = min_circle_radius + (max_circle_radius - min_circle_radius) * (double)i / (double)circles;
            x = cur_circle_radius * cos(j * 2 * M_PI / circle_resolution);
            y = cur_circle_radius * sin(j * 2 * M_PI / circle_resolution);
            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;
            ignition::msgs::Boolean result_msg;

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

            if (!result)
            {
                std::cout << "Update failed" << std::endl;
            }

            // sleep for a short duration to avoid
            std::this_thread::sleep_for(std::chrono::milliseconds(100));           
        }
    }

    

    return 0;
}
