#include "fgc/Application.h"

#include "fgc/CaptureScheduler.h"
#include "fgc/CommandParser.h"
#include "fgc/DumpParser.h"
#include "fgc/HelpText.h"
#include "fgc/ICameraSource.h"
#include "fgc/IControlChannel.h"
#include "fgc/IMotorController.h"
#include "fgc/ImagePipeline.h"
#include "fgc/Logger.h"
#include "fgc/ScanGrid.h"
#include "fgc/SerialMotorController.h"
#include "fgc/mock/MockCameraSource.h"
#include "fgc/mock/MockMotorController.h"
#include "fgc/mock/NullControlChannel.h"
#include "fgc/ui/HeadlessUi.h"
#include "fgc/ui/IUserInterface.h"
#include "fgc/ui/UiSnapshot.h"

#include <atomic>
#include <chrono>
#include <memory>
#include <mutex>
#include <queue>
#include <sstream>
#include <string>
#include <thread>

#if FGC_WITH_MQTT
#include "fgc/MqttControlChannel.h"
#endif
#if FGC_WITH_VIMBA
#include "fgc/VimbaCameraSource.h"
#endif
#if FGC_WITH_TUI
#include "fgc/ui/TuiUi.h"
#endif

namespace fgc {

namespace {

// Human-readable list of the enabled wire-trace categories, for echoing back.
std::string traceNames(unsigned mask) {
    if (mask == 0) return "none";
    std::string out;
    const std::pair<LogCat, const char*> all[] = {
        {LogCat::Serial, "serial"}, {LogCat::Mqtt, "mqtt"},
        {LogCat::Camera, "camera"}, {LogCat::Control, "control"},
    };
    for (const auto& [cat, name] : all) {
        if (mask & static_cast<unsigned>(cat)) {
            if (!out.empty()) out += ',';
            out += name;
        }
    }
    return out;
}

}  // namespace

struct Application::Impl {
    Impl(AppConfig c, RuntimeOptions o) : cfg(std::move(c)), opts(std::move(o)) {}

    AppConfig      cfg;
    RuntimeOptions opts;

    std::unique_ptr<IControlChannel>  channel;
    std::unique_ptr<IMotorController> motor;
    std::unique_ptr<ICameraSource>    camera;
    std::unique_ptr<ImagePipeline>    pipeline;
    std::unique_ptr<CaptureScheduler> scheduler;
    std::unique_ptr<IUserInterface>   ui;
    ScanGrid                          grid;  // outlives scheduler (holds a reference to it)

    std::atomic<bool>       running{true};
    std::mutex              cmd_mutex;
    std::queue<std::string> cmd_queue;

    std::chrono::steady_clock::time_point start_time;
    std::mutex                            snapshot_mutex;
    UiSnapshot                            latest_snapshot;

    std::string statusTopicTower() const { return cfg.general.tower_name; }

    std::unique_ptr<IControlChannel> makeChannel() {
        bool want = opts.use_mqtt.value_or(cfg.features.enable_mqtt);
#if FGC_WITH_MQTT
        if (want) {
            return std::make_unique<MqttControlChannel>(cfg.network.broker_ip,
                                                        cfg.general.tower_name,
                                                        cfg.network.mqtt_user,
                                                        cfg.network.mqtt_pw);
        }
#else
        if (want) LOG_WARN << "MQTT requested but binary built without MQTT; using null channel";
#endif
        return std::make_unique<NullControlChannel>();
    }

    std::unique_ptr<IMotorController> makeMotor() {
        bool mock = opts.mock_serial.value_or(cfg.features.mock_serial);
        if (mock) return std::make_unique<MockMotorController>();
        return std::make_unique<SerialMotorController>(cfg.serial.device, cfg.serial.baud);
    }

    std::unique_ptr<ICameraSource> makeCamera() {
        bool mock = opts.mock_camera.value_or(cfg.features.mock_camera);
#if !FGC_WITH_VIMBA
        if (!mock) {
            LOG_WARN << "Camera requested but binary built without Vimba X; using mock camera";
            mock = true;
        }
#endif
        if (mock) {
            int count = cfg.camera.ids.empty() ? 1 : static_cast<int>(cfg.camera.ids.size());
            return std::make_unique<MockCameraSource>(count);
        }
#if FGC_WITH_VIMBA
        return std::make_unique<VimbaCameraSource>(cfg.camera.ids);
#else
        return std::make_unique<MockCameraSource>(1);
#endif
    }

    std::unique_ptr<IUserInterface> makeUi() {
        bool want = opts.use_tui.value_or(cfg.ui.enable_tui);
#if FGC_WITH_TUI
        if (want) return std::make_unique<TuiUi>();
#else
        if (want) LOG_WARN << "TUI requested but binary built without TUI support; using headless console";
#endif
        return std::make_unique<HeadlessUi>();
    }

    // Read live state (on the control thread) into a plain snapshot the UI can
    // copy and render without touching the logic objects. The log pane is filled
    // by the UI from its own ring buffer, so it is left empty here.
    UiSnapshot buildSnapshot() const {
        UiSnapshot s;

        const bool mock_serial = opts.mock_serial.value_or(cfg.features.mock_serial);
        s.header.tower     = cfg.general.tower_name;
        s.header.build     = "dev";
        s.header.uptime_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                                 std::chrono::steady_clock::now() - start_time).count();
        s.header.live      = !mock_serial;

        // --- Gimbal ---
        MotorTelemetry t = motor->telemetry();
        s.gimbal.present       = motor->connected();
        s.gimbal.pitch_present = t.pitch_present;
        auto fillAxis = [](AxisView& v, const char* label, const AxisTelemetry& a,
                           const AxisMap& map, long target_counts) {
            v.label      = label;
            v.state      = a.state;
            v.deg        = map.toDeg(a.xenc);
            v.xactual    = a.xactual;
            v.xenc       = a.xenc;
            v.target_deg = map.toDeg(target_counts);
            v.sg         = a.sg;
            v.cs         = a.cs;
            v.pwm        = a.pwm;
            v.drv_status = a.drv_status;
            v.moving     = a.moving();
            v.standstill = a.standstill;
            v.stall      = a.stall;
            v.overtemp   = a.overtemp;
            v.endstop_l  = a.endstop_l;
            v.endstop_r  = a.endstop_r;
        };
        long yaw_tgt   = scheduler ? scheduler->yawTargetCounts()   : 0;
        long pitch_tgt = scheduler ? scheduler->pitchTargetCounts() : 0;
        fillAxis(s.gimbal.yaw,   "YAW",   t.yaw,   cfg.geometry.yaw,   yaw_tgt);
        fillAxis(s.gimbal.pitch, "PITCH", t.pitch, cfg.geometry.pitch, pitch_tgt);

        // Homing travel limits come from the last firmware dump (lim_neg/lim_pos);
        // convert the endstop counts to degrees via the per-axis geometry.
        DumpData dd = parseDump(motor->lastDump());
        if (dd.valid) {
            auto fillLimits = [](AxisView& v, const DumpAxis& a, const AxisMap& map) {
                v.has_limits  = true;
                v.lim_neg     = a.lim_neg;
                v.lim_pos     = a.lim_pos;
                v.lim_neg_deg = map.toDeg(a.lim_neg);
                v.lim_pos_deg = map.toDeg(a.lim_pos);
            };
            for (const auto& a : dd.axes) {
                if (a.axis == 'Y') fillLimits(s.gimbal.yaw,   a, cfg.geometry.yaw);
                else if (a.axis == 'P') fillLimits(s.gimbal.pitch, a, cfg.geometry.pitch);
            }
        }

        // --- Sensors (DHT11 + MTi not integrated yet) ---
        s.sensors = pendingSensorsView();

        // --- Camera / capture ---
        s.capture.present      = true;
        s.capture.active       = scheduler && scheduler->captureActive();
        s.capture.image_rate   = scheduler ? scheduler->imageRate() : 0.0;
        s.capture.camera_count = camera ? camera->cameraCount() : 0;
        s.capture.labels       = cfg.camera.labels;
        if (pipeline) {
            if (auto ev = pipeline->lastEvent()) {
                s.capture.has_last         = true;
                s.capture.last_label       = ev->camera;
                s.capture.last_heading_deg = ev->heading_decideg / 10.0;
                s.capture.last_pitch_deg   = ev->pitch_decideg / 10.0;
                s.capture.last_ts_ms       = ev->timestamp_ms;
            }
        }

        // --- Connectivity ---
        s.conn.mqtt_enabled     = opts.use_mqtt.value_or(cfg.features.enable_mqtt);
        s.conn.mqtt_connected   = channel && channel->connected();
        s.conn.broker           = cfg.network.broker_ip;
        s.conn.tower            = cfg.general.tower_name;
        s.conn.control_code     = scheduler ? scheduler->controlCode() : 0;
        s.conn.target_heading   = scheduler ? scheduler->targetHeading() : "0";
        s.conn.last_status_code = s.conn.control_code;  // echoed back as status

        // --- Diagnostics (last firmware DUMP) ---
        s.dump.text = motor->lastDump();
        s.dump.has  = !s.dump.text.empty();
        return s;
    }

    void publishSnapshot() {
        UiSnapshot s = buildSnapshot();
        std::lock_guard<std::mutex> lock(snapshot_mutex);
        latest_snapshot = std::move(s);
    }

    void runInitSequence() {
        using namespace std::chrono_literals;
        LOG_INFO << "Running gimbal init sequence (enable + home)";
        motor->sendCommand("ENABLE Y");
        motor->sendCommand("ENABLE P");
        motor->sendCommand("HOME");  // homes all axes; firmware runs it non-blocking

        auto allReady = [](const MotorTelemetry& t) {
            return t.yaw.ready() && (!t.pitch_present || t.pitch.ready());
        };

        // The axes may already be READY (EEPROM fast-path), so first wait briefly
        // for homing to actually begin (an axis leaves READY) before waiting for
        // it to finish - otherwise we'd see the residual READY and return early.
        const auto t0 = std::chrono::steady_clock::now();
        while (running && std::chrono::steady_clock::now() < t0 + 3s) {
            std::this_thread::sleep_for(100ms);
            publishSnapshot();  // keep the TUI's gimbal panel live during homing
            if (!allReady(motor->telemetry())) break;  // homing started
        }

        // Wait for completion (both axes READY again) or the homing timeout.
        const auto deadline = std::chrono::steady_clock::now() + 65s;
        while (running && std::chrono::steady_clock::now() < deadline) {
            std::this_thread::sleep_for(250ms);
            publishSnapshot();  // keep the TUI's gimbal panel live during homing
            MotorTelemetry t = motor->telemetry();
            if (t.yaw.state == AxisState::Error ||
                (t.pitch_present && t.pitch.state == AxisState::Error)) {
                LOG_WARN << "Homing reported ERROR state; check the gimbal";
                break;
            }
            if (allReady(t)) {
                LOG_INFO << "Gimbal homed (both axes READY)";
                break;
            }
        }
        // Production move speeds for subsequent MOVE commands.
        motor->sendCommand("SPEED Y 50000");
        motor->sendCommand("SPEED P 150000");
        LOG_INFO << "Gimbal init finished";
    }

    void startCapture() {
        camera->start();
        scheduler->setCaptureActive(true);
        LOG_INFO << "Capture started";
    }
    void stopCapture() {
        camera->stop();
        scheduler->setCaptureActive(false);
        LOG_INFO << "Capture stopped";
    }

    // `trace off|none`, `trace all`, or `trace <cat> [on|off]` — toggle the
    // verbatim wire-trace categories (serial/mqtt/camera/control).
    void handleTrace(const Command& c) {
        if (c.device.empty() || c.device == "off" || c.device == "none") {
            Logger::setCategories(0);
        } else if (c.device == "all") {
            Logger::setCategories(static_cast<unsigned>(LogCat::All));
        } else {
            bool ok = true;
            LogCat cat = Logger::catFromString(c.device, &ok);
            if (!ok) {
                LOG_WARN << "unknown trace category: " << c.device;
                return;
            }
            bool on = (c.option != "off");  // "trace serial" / "trace serial on" => enable
            if (on) Logger::enableCategory(cat);
            else    Logger::disableCategory(cat);
        }
        LOG_INFO << "trace categories: " << traceNames(Logger::categories());
    }

    // `goto <yaw_deg> <pitch_deg>` — aim the gimbal at an absolute heading and
    // elevation in degrees. Converts to encoder counts via the operator-calibrated
    // Geometry maps (which soft-clamp to the travel limits) and issues a two-axis
    // MOVE so both axes start together.
    void handleGoto(const std::string& line) {
        std::istringstream iss(line);
        std::string verb;
        double yaw_deg = 0.0, pitch_deg = 0.0;
        if (!(iss >> verb >> yaw_deg >> pitch_deg)) {
            LOG_WARN << "usage: goto <yaw_deg> <pitch_deg>  (e.g. goto 30 -10)";
            return;
        }
        long yc = cfg.geometry.yaw.toCounts(yaw_deg);
        long pc = cfg.geometry.pitch.toCounts(pitch_deg);
        LOG_INFO << "goto yaw=" << yaw_deg << "deg pitch=" << pitch_deg
                 << "deg -> MOVE " << yc << "," << pc;
        motor->sendCommand("MOVE " + std::to_string(yc) + "," + std::to_string(pc));
    }

    void handleCommand(const std::string& line) {
        Command c = parseCommand(line);
        if (c.empty()) return;
        LOG_TRACE_CAT(LogCat::Control) << "cmd " << line;

        if (c.verb == "help") {
            // c.device holds the optional topic (first token after "help").
            for (const auto& l : renderHelp(c.device)) LOG_INFO << l;
        } else if (c.verb == "goto") {
            handleGoto(line);
        } else if (c.verb == "dump") {
            LOG_INFO << "requesting firmware dump...";
            motor->sendCommand("DUMP");
        } else if (c.verb == "exit") {
            running = false;
        } else if (c.verb == "start") {
            startCapture();
        } else if (c.verb == "stop") {
            stopCapture();
        } else if (c.verb == "debug") {
            bool on = Logger::level() != LogLevel::Debug;
            Logger::setLevel(on ? LogLevel::Debug : LogLevel::Info);
            LOG_INFO << "debug logging " << (on ? "on" : "off");
        } else if (c.verb == "trace") {
            handleTrace(c);
        } else if (c.verb == "set") {
            if (c.device == "camera") {
                if (c.option == "fps" && c.has_value) camera->setFrameRate(c.value);
                else if (c.option == "jxlq" && c.has_value) pipeline->setDistance(c.value);
                else if (c.option == "jxle" && c.has_value) pipeline->setEffort(static_cast<int>(c.value));
                else if (c.option == "display") pipeline->setDisplay(c.value != 0.0);
                else LOG_WARN << "unknown 'set camera' option: " << c.option;
            } else if (c.device == "fps" && c.has_value) {
                scheduler->setImageRate(c.value);
                LOG_INFO << "capture rate set to " << c.value << " img/s";
            } else if (c.device == "motorctl") {
                motor->sendCommand(c.option);
            } else {
                LOG_WARN << "unknown 'set' target: " << c.device;
            }
        } else {
            LOG_WARN << "unknown command: " << c.verb;
        }
    }

    void drainCommands() {
        std::queue<std::string> local;
        {
            std::lock_guard<std::mutex> lock(cmd_mutex);
            std::swap(local, cmd_queue);
        }
        while (!local.empty()) {
            handleCommand(local.front());
            local.pop();
        }
    }

    int run() {
        start_time = std::chrono::steady_clock::now();

        // Log level: config first, CLI overrides.
        if (!cfg.logging.level.empty() && !Logger::setLevelFromString(cfg.logging.level))
            LOG_WARN << "unknown Logging.level '" << cfg.logging.level << "', keeping default";
        if (!opts.log_level.empty() && !Logger::setLevelFromString(opts.log_level))
            LOG_WARN << "unknown log level '" << opts.log_level << "', keeping default";

        // Wire-trace categories: config first, CLI overrides (replaces, not merges).
        if (!cfg.logging.trace.empty()) {
            bool ok = true;
            unsigned m = Logger::parseCategories(cfg.logging.trace, &ok);
            if (!ok) LOG_WARN << "unknown trace category in Logging.trace '" << cfg.logging.trace << "'";
            Logger::setCategories(m);
        }
        if (!opts.trace_categories.empty()) {
            bool ok = true;
            unsigned m = Logger::parseCategories(opts.trace_categories, &ok);
            if (!ok) LOG_WARN << "unknown trace category in '" << opts.trace_categories << "'";
            Logger::setCategories(m);
        }
        if (Logger::categories())
            LOG_INFO << "wire trace enabled: " << traceNames(Logger::categories());

        channel = makeChannel();
        motor   = makeMotor();
        camera  = makeCamera();

        if (!channel->connect())
            LOG_WARN << "Control channel not connected; continuing in degraded mode";

        ImagePipeline::Params pp;
        pp.output_dir = cfg.paths.output_dir;
        pp.labels     = cfg.camera.labels;
        pp.tower      = cfg.general.tower_name;
        pp.demo       = opts.demo;
        // Orientation supplier: convert the per-axis encoder counts to degrees.
        pipeline = std::make_unique<ImagePipeline>(
            *channel,
            [this] {
                MotorTelemetry t = motor->telemetry();
                return Orientation{static_cast<float>(cfg.geometry.yaw.toDeg(t.yaw.xenc)),
                                   static_cast<float>(cfg.geometry.pitch.toDeg(t.pitch.xenc))};
            },
            pp);

        ImagePipeline* pipe_ptr = pipeline.get();
        camera->setFrameCallback([pipe_ptr](const Frame& f) { pipe_ptr->submit(f); });

        try {
            grid = ScanGrid::fromConfig(cfg.scan);
        } catch (const std::exception& e) {
            LOG_WARN << "scan grid load failed (" << e.what() << "); auto-sweep disabled";
        }
        LOG_INFO << "scan grid: " << grid.size() << " waypoints";
        scheduler = std::make_unique<CaptureScheduler>(*motor, *camera, *channel, cfg.image_rate(),
                                                       cfg.geometry, grid);

        motor->start();
        camera->open();
        pipeline->start();
        channel->publishStatus(0);

        // Seed an initial snapshot so the UI's first frame is populated, then
        // start the interface (headless console or TUI) on its own thread. The
        // UI is a pure observer: it pulls snapshots and pushes command strings
        // into the same queue the console always used.
        //
        // The UI is brought up BEFORE the init/home sequence so the dashboard is
        // visible during homing (which can take many seconds) and so the serial
        // wire traces render in the TUI's log pane instead of scrolling raw —
        // makeUi()/start() installs the TUI log sink, so anything logged after
        // this point is captured by it.
        publishSnapshot();
        ui = makeUi();
        ui->start([this] { std::lock_guard<std::mutex> lk(snapshot_mutex); return latest_snapshot; },
                  [this](const std::string& line) {
                      std::lock_guard<std::mutex> lk(cmd_mutex);
                      cmd_queue.push(line);
                  });

        if (opts.init) runInitSequence();
        if (opts.start) startCapture();

        LOG_INFO << "Entering control loop (type 'exit' to quit)";
        while (running) {
            drainCommands();
            scheduler->tick();
            publishSnapshot();
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }

        LOG_INFO << "Shutting down";
        if (ui) ui->stop();
        pipeline->stop();
        camera->stop();
        camera->close();
        motor->stop();
        channel->disconnect();
        return 0;
    }
};

Application::Application(AppConfig config, RuntimeOptions options)
    : impl_(std::make_unique<Impl>(std::move(config), std::move(options))) {}

Application::~Application() = default;

int Application::run() { return impl_->run(); }

}  // namespace fgc