import XCTest
@testable import RideFormCore

final class CalibrationTests: XCTestCase {
    func testSmallNeutralOscillationAcceptedButSavedStrictPolicyAndDriftStillReject() throws {
        let neutral = (0...250).map { i -> MotionSample in
            let t = Double(i)/100, amplitude = Double.pi/180
            return MotionSample(sensorTimestamp: t,
                attitudeQuaternion: OrientationMath.quaternion(rotationVector: Vector3(x: amplitude*sin(2 * .pi*t))),
                rotationRate: Vector3(x: amplitude*2 * .pi*cos(2 * .pi*t)))
        }
        let left = samples(OrientationMath.quaternion(rotationVector: Vector3(x: -0.3)))
        let forward = samples(OrientationMath.quaternion(rotationVector: Vector3(y: 0.3)))
        let result = try CalibrationSolver.fit(neutral: neutral, left: left, forward: forward, convention: convention)
        XCTAssertEqual(result.neutralSamples, neutral)
        var strict = CalibrationSolver.defaultConfiguration
        strict.parameters["maximumNeutralGyroRadiansPerSecond"] = 0.08
        XCTAssertThrowsError(try CalibrationSolver.fit(neutral: neutral, left: left, forward: forward, convention: convention, configuration: strict))
        let drift = (0...250).map { i in MotionSample(sensorTimestamp: Double(i)/100,
            attitudeQuaternion: OrientationMath.quaternion(rotationVector: Vector3(x: Double(i)*0.001)), rotationRate: Vector3(x: 0.1)) }
        XCTAssertThrowsError(try CalibrationSolver.fit(neutral: drift, left: left, forward: forward, convention: convention))
    }
    let convention = RotationConvention(capturedToCanonical: "identity", verified: true)
    func samples(_ q: Quaternion, duration: Double = 2.5) -> [MotionSample] {
        (0...250).map { MotionSample(sensorTimestamp: Double($0) * duration / 250, attitudeQuaternion: q) }
    }
    func fixture() throws -> Calibration {
        try CalibrationSolver.fit(neutral: samples(Quaternion()), left: samples(OrientationMath.quaternion(rotationVector: Vector3(x: -0.3))), forward: samples(OrientationMath.quaternion(rotationVector: Vector3(y: 0.3))), convention: convention)
    }
    func testHemisphereMeanAndWrap() throws {
        let q = OrientationMath.quaternion(rotationVector: Vector3(z: 179 * .pi / 180))
        let mean = try OrientationMath.mean([q, Quaternion(x: -q.x, y: -q.y, z: -q.z, w: -q.w)])
        XCTAssertEqual(OrientationMath.rotationVector(OrientationMath.multiply(OrientationMath.inverse(q), mean)).z, 0, accuracy: 1e-12)
        let next = OrientationMath.quaternion(rotationVector: Vector3(z: -179 * .pi / 180))
        XCTAssertEqual(OrientationMath.rotationVector(OrientationMath.multiply(OrientationMath.inverse(q), next)).z * 180 / .pi, 2, accuracy: 1e-10)
    }
    func testObliqueMountNoncommutingNeutralAndPersistedValues() throws {
        for mount in [Vector3(x: 0.4, y: -0.7, z: 1.1), Vector3(x: -0.9, y: 0.2, z: -0.5)] {
            let basis = OrientationMath.quaternion(rotationVector: mount)
            let neutral = OrientationMath.multiply(OrientationMath.quaternion(rotationVector: Vector3(x: 0.6)), OrientationMath.quaternion(rotationVector: Vector3(y: 0.5)))
            func attitude(_ rider: Vector3) -> Quaternion {
                OrientationMath.multiply(neutral, OrientationMath.multiply(OrientationMath.inverse(basis), OrientationMath.multiply(OrientationMath.quaternion(rotationVector: rider), basis)))
            }
            let calibration = try CalibrationSolver.fit(neutral: samples(neutral), left: samples(attitude(Vector3(x: -0.3))), forward: samples(attitude(Vector3(y: 0.3))), convention: convention)
            let data = try SessionCodec.encode(calibration)
            let reloaded = try JSONDecoder().decode(Calibration.self, from: data)
            XCTAssertEqual(calibration, reloaded)
            for axis in 0..<3 {
                for sign in [-1.0, 1.0] {
                    let degrees = axis == 1 ? 15.0 : 20.0
                    let angle = sign * degrees * .pi / 180
                    let r = axis == 0 ? Vector3(x: -angle) : axis == 1 ? Vector3(y: angle) : Vector3(z: angle)
                    let deviceVector = OrientationMath.rotate(OrientationMath.inverse(basis), Vector3(x: 1, y: 2, z: 3))
                    let q = attitude(r)
                    let relative = OrientationMath.multiply(OrientationMath.inverse(neutral), q)
                    let currentVector = OrientationMath.rotate(OrientationMath.inverse(relative), deviceVector)
                    let sample = MotionSample(attitudeQuaternion: q, rotationRate: currentVector, userAcceleration: currentVector)
                    let value = try OrientationMath.riderMotion(sample: sample, calibration: reloaded, convention: convention)
                    let displayed = [value.displayAnglesRadians.x, value.displayAnglesRadians.y, value.displayAnglesRadians.z]
                    XCTAssertEqual(displayed[axis] * 180 / .pi, sign * degrees, accuracy: 0.1)
                    XCTAssertEqual(value.rotationRate.x, 1, accuracy: 1e-10)
                    XCTAssertEqual(value.userAcceleration.y, 2, accuracy: 1e-10)
                }
            }
        }
    }
    func testInverseConventionAndReturningGesturesPreserveRawEvidence() throws {
        let neutral = samples(Quaternion())
        var left = samples(OrientationMath.quaternion(rotationVector: Vector3(x: 0.3)))
        var forward = samples(OrientationMath.quaternion(rotationVector: Vector3(y: -0.3)))
        left.append(MotionSample(sensorTimestamp: 2.51))
        forward.append(MotionSample(sensorTimestamp: 2.51))
        let inverse = RotationConvention(capturedToCanonical: "quaternion conjugate / matrix transpose", verified: true)
        let current = RotationConvention(capturedToCanonical: "quaternion conjugate", verified: true)
        XCTAssertEqual(try OrientationMath.canonical(left[0].attitudeQuaternion, convention: current), try OrientationMath.canonical(left[0].attitudeQuaternion, convention: inverse))
        let calibration = try CalibrationSolver.fit(neutral: neutral, left: left, forward: forward, convention: inverse)
        XCTAssertEqual(calibration.leftGestureSamples, left)
        XCTAssertEqual(calibration.forwardGestureSamples, forward)
        let value = try OrientationMath.riderMotion(sample: MotionSample(attitudeQuaternion: OrientationMath.quaternion(rotationVector: Vector3(x: 0.2))), calibration: calibration, convention: inverse)
        XCTAssertEqual(value.displayAnglesRadians.x, 0.2, accuracy: 1e-12)
        XCTAssertThrowsError(try OrientationMath.riderMotion(sample: MotionSample(), calibration: calibration, convention: RotationConvention()))
    }
    func testStandardMountShortcutRequiresBothSignChecks() throws {
        let neutral = samples(Quaternion())
        let left = samples(OrientationMath.quaternion(rotationVector: Vector3(z: -0.3)))
        let forward = samples(OrientationMath.quaternion(rotationVector: Vector3(x: 0.3)))
        let calibration = try CalibrationSolver.fitStandardMount(neutral: neutral, left: left, forward: forward, convention: convention)
        XCTAssertEqual(calibration.deviceToRiderBasis.m21, 1)
        let value = try OrientationMath.riderMotion(sample: MotionSample(attitudeQuaternion: OrientationMath.quaternion(rotationVector: Vector3(z: -0.3))), calibration: calibration, convention: convention)
        XCTAssertEqual(value.displayAnglesRadians.x, 0.3, accuracy: 1e-12)
        XCTAssertThrowsError(try CalibrationSolver.fitStandardMount(neutral: neutral, left: forward, forward: left, convention: convention))
    }
    func testRejectShortMovingCollinearAndUnverified() throws {
        let good = try fixture()
        XCTAssertTrue(good.verified)
        let left = samples(OrientationMath.quaternion(rotationVector: Vector3(x: -0.3)))
        let forward = samples(OrientationMath.quaternion(rotationVector: Vector3(y: 0.3)))
        XCTAssertThrowsError(try CalibrationSolver.fit(neutral: [], left: left, forward: forward, convention: convention))
        XCTAssertThrowsError(try CalibrationSolver.fit(neutral: samples(Quaternion(), duration: 1), left: left, forward: forward, convention: convention))
        var moving = samples(Quaternion()); moving[10].rotationRate.x = 1
        var dispersed = samples(Quaternion())
        dispersed[0].attitudeQuaternion = OrientationMath.quaternion(rotationVector: Vector3(z: 0.2))
        XCTAssertThrowsError(try CalibrationSolver.fit(neutral: dispersed, left: left, forward: forward, convention: convention))
        XCTAssertThrowsError(try CalibrationSolver.fit(neutral: moving, left: left, forward: forward, convention: convention))
        XCTAssertThrowsError(try CalibrationSolver.fit(neutral: samples(Quaternion()), left: left, forward: left, convention: convention))
        XCTAssertThrowsError(try CalibrationSolver.fit(neutral: samples(Quaternion()), left: left, forward: forward, convention: RotationConvention()))
    }
}