#include "sync_board.hpp"
#include <iostream>
#include <chrono>
#include <thread>

SyncBoard::SyncBoard() {}
SyncBoard::~SyncBoard() { stop_recv_message(); disconnect(); }

bool SyncBoard::connect(const std::string &port, int baudrate) {
    if (!serial_.openPort(port, baudrate)) return false;

    mavlink_handler_ = new MavlinkHandler(serial_);
    mavlink_handler_->setHeartbeatCallback([this](bool ok) {
        timesync_ok_ = ok;
        if (ok) std::cout << "[TIMESYNC] SUCCESS" << std::endl;
        else std::cout << "[TIMESYNC] IN PROGRESS" << std::endl;
    });

    return true;
}

void SyncBoard::start_recv_message() {
    running_ = true;
    recv_thread_ = std::thread([this]() {
        while (running_) {
            mavlink_handler_->processIncoming();
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    });
}

void SyncBoard::read_system_time(){
    mavlink_handler_->setSystemtimeCallback([this](uint64_t time_unix_usec){
        timestamp = time_unix_usec;
        // std::cout << "[System Time Callback] MAVLink time: " << time_unix_usec << std::endl;
    });
}

void SyncBoard::read_camera_trigger(){
    mavlink_handler_->setCameratriggerCallback([this](uint64_t time_usec){
        std::cout << "[Camera Trigger] Camera triggered at time: " << time_usec << " us" << std::endl;
        std::lock_guard<std::mutex> lock(camera_ts_mutex_);

        if(camera_ts_queue_.size() >= MAX_QUEUE_SIZE) {
            camera_ts_queue_.pop();
        }
        camera_ts_queue_.push(time_usec);
    });
}

bool SyncBoard::pop_timestamp(uint64_t &cam_ts) {
    std::lock_guard<std::mutex> lock(camera_ts_mutex_);
    if(camera_ts_queue_.empty()) return false;
    cam_ts = camera_ts_queue_.front();
    camera_ts_queue_.pop();
    return true;
}

void SyncBoard::read_highres_imu(){
    mavlink_handler_->setImuCallback([this](uint64_t ts,
                                        float ax, float ay, float az,
                                        float gx, float gy, float gz){
        this->onImuData(ts, ax, ay, az, gx, gy, gz);
    });
}

void SyncBoard::onImuData(uint64_t ts, float ax, float ay, float az, float gx, float gy, float gz) {
    std::lock_guard<std::mutex> lock(imu_mutex_);
    if (imu_buffer_.size() >= imu_buffer_max_) {
        imu_buffer_.pop_front();  // 移除最旧的数据
    }
    imu_buffer_.push_back({ts, ax, ay, az, gx, gy, gz});
}

bool SyncBoard::getLatestImu(ImuData &data) {
    std::lock_guard<std::mutex> lock(imu_mutex_);
    if (imu_buffer_.empty()) return false;
    data = imu_buffer_.back();
    return true;
}

void SyncBoard::CamOneShotTrigger()
{
    MavlinkSender sender(serial_);
    sender.sendCameraTrigger(1, 0.0f, 1, 0);
}

void SyncBoard::SetCamFrame(int fps)
{
    MavlinkSender sender(serial_);
    float interval_ms = 1000.0f / fps;
    sender.sendCameraTrigger(0, interval_ms, 0, 0);
}

void SyncBoard::SetTimeSource(int time_source)
{
    MavlinkSender sender(serial_);
    sender.sendTimeSource(time_source);
}
void SyncBoard::upgradeFirmware(const std::string& upgrade_file_path)
{
    MavlinkSender sender(serial_);
    sender.upgradeProcess(upgrade_file_path);
}

void SyncBoard::disconnect() {
    if (serial_.isOpen()) serial_.closePort();
    if (mavlink_handler_) {
        delete mavlink_handler_;
        mavlink_handler_ = nullptr;
    }
}

void SyncBoard::stop_recv_message() {
    running_ = false;
    if (recv_thread_.joinable()) recv_thread_.join();
}