#pragma once

#include "fgc/MtiProtocol.h"  // ImuSample

#include <array>

namespace fgc {

// How the IMU is physically bolted to the gimbal.
//
// Both frames are NED body frames:
//
//     X = forward     Y = right       Z = down
//     roll about X    pitch about Y   yaw about Z      (right-hand rule)
//
// x/y/z are degrees, and they are the rotation applied TO THE IMU to bring it
// into alignment with the gimbal - the direction the operator reads off the
// installation "to get from the IMU to the gimbal". An IMU that physically sits
// yawed +90 deg is therefore corrected with z = -90.
//
// Composition is intrinsic Z-X-Y: turn about Z, then about the X axis as it now
// stands, then the new Y.
struct ImuMount {
    double x_deg = 0.0;   // roll  - about forward
    double y_deg = 0.0;   // pitch - about right
    double z_deg = 0.0;   // yaw   - about down

    // No rotation at all. The common case, and worth testing for: an unmounted
    // rig then pays nothing and its samples pass through bit for bit.
    bool identity() const;
};

// Row-major 3x3. m[r][c].
using Mat3 = std::array<std::array<double, 3>, 3>;

// The rotation carrying the device's local earth frame onto NED. Identity for a
// device already reporting NED.
Mat3 nedFromReference(ImuReference r);

// Short name for logs and the Sensors view ("NED", "NWU", "ENU").
const char* referenceName(ImuReference r);

// The mount rotation: R = Rz(z) * Rx(x) * Ry(y).
//
// R carries the IMU frame onto the gimbal frame. Two transforms follow from it,
// and they are NOT the same matrix:
//
//   - a sensor-frame vector's components in the gimbal frame:  v_gimbal = R^T v_sensor
//   - the gimbal's orientation:  R_earth<-gimbal = R_earth<-sensor * R
Mat3 mountRotationZXY(const ImuMount& m);

// The MTi's own orientation output is the ordinary aerospace Z-Y-X sequence
// (yaw, then pitch, then roll), which is a DIFFERENT order from the mount above.
// These two helpers exist as a named pair so the two conventions cannot quietly
// swap places: R = Rz(yaw) * Ry(pitch) * Rx(roll).
Mat3 rotationFromEulerZYX(double roll_deg, double pitch_deg, double yaw_deg);

// Inverse of the above. Angles come back in the ranges atan2/asin produce, which
// are the MTi's own: roll and yaw in -180..180, pitch in -90..90. Degenerate at
// pitch = +/-90, where roll and yaw are no longer separable: roll is pinned to 0
// and the whole rotation is carried in yaw.
void eulerZYXFromRotation(const Mat3& R, double& roll_deg, double& pitch_deg, double& yaw_deg);

// The whole reading moved into the gimbal frame: orientation, acceleration,
// rate of turn and magnetic field. Temperature, sample counter and the valid
// flag are properties of the device rather than of a frame, and pass through.
//
// Returns the sample unchanged when the mount is the identity.
ImuSample applyMount(const ImuMount& m, const ImuSample& s);

// The full frame correction: re-reference the orientation from the device's earth
// frame to NED, then move the whole reading into the gimbal's body frame.
//
//     R_ned<-gimbal = nedFromReference(ref) * R_local<-sensor * mountRotationZXY(m)
//                     \_____ earth _____/                      \____ body ____/
//
// Only the orientation is re-referenced. acc, gyr and mag are BODY-frame
// quantities - they describe directions in the sensor, not relative to the earth
// - so the earth frame does not touch them and only the mount rotates them.
//
// Returns the sample unchanged when there is nothing to do (NED already, and an
// identity mount).
ImuSample applyFrames(ImuReference ref, const ImuMount& m, const ImuSample& s);

}  // namespace fgc