#pragma once

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

#include <chrono>
#include <cmath>

namespace fgc {

// Simulated IMU for development without hardware: synthesizes a slowly varying,
// physically plausible full sample (orientation sweeps, ~1 g on accZ, small
// gyro/mag) so the Sensors panel and its expanded view animate.
class MockImuSource : public IImuSource {
public:
    void start() override {
        start_ = clock::now();
        LOG_INFO << "[mock] IMU started";
    }
    void stop() override { LOG_INFO << "[mock] IMU stopped"; }

    bool connected() const override { return true; }

    std::optional<ImuSample> sample() override {
        const double t = std::chrono::duration<double>(clock::now() - start_).count();
        ImuSample s;
        s.valid     = true;
        s.roll_deg  = static_cast<float>(15.0 * std::sin(t * 0.5));
        s.pitch_deg = static_cast<float>(10.0 * std::sin(t * 0.3 + 1.0));
        s.yaw_deg   = static_cast<float>(std::fmod(t * 8.0, 360.0));      // slow spin
        // Gravity tilts with roll/pitch; small free accel noise.
        const double r = s.roll_deg * M_PI / 180.0, p = s.pitch_deg * M_PI / 180.0;
        s.acc[0] = static_cast<float>(9.81 * -std::sin(p));
        s.acc[1] = static_cast<float>(9.81 * std::sin(r) * std::cos(p));
        s.acc[2] = static_cast<float>(9.81 * std::cos(r) * std::cos(p));
        s.gyr[0] = static_cast<float>(0.13 * std::cos(t * 0.5));
        s.gyr[1] = static_cast<float>(0.05 * std::cos(t * 0.3 + 1.0));
        s.gyr[2] = 0.14f;
        s.mag[0] = static_cast<float>(std::cos(s.yaw_deg * M_PI / 180.0));
        s.mag[1] = static_cast<float>(-std::sin(s.yaw_deg * M_PI / 180.0));
        s.mag[2] = 0.35f;
        s.temp_c = static_cast<float>(24.5 + 0.5 * std::sin(t * 0.05));
        s.sample_counter = static_cast<uint16_t>(static_cast<unsigned>(t * 100.0) & 0xFFFF);
        return s;
    }

    void refreshConfig() override { LOG_INFO << "[mock] IMU config refreshed"; }
    void noRotation(int seconds) override {
        LOG_INFO << "[mock] IMU no-rotation update for " << seconds << " s";
    }
    void headingReset() override {
        LOG_INFO << "[mock] IMU heading reset (current direction is now yaw 0)";
    }
    bool setFilterProfile(int type) override {
        scenario_type_ = type;  // observable in the next config()
        LOG_INFO << "[mock] IMU XKF profile set to type " << type;
        return true;
    }

    // Synthetic config mirroring the real handshake (Euler + calibrated, 100 Hz),
    // so the expanded view's IMU CONFIG section renders without hardware. The
    // available list includes a no-mag profile so the calibration's profile switch
    // has something to pick.
    std::optional<ImuDeviceConfig> config() const override {
        ImuDeviceConfig c;
        c.valid = true;
        c.product_code = "MTi-28A53G35 (mock)";
        c.has_device_id = true;
        c.device_id = 0x00990ABC;
        c.firmware = "2.8.1 build 0";
        c.has_output_mode = true;
        c.output_mode = kOutputMode;
        c.out_temperature = c.out_calibrated = c.out_orientation = true;
        c.has_output_settings = true;
        c.output_settings = kOutputSettings;
        c.orientation_mode = "Euler";
        c.timestamp_mode = "Sample counter";
        c.data_format = "Float";
        c.has_period = true;
        c.period = 1152;  // 100 Hz
        c.sample_rate_hz = 100.0f;
        c.available_profiles = {{39, 11, "General"}, {40, 11, "High_mag_dep"},
                                {41, 11, "Dynamic"}, {53, 11, "VRU_general"}};
        c.has_scenario = true;
        c.scenario_type = static_cast<uint8_t>(scenario_type_);
        c.scenario_version = 11;
        for (const auto& p : c.available_profiles)
            if (p.type == c.scenario_type) c.scenario_label = p.label;
        return c;
    }

private:
    using clock = std::chrono::steady_clock;
    clock::time_point start_ = clock::now();
    int               scenario_type_ = 39;  // current XKF profile (mutable via setFilterProfile)
};

}  // namespace fgc