#include "fgc/MtiImuSource.h"

#include "fgc/Logger.h"
#include "fgc/MtiProtocol.h"

#include <array>
#include <atomic>
#include <chrono>
#include <functional>
#include <mutex>
#include <thread>

#include <boost/asio.hpp>
#include <termios.h>
#include <unistd.h>

namespace fgc {

struct MtiImuSource::Impl {
    Impl(std::string dev, unsigned int b)
        : device(std::move(dev)), baud(b), serial(io),
          framer([this](uint8_t mid, const uint8_t* d, std::size_t n) { onFrame(mid, d, n); }) {}

    using clock = std::chrono::steady_clock;

    std::string              device;
    unsigned int             baud;
    boost::asio::io_context  io;
    boost::asio::serial_port serial;
    std::thread              io_thread;
    std::array<uint8_t, 256> rxbuf{};

    std::mutex        mutex;
    ImuSample         latest;
    clock::time_point last_rx{};
    std::atomic<bool> open{false};

    ImuDeviceConfig   cfg;          // device config read back at startup
    bool              have_cfg = false;

    MtiFramer framer;
    unsigned  bad_len_warned = 0;

    void onFrame(uint8_t mid, const uint8_t* d, std::size_t n) {
        if (mid != kMidMTData) return;  // ignore acks/other during streaming
        auto s = parseMTData(mid, d, n);
        if (!s) {
            if (bad_len_warned++ < 1)
                LOG_WARN << "MTi: unexpected MTData length " << n << " (expected "
                         << int(kMTDataLen) << "); device may not be in the configured mode";
            return;
        }
        std::lock_guard<std::mutex> lock(mutex);
        latest  = *s;
        last_rx = clock::now();
    }

    void doRead() {
        serial.async_read_some(
            boost::asio::buffer(rxbuf),
            [this](const boost::system::error_code& ec, std::size_t n) {
                if (ec) {
                    if (ec != boost::asio::error::operation_aborted)
                        LOG_WARN << "MTi serial read failed: " << ec.message();
                    return;
                }
                framer.feed(rxbuf.data(), n);
                doRead();
            });
    }

    // Synchronous, best-effort config handshake (no ack parsing): runs on the
    // calling thread BEFORE the io_thread starts, so there is no concurrent
    // access to the serial port. Small delays let the device switch states.
    // If set_profile >= 0, also selects that XKF profile while in Config state
    // (persists to non-volatile memory).
    void configure(int set_profile = -1) {
        using namespace std::chrono_literals;
        auto write = [this](const std::vector<uint8_t>& m) {
            boost::system::error_code ec;
            boost::asio::write(serial, boost::asio::buffer(m), ec);
            if (ec) LOG_WARN << "MTi config write failed: " << ec.message();
        };
        write(msgGoToConfig());        std::this_thread::sleep_for(60ms);
        if (set_profile >= 0) {
            write(msgSetFilterProfile(static_cast<uint16_t>(set_profile)));
            std::this_thread::sleep_for(60ms);
        }
        write(msgSetOutputMode());     std::this_thread::sleep_for(60ms);
        write(msgSetOutputSettings());  std::this_thread::sleep_for(60ms);
        // Read back the device configuration while still in Config State.
        queryConfig(write);
        write(msgGoToMeasurement());    std::this_thread::sleep_for(60ms);
        // Drop any pre-config (old-format) bytes so the framer starts clean.
        ::tcflush(serial.native_handle(), TCIFLUSH);
    }

    // Pause the streaming io_thread, re-run the handshake (optionally setting an
    // XKF profile), then resume streaming. Cancels + drains the outstanding read
    // first so it can't steal the config-ack bytes during the re-query. Called
    // from the control thread (no concurrent serial access). Used by refreshConfig
    // and setFilterProfile.
    void restartWithConfig(int set_profile) {
        io.stop();
        if (io_thread.joinable()) io_thread.join();
        io.restart();
        boost::system::error_code ec;
        serial.cancel(ec);
        static_cast<void>(io.run());  // run the aborted read handler (no reschedule)
        io.restart();
        configure(set_profile);
        doRead();
        io_thread = std::thread([this] { io.run(); });
    }

    // Query the device configuration in Config State and decode the acks. Runs on
    // the calling thread before the streaming io_thread starts; it drives the
    // io_context itself with run_for() and leaves it clean (restarted, all
    // pending ops cancelled) so the later io.run() streams normally.
    template <class WriteFn>
    void queryConfig(WriteFn&& write) {
        using namespace std::chrono_literals;
        ImuDeviceConfig local;
        MtiFramer cf([&](uint8_t mid, const uint8_t* d, std::size_t n) {
            applyImuConfigAck(local, mid, d, n);
        });

        // Drop any buffered pre-config stream (e.g. MTData from before GoToConfig
        // took effect) so the read window below is dominated by the query acks.
        ::tcflush(serial.native_handle(), TCIFLUSH);

        write(msgReqProductCode());
        write(msgReqDID());
        write(msgReqFWRev());
        write(msgReqPeriod());
        write(msgReqOutputMode());
        write(msgReqOutputSettings());
        write(msgReqFilterProfile());
        write(msgReqAvailFilterProfiles());

        std::array<uint8_t, 256> buf{};
        std::function<void()> rd = [&]() {
            serial.async_read_some(boost::asio::buffer(buf),
                                   [&](const boost::system::error_code& ec, std::size_t n) {
                                       if (ec) return;  // cancelled / error: stop
                                       cf.feed(buf.data(), n);
                                       rd();
                                   });
        };
        io.restart();
        rd();
        static_cast<void>(io.run_for(500ms));  // collect acks for up to half a second
        // Drain the pending read so its by-ref handler can't fire later with the
        // local buffer/framer already destroyed.
        boost::system::error_code ec;
        serial.cancel(ec);
        io.restart();
        static_cast<void>(io.run());
        io.restart();

        finalizeImuConfig(local);
        if (local.valid) {
            std::lock_guard<std::mutex> lock(mutex);
            cfg = local;
            have_cfg = true;
            LOG_INFO << "MTi config: " << (local.product_code.empty() ? "?" : local.product_code)
                     << " fw " << (local.firmware.empty() ? "?" : local.firmware)
                     << " scenario " << int(local.scenario_type)
                     << (local.scenario_label.empty() ? "" : " (" + local.scenario_label + ")")
                     << " @ " << local.sample_rate_hz << " Hz";
        } else {
            LOG_WARN << "MTi: no configuration acks received (device may not answer Req* in this firmware)";
        }
    }
};

MtiImuSource::MtiImuSource(std::string device, unsigned int baud)
    : impl_(std::make_unique<Impl>(std::move(device), baud)) {}

MtiImuSource::~MtiImuSource() { stop(); }

void MtiImuSource::start() {
    namespace asio = boost::asio;
    boost::system::error_code ec;
    impl_->serial.open(impl_->device, ec);
    if (ec) {
        LOG_ERROR << "MTi: failed to open " << impl_->device << ": " << ec.message();
        return;
    }
    impl_->serial.set_option(asio::serial_port_base::baud_rate(impl_->baud));
    impl_->serial.set_option(asio::serial_port_base::character_size(8));
    impl_->serial.set_option(asio::serial_port_base::parity(asio::serial_port_base::parity::none));
    impl_->serial.set_option(
        asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one));
    impl_->serial.set_option(
        asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none));

    impl_->open = true;
    impl_->configure();
    impl_->doRead();
    impl_->io_thread = std::thread([this] { impl_->io.run(); });
    LOG_INFO << "MTi IMU started on " << impl_->device << " @ " << impl_->baud;
}

void MtiImuSource::stop() {
    if (!impl_) return;
    impl_->io.stop();
    if (impl_->io_thread.joinable()) impl_->io_thread.join();
    boost::system::error_code ec;
    if (impl_->serial.is_open()) impl_->serial.close(ec);
    impl_->open = false;
}

bool MtiImuSource::connected() const {
    if (!impl_->open) return false;
    std::lock_guard<std::mutex> lock(impl_->mutex);
    return impl_->latest.valid &&
           (Impl::clock::now() - impl_->last_rx) < std::chrono::seconds(1);
}

std::optional<ImuSample> MtiImuSource::sample() {
    std::lock_guard<std::mutex> lock(impl_->mutex);
    if (!impl_->latest.valid ||
        (Impl::clock::now() - impl_->last_rx) >= std::chrono::seconds(1))
        return std::nullopt;
    return impl_->latest;
}

std::optional<ImuDeviceConfig> MtiImuSource::config() const {
    std::lock_guard<std::mutex> lock(impl_->mutex);
    if (!impl_->have_cfg) return std::nullopt;
    return impl_->cfg;
}

// Re-read the device configuration: briefly pause the streaming io_thread,
// re-run the Config-state handshake (which re-queries product/firmware/output/
// XKF profile and re-applies our output settings), then resume streaming. Mirrors
// start() minus opening the port. Called from the control thread; sample()/config()
// are called from that same thread when building the snapshot, so there is no
// concurrent serial access during the refresh.
void MtiImuSource::refreshConfig() {
    if (!impl_->open) { LOG_WARN << "MTi: refresh ignored (not started)"; return; }
    LOG_INFO << "MTi: refreshing configuration (briefly pausing the stream)...";
    impl_->restartWithConfig(-1);   // re-query only, no profile change
}

bool MtiImuSource::setFilterProfile(int type) {
    if (!impl_->open) { LOG_WARN << "MTi: setFilterProfile ignored (not started)"; return false; }
    if (type < 0) return false;
    LOG_INFO << "MTi: setting XKF profile to type " << type << " (persists on device)...";
    impl_->restartWithConfig(type);
    std::lock_guard<std::mutex> lock(impl_->mutex);
    return impl_->have_cfg && impl_->cfg.has_scenario && impl_->cfg.scenario_type == type;
}

// Send a command while the streaming io_thread is running. The write is posted
// onto the io_context so it runs on the io_thread, never concurrently with the
// in-flight async_read (concurrent ops on one serial_port are unsafe — same rule
// as SerialMotorController).
void MtiImuSource::noRotation(int seconds) {
    if (!impl_->open) { LOG_WARN << "MTi: noRotation ignored (not started)"; return; }
    if (seconds < 1) seconds = 1;
    auto msg = std::make_shared<std::vector<uint8_t>>(
        msgSetNoRotation(static_cast<uint16_t>(seconds)));
    boost::asio::post(impl_->io, [this, msg] {
        boost::system::error_code ec;
        boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec);
        if (ec) LOG_WARN << "MTi noRotation write failed: " << ec.message();
    });
    LOG_INFO << "MTi: no-rotation update for " << seconds << " s (hold still)";
}

void MtiImuSource::headingReset() {
    if (!impl_->open) { LOG_WARN << "MTi: headingReset ignored (not started)"; return; }
    auto msg = std::make_shared<std::vector<uint8_t>>(msgResetOrientation(kResetHeading));
    boost::asio::post(impl_->io, [this, msg] {
        boost::system::error_code ec;
        boost::asio::write(impl_->serial, boost::asio::buffer(*msg), ec);
        if (ec) LOG_WARN << "MTi headingReset write failed: " << ec.message();
    });
    LOG_INFO << "MTi: heading reset (current direction is now yaw 0)";
}

}  // namespace fgc