#pragma once

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

#include <memory>
#include <utility>

namespace fgc {

// Wraps any IImuSource and puts its readings into the gimbal's NED body frame:
// the device's earth reference is re-referenced to NED, and the [IMU] x/y/z mount
// carries the body frame onto the gimbal's. Both are needed and they are not
// interchangeable - see applyFrames().
//
// This sits directly on the factory's output, which is the whole point: every
// consumer - the calibration routine, the self-test suite, the TUI, `imu status`,
// the MQTT path - already reads through IImuSource::sample(), so one wrap covers
// all of them and no consumer needs to know a mount exists. It wraps the mock as
// readily as the MTi, which is what makes the feature exercisable with
// --mock-imu on a machine with no hardware.
//
// Application skips the wrapper entirely when there is nothing to do (the device
// already reports NED and the mount is the identity), so such a rig carries
// neither the indirection nor the arithmetic.
class MountedImuSource : public IImuSource {
public:
    MountedImuSource(std::unique_ptr<IImuSource> inner, ImuReference reference, ImuMount mount)
        : inner_(std::move(inner)), reference_(reference), mount_(mount) {}

    void start() override { inner_->start(); }
    void stop() override { inner_->stop(); }
    bool connected() const override { return inner_->connected(); }

    std::optional<ImuSample> sample() override {
        auto s = inner_->sample();
        if (!s) return s;
        return applyFrames(reference_, mount_, *s);
    }

    // Everything below describes the device, not a frame, so it passes straight
    // through. The mount is a host-side correction and never reaches the MTi.
    std::optional<ImuDeviceConfig> config() const override { return inner_->config(); }
    std::vector<std::string> configProblems() const override { return inner_->configProblems(); }
    void refreshConfig() override { inner_->refreshConfig(); }
    void noRotation(int seconds) override { inner_->noRotation(seconds); }
    void headingReset() override { inner_->headingReset(); }
    bool setFilterProfile(int type) override { return inner_->setFilterProfile(type); }

private:
    std::unique_ptr<IImuSource> inner_;
    ImuReference                reference_;
    ImuMount                    mount_;
};

}  // namespace fgc