/**
 * @file gamepad_interface.hpp
 * @brief gamepad
 * @author DeepRobotics
 * @version 1.0
 * @date 2026-01-07
 *
 * @copyright Copyright (c) 2025  DeepRobotics
 *
 */

#pragma once

#include <arpa/inet.h>
#include <unistd.h>

#include <atomic>
#include <cstdint>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <list>
#include <set>
#include <sstream>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "common_types.h"
#include "custom_types.h"
#include "heartbeat.hpp"
#include "json.hpp"
#include "user_command_interface.h"

using json = nlohmann::json;

class GamepadInterface : public UserCommandInterface {
public:
    GamepadInterface(RobotName robot_name) : UserCommandInterface(robot_name) {
        mHeartbeat_ = std::make_unique<Heartbeat>();

        mHandleMap_[100][100] = std::bind(&GamepadInterface::Handle_Heartbeat, this, std::placeholders::_1,
                                          std::placeholders::_2, std::placeholders::_3);
        mHandleMap_[2][21] = std::bind(&GamepadInterface::Handle_Steer, this, std::placeholders::_1,
                                       std::placeholders::_2, std::placeholders::_3);
        mHandleMap_[1101][12] = std::bind(&GamepadInterface::Handle_Key, this, std::placeholders::_1,
                                          std::placeholders::_2, std::placeholders::_3);
    }

    ~GamepadInterface() {}

    void Start() override {
        std::cout << "\n╔════════════════════════════════════════════════╗\n"
                  << "║                GAMEPAD TELEOP                  ║\n"
                  << "╚════════════════════════════════════════════════╝\n"
                  << "  Movement:  Left joystick\n"
                  << "  Rotation:  Right joystick\n"
                  << "  Mode:      R2 (damping)  L1 (stand)  L2 (control)\n"
                  << "\n";
    }

    void Stop() override {}
    UserCommand* GetUserCommand() override { return usr_cmd_; }
    int ParseHeader(const char* buffer, int len) {
        const int headerSize = 16;
        if (len < headerSize) return -1;

        if ((uint8_t)buffer[0] == 0xeb && (uint8_t)buffer[2] == 0xeb && (uint8_t)buffer[3] == 0x90) {
            if ((uint8_t)buffer[1] != 0x90 && (uint8_t)buffer[1] != 0x91) {
                std::cout<<"Header format error"<<std::endl;
                return -1;
            }
            int bodyLen = ((uint16_t)buffer[5] << 8) | (uint8_t)buffer[4];
            return bodyLen + headerSize;
        }
        std::cout<<"Header magic error"<<std::endl;
        return -1;
    }

    bool ParseHeader(const char* buffer, int len, int& data_id, char* body, FromType& fromType) {
        const int headerSize = 16;
        if (len < headerSize) return false;

        if ((uint8_t)buffer[0] == 0xeb && (uint8_t)buffer[2] == 0xeb && (uint8_t)buffer[3] == 0x90) {
            if ((uint8_t)buffer[1] == 0x90) {
                fromType = FromType::inside;
            } else if ((uint8_t)buffer[1] == 0x91) {
                fromType = FromType::user;
            } else {
                return false;
            }

            int bodyLen = ((uint16_t)buffer[5] << 8) | (uint8_t)buffer[4];
            data_id = ((uint16_t)buffer[7] << 8) | (uint8_t)buffer[6];

            if (bodyLen != len - headerSize) {
                std::cout << "recv lost! bodyLen=" << bodyLen << " real=" << len - headerSize << std::endl;
                return false;
            }

            uint8_t dateFlag = buffer[8] & 0x03;
            if (dateFlag != 0x01) {
                std::cout << "Data format error" << std::endl;
                return false;
            }

            memcpy(body, buffer + headerSize, bodyLen);
            body[bodyLen] = '\0';
        } else {
            std::cout << "Header sync error" << std::endl;
            return false;
        }
        return true;
    }

    bool ParseData(const DataInfo& dataInfo, std::string& response) {
        int type = 0, command = 0;
        bool ret = false;

        mHeartbeat_->isConntected_ = true;
        mHeartbeat_->lastHeartbeatTime_ = std::chrono::steady_clock::now();

        json document;
        json items;

        ret = ParseJson(dataInfo.buffer, dataInfo.len, document, type, command, items);
        if (!ret) {
            std::cout<<"JSON parse failed"<<std::endl;
            response = "JSON format parsing failed";
            return false;
        }

        CmdInfo cmd_info;
        cmd_info.ip = dataInfo.ip;
        cmd_info.port = dataInfo.port;
        cmd_info.fromType = dataInfo.fromType;
        cmd_info.type = type;
        cmd_info.command = command;

        if (mHandleMap_[type][command]) {
            mHandleMap_[type][command](items, response, cmd_info);
        } else {
            response = buildResponse(cmd_info, 1, "Unknown command");
        }
        return true;
    }

    void AddDataHeader(const std::string& in, std::string& out, int data_id = 0, NetInfo perNetInfo = {0}) {
        out.resize(16);

        out[0] = 0xeb;
        if (perNetInfo.fromType == FromType::inside) {
            out[1] = 0x90;
        } else {
            out[1] = 0x91;
        }
        out[2] = 0xeb;
        out[3] = 0x90;

        auto size = in.size();

        out[4] = (uint8_t)(size & 0xFF);
        out[5] = (uint8_t)((size >> 8) & 0xFF);

        out[6] = (uint8_t)(data_id & 0xFF);
        out[7] = (uint8_t)((data_id >> 8) & 0xFF);

        out[8] = 1;

        for (int i = 9; i < 16; i++) out[i] = 0x00;

        out += in;
    }

private:
    std::unique_ptr<Heartbeat> mHeartbeat_;

    using HANDLE_FUNC = std::function<void(json& item, std::string& res, const CmdInfo& cmd_info)>;

    std::unordered_map<int, std::unordered_map<int, HANDLE_FUNC>> mHandleMap_;

    bool ParseJson(const char* buf, int len, json& document, int& type, int& command, json& items) {
        if (!buf || len == 0) return false;

        try {
            document = json::parse(buf, buf + len);

            if (!document.is_object() || !document.contains("PatrolDevice"))
                throw std::runtime_error("No PatrolDevice");

            json& root = document["PatrolDevice"];
            if (!root.is_object()) throw std::runtime_error("PatrolDevice not object");

            if (!root.contains("Items")) throw std::runtime_error("No Items");

            items = root["Items"];

            if (!root.contains("Type") || !root.contains("Command")) throw std::runtime_error("No Type/Command");

            if (!root["Type"].is_number_integer() || !root["Command"].is_number_integer())
                throw std::runtime_error("Type/Command not Int");

            type = root["Type"].get<int>();
            command = root["Command"].get<int>();

        } catch (const json::parse_error& e) {
            std::cerr << "JSON Parse Error: " << e.what() << std::endl;
            return false;
        } catch (const std::exception& e) {
            std::cerr << "JSON Logic Error: " << e.what() << std::endl;
            return false;
        }
        return true;
    }

    void JSONRootToBroker(const int& type, const int& command, json& root, bool isNew = false) {
        if (isNew) {
            root = json::object();
            root["PatrolDevice"] = {
                {"Items", json::object()}, {"Type", type}, {"Command", command}, {"Time", getCurrentDateTime()}};
        } else {
            root["PatrolDevice"]["Type"] = type;
            root["PatrolDevice"]["Command"] = command;
            root["PatrolDevice"]["Time"] = getCurrentDateTime();
        }
    }

    std::string buildResponse(const CmdInfo& cmd_info, const int& errorCode, std::string errorMsg = "") {
        std::string res;
        json document;

        JSONRootToBroker(cmd_info.type, cmd_info.command, document, true);

        document["PatrolDevice"]["Items"]["ErrorCode"] = errorCode;

        if (!errorMsg.empty()) {
            document["PatrolDevice"]["Items"]["ErrorMessage"] = errorMsg;
        }

        res = document.dump();
        return res;
    }

    void Handle_Heartbeat(json& items, std::string& res, const CmdInfo& cmd_info) {
        NetInfo netInfo;
        netInfo.ip = cmd_info.ip;
        netInfo.port = cmd_info.port;
        netInfo.fromType = cmd_info.fromType;
        mHeartbeat_->wakeUp(netInfo);

        res = buildResponse(cmd_info, 0);
    }

    void Handle_Steer(json& items, std::string& res, const CmdInfo& cmd_info) {
        if (!items.contains("X") || !items.contains("Y")) {
            res = buildResponse(cmd_info, 2, "Missing fields");
            return;
        }

        try {
            if (items["X"].is_number()) usr_cmd_->forward_vel_scale = items["X"].get<float>();
            if (items["Y"].is_number()) usr_cmd_->side_vel_scale = items["Y"].get<float>();
            if (items.contains("Yaw") && items["Yaw"].is_number())
                usr_cmd_->turnning_vel_scale = items["Yaw"].get<float>();
        } catch (const std::exception& e) {
            res = buildResponse(cmd_info, 3, "Data type error");
            return;
        }

        static std::string LastPerIp = "";
        static int64_t LastPerTime = 0;
        auto now = std::chrono::steady_clock::now();
        auto timestamp = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();

        if (LastPerIp == "" || LastPerIp == cmd_info.ip) {
            LastPerIp = cmd_info.ip;
            LastPerTime = timestamp;
        } else if (timestamp - LastPerTime > 2) {
            LastPerIp = cmd_info.ip;
            LastPerTime = timestamp;
        } else {
            std::cout << "IP Conflict" << std::endl;
            res = buildResponse(cmd_info, 4, "IP Conflict");
            return;
        }
    }

    void Handle_Key(json& items, std::string& res, const CmdInfo& cmd_info) {
        static std::unordered_map<std::string, KeyCode> keyMap = {{"G20_KEY_L1", KeyCode::L1},
                                                                  {"G20_KEY_L2", KeyCode::L2},
                                                                  {"G20_KEY_R1", KeyCode::R1},
                                                                  {"G20_KEY_R2", KeyCode::R2}};

        if (items.contains("KeyList") && items["KeyList"].is_array()) {
            for (const auto& key : items["KeyList"]) {
                if (key.is_string()) {
                    std::string keyStr = key.get<std::string>();

                    if (keyMap.count(keyStr)) {
                        KeyCode code = keyMap[keyStr];
                        switch (code) {
                            case KeyCode::L1:
                                if (msfb_->GetCurrentState() == RobotMotionState::WaitingForStand 
                                    || msfb_->GetCurrentState() == RobotMotionState::LieDown) {
                                    usr_cmd_->target_mode = uint8_t(RobotMotionState::StandingUp);
                                    std::cout << "[MODE] Standing Up\n";
                                }
                                break;
                            case KeyCode::L2:
                                if (msfb_->GetCurrentState() == RobotMotionState::StandingUp) {
                                    usr_cmd_->target_mode = uint8_t(RobotMotionState::RLControlMode);
                                    std::cout << "[MODE] RL Control\n";
                                }
                                break;
                            case KeyCode::R1:
                                if (msfb_->GetCurrentState() == RobotMotionState::StandingUp
                                    || msfb_->GetCurrentState() == RobotMotionState::RLControlMode) {
                                    usr_cmd_->target_mode = uint8_t(RobotMotionState::LieDown);
                                    std::cout << "[MODE] Lie Down\n";
                                }
                                break;
                            case KeyCode::R2:
                                usr_cmd_->target_mode = uint8_t(RobotMotionState::JointDamping);
                                std::cout << "[MODE] Joint Damping\n";
                                break;
                            default:
                                break;
                        }
                    }
                }
            }
        }
        res = buildResponse(cmd_info, 0);
    }

    std::string getCurrentDateTime() {
        auto now = std::chrono::system_clock::now();
        std::time_t currentTime = std::chrono::system_clock::to_time_t(now);
        std::tm* localTime = std::localtime(&currentTime);
        auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
        std::stringstream ss;
        ss << std::put_time(localTime, "%Y-%m-%d %H:%M:%S") << "." << std::setw(3) << std::setfill('0') << ms.count();
        return ss.str();
    }
};