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

private:
    using clock = std::chrono::steady_clock;
    clock::time_point start_ = clock::now();
};

}  // namespace fgc