#include "fgc/Application.h"

#include "fgc/CalibrationRoutine.h"
#include "fgc/CaptureScheduler.h"
#include "fgc/CommandParser.h"
#include "fgc/DiagParser.h"
#include "fgc/DumpParser.h"
#include "fgc/HelpText.h"
#include "fgc/Paths.h"
#include "fgc/ICameraSource.h"
#include "fgc/IControlChannel.h"
#include "fgc/IImuSource.h"
#include "fgc/IMotorController.h"
#include "fgc/ImagePipeline.h"
#include "fgc/Logger.h"
#include "fgc/MtiImuSource.h"
#include "fgc/ScanGrid.h"
#include "fgc/SerialMotorController.h"
#include "fgc/mock/MockCameraSource.h"
#include "fgc/mock/MockImuSource.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 <cctype>
#include <chrono>
#include <cstdio>
#include <cstdlib>
#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 {

long long nowEpochMs() {
    return std::chrono::duration_cast<std::chrono::milliseconds>(
               std::chrono::system_clock::now().time_since_epoch())
        .count();
}

// 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;
}

// Build the expanded-view ImuConfigView from the device's decoded config.
ImuConfigView formatImuConfig(const ImuDeviceConfig& c) {
    ImuConfigView v;
    v.present = true;
    v.product_code = c.product_code.empty() ? "—" : c.product_code;
    if (c.has_device_id) {
        char b[16];
        std::snprintf(b, sizeof(b), "0x%08X", c.device_id);
        v.device_id = b;
    } else {
        v.device_id = "—";
    }
    v.firmware = c.firmware.empty() ? "—" : c.firmware;

    if (c.has_output_mode) {
        std::string m;
        auto add = [&](bool on, const char* name) {
            if (on) { if (!m.empty()) m += " · "; m += name; }
        };
        add(c.out_temperature, "Temp");
        add(c.out_calibrated, "Calibrated");
        add(c.out_orientation, "Orientation");
        add(c.out_auxiliary, "Auxiliary");
        add(c.out_status, "Status");
        v.output_mode = m.empty() ? "—" : m;
    } else {
        v.output_mode = "—";
    }

    if (c.has_output_settings) {
        v.output_settings = c.orientation_mode + " · " + c.timestamp_mode + " · " + c.data_format;
        std::string ch;
        if (c.acc_enabled) ch += "acc ";
        if (c.gyr_enabled) ch += "gyr ";
        if (c.mag_enabled) ch += "mag";
        v.channels = ch.empty() ? "none" : ch;
    } else {
        v.output_settings = "—";
        v.channels = "—";
    }

    if (c.has_period && c.sample_rate_hz > 0) {
        char b[24];
        std::snprintf(b, sizeof(b), "%.0f Hz", c.sample_rate_hz);
        v.sample_rate = b;
    } else {
        v.sample_rate = "—";
    }

    // XKF profiles: list every available profile by name (no numeric IDs) and
    // flag the active one (matched by profile type).
    for (const auto& p : c.available_profiles) {
        ImuProfileView pv;
        pv.name = p.label.empty() ? "(unnamed)" : p.label;
        pv.selected = c.has_scenario && p.type == c.scenario_type;
        v.xkf_profiles.push_back(std::move(pv));
    }
    // If the device reported the active profile but not the full list, still show
    // the one in use (by its resolved label, when known).
    if (v.xkf_profiles.empty() && c.has_scenario && !c.scenario_label.empty())
        v.xkf_profiles.push_back({c.scenario_label, true});
    return v;
}

}  // 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<IImuSource>       imu;
    std::unique_ptr<ImagePipeline>    pipeline;
    std::unique_ptr<CaptureScheduler> scheduler;
    std::unique_ptr<IUserInterface>   ui;
    std::unique_ptr<CalibrationRoutine> calib;
    unsigned                          last_diag_seq = 0;
    ScanGrid                          grid;  // outlives scheduler (holds a reference to it)

    // Persisted last-results for the TUI activity strip (survive in the log churn).
    bool                     diag_running_ = false;
    std::chrono::steady_clock::time_point diag_started_;
    std::vector<std::string> last_diag_summary;
    long long                last_diag_ts = 0;   // epoch ms
    bool                     last_diag_pass = false;
    CalibResultView          last_calib_view;
    std::vector<std::string> last_calib_summary;
    long long                last_calib_ts = 0;   // epoch ms
    bool                     calib_save_pending_ = false;  // awaiting y/n to persist to config
    mutable ImuConfigView    imu_config_view_;     // formatted MTi config (read once)
    mutable bool             imu_config_done_ = false;

    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<IImuSource> makeImu() {
        if (!cfg.features.enable_imu) return nullptr;  // sensors panel stays "pending"
        if (cfg.features.mock_imu) return std::make_unique<MockImuSource>();
        if (cfg.imu.device.empty()) {
            LOG_WARN << "IMU enabled but [IMU] device is empty; disabling IMU";
            return nullptr;
        }
        return std::make_unique<MtiImuSource>(cfg.imu.device, cfg.imu.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: MTi (orientation) live if present; DHT11 (ambient) pending ---
        s.sensors = pendingSensorsView();
        if (imu) {
            auto fmt1 = [](float v) {
                char b[24];
                std::snprintf(b, sizeof(b), "%.1f", v);
                return std::string(b);
            };
            auto setField = [&](SensorField& f, const std::string& v) {
                f.value   = v;
                f.present = true;
            };
            // Device configuration (read once at startup): format and cache.
            if (!imu_config_done_) {
                if (auto c = imu->config()) {
                    imu_config_view_ = formatImuConfig(*c);
                    imu_config_done_ = true;
                }
            }
            s.imu.config = imu_config_view_;
            if (auto m = imu->sample()) {
                // Full reading for the expanded view.
                s.imu.present        = true;
                s.imu.roll_deg       = m->roll_deg;
                s.imu.pitch_deg      = m->pitch_deg;
                s.imu.yaw_deg        = m->yaw_deg;
                for (int i = 0; i < 3; ++i) {
                    s.imu.acc[i] = m->acc[i];
                    s.imu.gyr[i] = m->gyr[i];
                    s.imu.mag[i] = m->mag[i];
                }
                s.imu.temp_c         = m->temp_c;
                s.imu.sample_counter = m->sample_counter;
                // Compact MTi subsection: Roll/Pitch/Yaw + the device's internal temp.
                auto& g = s.sensors.imu;
                g.present = true;
                g.status  = "live";
                if (g.fields.size() >= 4) {
                    setField(g.fields[0], fmt1(m->roll_deg));
                    setField(g.fields[1], fmt1(m->pitch_deg));
                    setField(g.fields[2], fmt1(m->yaw_deg));
                    setField(g.fields[3], fmt1(m->temp_c));  // MTi internal temp
                }
            } else {
                s.sensors.imu.status = "no fix";
            }
        }

        // --- 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();

        // --- Activity strip + last calibration ---
        s.calib = last_calib_view;
        s.calib.pitch_present = s.gimbal.pitch_present || s.calib.pitch_present;
        fillActivity(s);
        return s;
    }

    // The currently-running special op (priority: calib > diag > homing > capture)
    // plus the most recent persisted result, for the activity strip.
    void fillActivity(UiSnapshot& s) const {
        ActivityView& a = s.activity;

        if (calib && calib->running()) {
            CalibProgress p = calib->progress();
            a.active = true;
            a.title  = "CALIBRATING";
            std::string axis = (p.axis == 'P') ? "PITCH" : (p.axis == 'Y') ? "YAW" : "";
            a.status = axis.empty() ? p.phase
                     : axis + " step " + std::to_string(p.step) + "/" +
                           std::to_string(p.total) + " — " + p.phase;
        } else if (diag_running_) {
            a.active = true;
            a.title  = "DIAGNOSTICS";
            a.status = "running motor self-test…";
        } else if (s.gimbal.yaw.state == AxisState::Homing ||
                   (s.gimbal.pitch_present && s.gimbal.pitch.state == AxisState::Homing)) {
            a.active = true;
            a.title  = "HOMING";
            a.status = "finding endstops…";
        } else if (scheduler && scheduler->captureActive()) {
            a.active = true;
            a.title  = "CAPTURE";
            a.status = s.capture.has_last
                           ? ("scanning · last " + s.capture.last_label)
                           : "scanning…";
        }

        // Persisted result: the more recent of the last calibration / diagnostics.
        const long long now = nowEpochMs();
        if (last_calib_ts || last_diag_ts) {
            const bool calib_newer = last_calib_ts >= last_diag_ts;
            a.has_result = true;
            if (calib_newer) {
                a.result_title = "Calibration · " + formatTimeAgo(now, last_calib_ts);
                a.result       = last_calib_summary;
                a.result_color = last_calib_view.has &&
                                 last_calib_view.yaw.ok ? UiColor::Green : UiColor::Yellow;
            } else {
                a.result_title = std::string("Diagnostics ") +
                                 (last_diag_pass ? "PASS" : "FAIL") + " · " +
                                 formatTimeAgo(now, last_diag_ts);
                a.result       = last_diag_summary;
                a.result_color = last_diag_pass ? UiColor::Green : UiColor::Red;
            }
        }

        // Pending yes/no: persist the just-applied calibration as the new default?
        if (calib_save_pending_)
            a.prompt = "Save this calibration to config as the new default?  (y / n)";
    }

    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.
    // Parse "<a>,<b>" or "<a> <b>" (tokens from `from` onward) into two numbers.
    static bool parsePair(const std::vector<std::string>& tok, size_t from,
                          double& a, double& b) {
        std::string s;
        for (size_t i = from; i < tok.size(); ++i) s += tok[i] + " ";
        for (char& ch : s) if (ch == ',') ch = ' ';
        std::istringstream iss(s);
        return static_cast<bool>(iss >> a >> b);
    }

    // Soft-limit travel (counts) for an axis from the last dump, else the
    // configured degree range converted to counts. Returns 0 if unknown.
    long axisRangeCounts(char axis) {
        DumpData d = parseDump(motor->lastDump());
        if (d.valid)
            for (const auto& ax : d.axes)
                if (ax.axis == axis) return std::labs(ax.lim_pos - ax.lim_neg);
        const AxisMap& m = (axis == 'Y') ? cfg.geometry.yaw : cfg.geometry.pitch;
        return std::labs(m.toCounts(m.max_deg) - m.toCounts(m.min_deg));
    }

    // `gimbal <subcommand>` — unified, lowercase motor control. `move` is degrees
    // (default), `steps` is raw encoder counts; the rest map to firmware verbs.
    void handleGimbal(const std::string& line) {
        std::istringstream iss(line);
        std::vector<std::string> tok;
        std::string t;
        while (iss >> t) tok.push_back(t);  // tok[0] == "gimbal"
        if (tok.size() < 2) {
            LOG_WARN << "usage: gimbal <move|steps|nudge|home|stop|reset|enable|disable|"
                        "speed|setpos|status|dump|diag|calib|raw>";
            return;
        }
        std::string sub = tok[1];
        for (char& ch : sub) ch = static_cast<char>(std::tolower((unsigned char)ch));

        if (sub == "move") {
            double yaw_deg = 0, pitch_deg = 0;
            if (!parsePair(tok, 2, yaw_deg, pitch_deg)) {
                LOG_WARN << "usage: gimbal move <yaw_deg>,<pitch_deg>";
                return;
            }
            long yc = cfg.geometry.yaw.toCounts(yaw_deg);
            long pc = cfg.geometry.pitch.toCounts(pitch_deg);
            LOG_INFO << "gimbal move yaw=" << yaw_deg << " pitch=" << pitch_deg
                     << " deg -> MOVE " << yc << "," << pc;
            motor->sendCommand("MOVE " + std::to_string(yc) + "," + std::to_string(pc));
        } else if (sub == "steps") {
            double yc = 0, pc = 0;
            if (!parsePair(tok, 2, yc, pc)) {
                LOG_WARN << "usage: gimbal steps <yaw_counts>,<pitch_counts>";
                return;
            }
            motor->sendCommand("MOVE " + std::to_string((long)yc) + "," + std::to_string((long)pc));
        } else if (sub == "nudge") {
            if (tok.size() < 4) { LOG_WARN << "usage: gimbal nudge <yaw|pitch> <+/-pct>"; return; }
            char axis = (tok[2][0] == 'p' || tok[2][0] == 'P') ? 'P' : 'Y';
            double pct = 0;
            try { pct = std::stod(tok[3]); } catch (...) { LOG_WARN << "gimbal nudge: bad percent"; return; }
            long range = axisRangeCounts(axis);
            if (range <= 0) { LOG_WARN << "gimbal nudge: travel unknown; run gimbal home/dump first"; motor->sendCommand("DUMP"); return; }
            MotorTelemetry tel = motor->telemetry();
            long cur = (axis == 'Y') ? tel.yaw.xenc : tel.pitch.xenc;
            long target = cur + static_cast<long>(pct / 100.0 * range);
            LOG_INFO << "gimbal nudge " << axis << " " << pct << "% -> MOVE " << axis << " " << target;
            motor->sendCommand(std::string("MOVE ") + axis + " " + std::to_string(target));
        } else if (sub == "diag") {
            MotorTelemetry tel = motor->telemetry();
            if (!tel.yaw.ready() && !(tel.pitch_present && tel.pitch.ready())) {
                LOG_WARN << "gimbal diag: axes not READY (home first)";
                return;
            }
            std::string fw = "DIAG";
            for (size_t i = 2; i < tok.size(); ++i) fw += " " + tok[i];
            LOG_INFO << "running gimbal diagnostics...";
            motor->sendCommand(fw);
            diag_running_ = true;
            diag_started_ = std::chrono::steady_clock::now();
        } else if (sub == "calib") {
            startCalibration();
        } else if (sub == "dump") {
            LOG_INFO << "requesting firmware dump...";
            motor->sendCommand("DUMP");
        } else if (sub == "raw") {
            // Low-level escape hatch: send the text inside the quotation marks to
            // the serial port verbatim (the transport appends the newline). This
            // exposes any firmware verb directly, including ones with no wrapper.
            std::string raw;
            auto q1 = line.find('"');
            auto q2 = line.rfind('"');
            if (q1 != std::string::npos && q2 != std::string::npos && q2 > q1) {
                raw = line.substr(q1 + 1, q2 - q1 - 1);  // exact text between quotes
            } else {
                for (size_t i = 2; i < tok.size(); ++i)  // unquoted: join remaining tokens
                    raw += (raw.empty() ? "" : " ") + tok[i];
            }
            if (raw.empty()) {
                LOG_WARN << "usage: gimbal raw \"<verbatim firmware command>\"";
                return;
            }
            LOG_INFO << "gimbal raw -> " << raw;
            motor->sendCommand(raw);
        } else if (sub == "home" || sub == "stop" || sub == "reset" || sub == "enable" ||
                   sub == "disable" || sub == "speed" || sub == "setpos" || sub == "status") {
            // Passthrough to the (case-insensitive) firmware verb + args.
            std::string fw = sub;
            for (char& ch : fw) ch = static_cast<char>(std::toupper((unsigned char)ch));
            for (size_t i = 2; i < tok.size(); ++i) fw += " " + tok[i];
            motor->sendCommand(fw);
        } else {
            LOG_WARN << "unknown gimbal subcommand: " << sub;
        }
    }

    void startCalibration() {
        if (!imu) { LOG_WARN << "gimbal calib requires the IMU (set [Features] enable_imu)"; return; }
        if (calib && calib->running()) { LOG_WARN << "calibration already running (gimbal stop to cancel)"; return; }
        stopCapture();  // free the serial link + don't fight the routine's moves
        calib = std::make_unique<CalibrationRoutine>(*motor, *imu, cfg.geometry);
        calib->start();
    }

    // Persist the live (just-calibrated) [Motor] geometry back into config.ini.
    void saveCalibration() {
        if (!calib_save_pending_) return;
        calib_save_pending_ = false;
        if (saveMotorCalibration(opts.config_path, cfg.geometry))
            LOG_INFO << "calibration saved as the new default in " << opts.config_path;
        else
            LOG_WARN << "could not write calibration to " << opts.config_path;
    }

    void cancelCalibration() {
        if (calib && calib->running()) {
            LOG_INFO << "cancelling calibration";
            calib->cancel();
            motor->sendCommand("STOP ALL");
        }
    }

    // Pick up results produced by background work (calibration thread, DIAG
    // capture) — runs on the control thread each tick so all geometry/state
    // mutation stays single-threaded.
    void pollBackgroundResults() {
        if (calib) {
            if (auto g = calib->takeResult()) {
                cfg.geometry = *g;
                if (scheduler) scheduler->setGeometry(cfg.geometry);  // also fix scan targeting
                LOG_INFO << "calibration applied to live geometry";
                // Capture the structured report for the activity strip + gimbal view.
                CalibReport rep = calib->report();
                CalibResultView v;
                v.has = rep.valid;
                v.ts_ms = rep.ts_ms;
                last_calib_summary.clear();
                for (const auto& ax : rep.axes) {
                    CalibAxisView* dst = (ax.axis == 'P') ? &v.pitch : &v.yaw;
                    if (ax.axis == 'P') v.pitch_present = true;
                    dst->ok = ax.ok;
                    dst->counts_per_deg = ax.counts_per_deg;
                    dst->zero_count = ax.zero_count;
                    dst->r2 = ax.r2;
                    dst->n = ax.n;
                    char b[96];
                    std::snprintf(b, sizeof(b), "%s  cpd=%.3f  zero=%ld  R2=%.4f (n=%d)%s",
                                  ax.axis == 'P' ? "PITCH" : "YAW", ax.counts_per_deg,
                                  ax.zero_count, ax.r2, ax.n, ax.ok ? "" : "  FIT FAILED");
                    last_calib_summary.emplace_back(b);
                }
                last_calib_view = v;
                last_calib_ts   = nowEpochMs();
                // Offer to persist it as the new default (answered via the activity strip).
                if (rep.valid && !opts.config_path.empty()) calib_save_pending_ = true;
            }
        }
        unsigned seq = motor->diagSeq();
        if (seq != last_diag_seq) {
            last_diag_seq = seq;
            diag_running_ = false;
            DiagResult dr = parseDiag(motor->lastDiag());
            std::ostringstream summary;
            for (const auto& l : formatDiag(dr)) { LOG_INFO << l; summary << l << "\n"; }
            // Concise per-axis lines for the activity strip (full detail is logged).
            last_diag_summary.clear();
            for (const auto& ax : dr.axes)
                last_diag_summary.push_back(std::string(1, ax.axis) + ": " +
                                            (ax.pass ? "PASS" : "FAIL") + "  (" +
                                            std::to_string(ax.tests.size()) + " tests)");
            last_diag_pass = dr.allPass();
            last_diag_ts   = nowEpochMs();
            std::string path = paths::writeLogFile(
                paths::timestampedLogName("diag"),
                motor->lastDiag() + "\n--- parsed ---\n" + summary.str());
            if (!path.empty()) LOG_INFO << "diagnostics log written: " << path;
        }
        // Safety: clear a stuck diag flag if the firmware never replied.
        if (diag_running_ &&
            std::chrono::steady_clock::now() - diag_started_ > std::chrono::seconds(30))
            diag_running_ = false;
    }

    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 == "gimbal") {
            handleGimbal(line);
        } else if (c.verb == "calib") {
            // Answer to the "save calibration?" prompt (activity-strip y/n keys).
            if (c.device == "save") saveCalibration();
            else if (c.device == "discard") {
                calib_save_pending_ = false;
                LOG_INFO << "calibration kept for this session only (not saved to config)";
            }
        } else if (c.verb == "exit") {
            running = false;
        } else if (c.verb == "start") {
            startCapture();
        } else if (c.verb == "stop") {
            cancelCalibration();  // `stop` also aborts a running calibration
            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 == "refresh") {
            // Re-read live device state surfaced in the expanded views: the IMU
            // configuration (e.g. after the XKF profile was changed externally)
            // and the firmware register dump shown in the gimbal 'g' view.
            LOG_INFO << "refresh: re-reading IMU configuration + firmware dump";
            if (imu) {
                imu->refreshConfig();
                imu_config_done_ = false;  // force buildSnapshot to re-read config()
            }
            motor->sendCommand("DUMP");
        } 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 {
                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();
        imu     = makeImu();

        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();
        if (imu) imu->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();
            pollBackgroundResults();  // apply calibration result / emit DIAG summary+log
            scheduler->tick();
            publishSnapshot();
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }

        LOG_INFO << "Shutting down";
        if (calib) calib->cancel();  // stop any in-flight calibration before teardown
        if (ui) ui->stop();
        pipeline->stop();
        camera->stop();
        camera->close();
        if (imu) imu->stop();
        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
