#include <iostream>
#include <vector>
#include <cmath>
#include <chrono>
#include <thread>
#include <rec/robotino/api2/all.h>
#include <boost/asio.hpp>

struct Point
{
    double x, y;
    Point(double x = 0, double y = 0) : x(x), y(y) {}
};

class HokuyoURG
{
private:
    boost::asio::io_service io;
    boost::asio::serial_port serial;
    std::vector<uint32_t> distances;

public:
    HokuyoURG(const std::string &port, unsigned int baud_rate)
        : serial(io, port)
    {
        serial.set_option(boost::asio::serial_port_base::baud_rate(baud_rate));
    }

    bool initialize()
    {
        write("SCIP2.0\n");
        std::string response = read();
        return response.find("SCIP2.0") != std::string::npos;
    }

    std::vector<Point> scan()
    {
        write("GD0000108000\n"); // Request scan from step 0 to step 1080
        std::string response = read();

        std::vector<Point> scan_points;

        // Check if the response starts with "GD"
        if (response.substr(0, 2) != "GD")
        {
            std::cerr << "Invalid response from LIDAR" << std::endl;
            return scan_points;
        }

        // Skip header (first two lines)
        size_t data_start = response.find('\n');
        data_start = response.find('\n', data_start + 1);
        data_start++; // Move past the newline

        // Parse the data
        const double STEP_ANGLE = 0.25 * M_PI / 180.0;    // 0.25 degrees in radians
        const double START_ANGLE = -135.0 * M_PI / 180.0; // -135 degrees in radians

        for (size_t i = 0; i < 1081; ++i)
        {
            if (data_start + 3 > response.length())
                break;

            // Extract 3 characters for each measurement
            std::string encoded_dist = response.substr(data_start, 3);
            data_start += 3;

            // Decode the distance
            int distance = (encoded_dist[0] - 0x30) << 12 | (encoded_dist[1] - 0x30) << 6 | (encoded_dist[2] - 0x30);

            // Skip invalid measurements
            if (distance == 0)
                continue;

            // Convert polar coordinates to Cartesian
            double angle = START_ANGLE + i * STEP_ANGLE;
            double x = distance * cos(angle) / 1000.0; // Convert to meters
            double y = distance * sin(angle) / 1000.0; // Convert to meters

            scan_points.emplace_back(x, y);
        }

        return scan_points;
    }

private:
    void write(const std::string &data)
    {
        boost::asio::write(serial, boost::asio::buffer(data));
    }

    std::string read()
    {
        char c;
        std::string result;
        while (boost::asio::read(serial, boost::asio::buffer(&c, 1)))
        {
            if (c == '\n')
                break;
            result += c;
        }
        return result;
    }
};

class RobotinoRobot : public rec::robotino::api2::Com
{
private:
    rec::robotino::api2::OmniDrive omniDrive;
    rec::robotino::api2::Odometry odometry;
    HokuyoURG *lidar;
    Point goal;
    std::vector<Point> obstacles;
    double attractiveConst = 1.0;
    double repulsiveConst = 100.0;
    double influenceRange = 0.5; // in meters
    double maxVelocity = 0.2;    // in m/s

public:
    RobotinoRobot(const std::string &hostname, Point goal, HokuyoURG *lidar)
        : goal(goal), lidar(lidar)
    {
        setAddress(hostname.c_str());
    }

    void updateLidarData()
    {
        obstacles = lidar->scan();
    }

    Point calculateAttractivePotential()
    {
        float x, y, phi;
        odometry.readings(&x, &y, &phi);
        double dx = goal.x - x;
        double dy = goal.y - y;
        double distance = std::sqrt(dx * dx + dy * dy);
        return Point(attractiveConst * dx / distance, attractiveConst * dy / distance);
    }

    Point calculateRepulsivePotential()
    {
        Point total(0, 0);
        float robot_x, robot_y, robot_phi;
        odometry.readings(&robot_x, &robot_y, &robot_phi);

        for (const auto &obstacle : obstacles)
        {
            double dx = robot_x - obstacle.x;
            double dy = robot_y - obstacle.y;
            double distance = std::sqrt(dx * dx + dy * dy);

            if (distance < influenceRange)
            {
                double magnitude = repulsiveConst * (1 / distance - 1 / influenceRange) / (distance * distance);
                total.x += magnitude * dx / distance;
                total.y += magnitude * dy / distance;
            }
        }
        return total;
    }

    void move()
    {
        updateLidarData();
        Point attractive = calculateAttractivePotential();
        Point repulsive = calculateRepulsivePotential();

        double vx = attractive.x + repulsive.x;
        double vy = attractive.y + repulsive.y;

        // Normalize and scale velocity
        double magnitude = std::sqrt(vx * vx + vy * vy);
        if (magnitude > maxVelocity)
        {
            vx = (vx / magnitude) * maxVelocity;
            vy = (vy / magnitude) * maxVelocity;
        }

        // Robotino uses a different coordinate system, so we need to adjust
        omniDrive.setVelocity(vy, -vx, 0); // x and y are swapped, and x is negated

        float x, y, phi;
        odometry.readings(&x, &y, &phi);
        std::cout << "Robot at (" << x << ", " << y << "), φ = " << phi << std::endl;
    }

    bool reachedGoal()
    {
        float x, y, phi;
        odometry.readings(&x, &y, &phi);
        double dx = goal.x - x;
        double dy = goal.y - y;
        return std::sqrt(dx * dx + dy * dy) < 0.1;
    }

    void errorEvent(const char *errorString) override
    {
        std::cerr << "Error: " << errorString << std::endl;
    }

    void connectedEvent() override
    {
        std::cout << "Connected to Robotino" << std::endl;
    }

    void connectionClosedEvent() override
    {
        std::cout << "Connection to Robotino closed" << std::endl;
    }
};

int main()
{
    HokuyoURG lidar("/dev/ttyACM0", 115200);
    if (!lidar.initialize())
    {
        std::cerr << "Failed to initialize LIDAR" << std::endl;
        return 1;
    }

    RobotinoRobot robot("172.26.1.1", Point(10, 15), &lidar); // Replace IP with your Robotino's IP

    if (!robot.connect())
    {
        std::cerr << "Failed to connect to Robotino" << std::endl;
        return 1;
    }

    while (!robot.reachedGoal())
    {
        robot.move();
        std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 10 Hz update rate
    }

    robot.omniDrive.setVelocity(0, 0, 0); // Stop the robot
    std::cout << "Goal reached!" << std::endl;

    robot.disconnect();
    return 0;
}