#include <unity.h>

#include <cmath>
#include <string>

#include "compass.h"

using namespace roro::gnss;

void setUp() {}
void tearDown() {}

namespace {
const float kFlat[3] = {0, 0, 1};
const float kNoTurn[3] = {0, 0, 0};

// `seconds` of readings at 100 Hz.
void run(HeadingFilter& f, const float gyro[3], const float accel[3], float seconds) {
    for (int i = 0; i < static_cast<int>(seconds * 100 + 0.5f); i++) f.motion(gyro, accel, 0.01f);
}
}  // namespace

void test_angles_wrap() {
    TEST_ASSERT_FLOAT_WITHIN(0.001, 350.0, wrap360(-10.0f));
    TEST_ASSERT_FLOAT_WITHIN(0.001, 10.0, wrap360(370.0f));
    TEST_ASSERT_FLOAT_WITHIN(0.001, 20.0, angleDiff(10.0f, 350.0f));
    TEST_ASSERT_FLOAT_WITHIN(0.001, -20.0, angleDiff(350.0f, 10.0f));
    TEST_ASSERT_FLOAT_WITHIN(0.001, -180.0, angleDiff(180.0f, 0.0f));
}

void test_bearing_to_a_point() {
    TEST_ASSERT_FLOAT_WITHIN(0.01, 0.0, bearingDegrees(50.0, 4.0, 51.0, 4.0));
    TEST_ASSERT_FLOAT_WITHIN(0.01, 180.0, bearingDegrees(50.0, 4.0, 49.0, 4.0));
    TEST_ASSERT_FLOAT_WITHIN(0.5, 90.0, bearingDegrees(50.0, 4.0, 50.0, 4.1));
    TEST_ASSERT_FLOAT_WITHIN(0.5, 270.0, bearingDegrees(50.0, 4.0, 50.0, 3.9));
    // Brussels to Paris: south-south-west.
    TEST_ASSERT_FLOAT_WITHIN(1.0, 214.0, bearingDegrees(50.8503, 4.3517, 48.8566, 2.3522));
}

void test_cardinal_points() {
    TEST_ASSERT_EQUAL_STRING("N", cardinal(0));
    TEST_ASSERT_EQUAL_STRING("N", cardinal(359));
    TEST_ASSERT_EQUAL_STRING("NNE", cardinal(22.5f));
    TEST_ASSERT_EQUAL_STRING("E", cardinal(90));
    TEST_ASSERT_EQUAL_STRING("WSW", cardinal(247));
    TEST_ASSERT_EQUAL_STRING("NNW", cardinal(340));
    TEST_ASSERT_EQUAL_STRING("N", cardinal(-5));
}

void test_distances_read_well() {
    TEST_ASSERT_EQUAL_STRING("0 m", formatDistance(0.2).c_str());
    TEST_ASSERT_EQUAL_STRING("340 m", formatDistance(340.4).c_str());
    TEST_ASSERT_EQUAL_STRING("1.0 km", formatDistance(999.6).c_str());
    TEST_ASSERT_EQUAL_STRING("1.2 km", formatDistance(1234).c_str());
    TEST_ASSERT_EQUAL_STRING("12 km", formatDistance(12345).c_str());
}

void test_the_sun_at_an_equinox() {
    // 2024-03-20 03:06 UTC, the March equinox: the sun is over the equator. Twelve hours of
    // turning later, at 12:07:30 UTC (the equation of time is -7.5 min), it is over Greenwich.
    int64_t noon = 1710936450;
    SunPosition s = sunPosition(noon, 0.0, 0.0);
    TEST_ASSERT_FLOAT_WITHIN(0.3, 90.0, s.elevationDeg);
    s = sunPosition(noon, 50.0, 0.0);  // from 50° north: due south, 40° up
    TEST_ASSERT_FLOAT_WITHIN(0.5, 180.0, s.azimuthDeg);
    TEST_ASSERT_FLOAT_WITHIN(0.3, 40.0, s.elevationDeg);
    s = sunPosition(noon, -50.0, 0.0);  // from 50° south: due north
    TEST_ASSERT_TRUE(s.azimuthDeg < 1.0f || s.azimuthDeg > 359.0f);
    // Six hours earlier it rises, due east at an equinox, anywhere.
    s = sunPosition(noon - 6 * 3600, 50.0, 0.0);
    TEST_ASSERT_FLOAT_WITHIN(1.0, 90.0, s.azimuthDeg);
    TEST_ASSERT_FLOAT_WITHIN(1.0, 0.0, s.elevationDeg);
    s = sunPosition(noon + 6 * 3600, 50.0, 0.0);  // and sets due west
    TEST_ASSERT_FLOAT_WITHIN(1.0, 270.0, s.azimuthDeg);
}

void test_the_sun_at_a_solstice() {
    // 2024-06-20 20:51 UTC: 23.44° north of the equator. At noon in Brussels that day
    // (11:44 UTC by the sun) it stands 90 - 50.85 + 23.44 = 62.6° up, in the south.
    SunPosition s = sunPosition(1718883840, 50.85, 4.35);
    TEST_ASSERT_FLOAT_WITHIN(1.0, 180.0, s.azimuthDeg);
    TEST_ASSERT_FLOAT_WITHIN(0.3, 62.6, s.elevationDeg);
    // At midnight it is under the horizon, in the north.
    s = sunPosition(1718883840 + 12 * 3600, 50.85, 4.35);
    TEST_ASSERT_TRUE(s.elevationDeg < 0);
    TEST_ASSERT_TRUE(s.azimuthDeg < 3.0f || s.azimuthDeg > 357.0f);
}

void test_no_heading_until_north_is_given() {
    HeadingFilter f;
    const float turn[3] = {0, 0, 30};
    run(f, turn, kFlat, 1);
    TEST_ASSERT_FALSE(f.known());
    f.course(120, 1.0f);  // too slow: the course means nothing
    TEST_ASSERT_FALSE(f.known());
    f.course(120, 4.0f);
    TEST_ASSERT_TRUE(f.known());
    TEST_ASSERT_FLOAT_WITHIN(0.01, 120.0, f.heading());
    TEST_ASSERT_TRUE(f.source() == HeadingFilter::Source::Course);
}

void test_the_gyroscope_follows_a_turn() {
    HeadingFilter f;
    f.course(100, 5.0f);
    // A positive turn around "up" is anticlockwise from above: the heading goes down.
    const float left[3] = {0, 0, 45};
    run(f, left, kFlat, 2);
    TEST_ASSERT_FLOAT_WITHIN(0.5, 10.0, f.heading());
    const float right[3] = {0, 0, -45};
    run(f, right, kFlat, 4);
    TEST_ASSERT_FLOAT_WITHIN(0.5, 190.0, f.heading());
    TEST_ASSERT_FLOAT_WITHIN(0.05, 6.0, f.heldSeconds());
}

void test_only_the_turn_around_the_vertical_counts() {
    HeadingFilter f;
    f.course(0, 5.0f);
    // Held upright, the screen facing the user: "up" is along the sensor's y.
    const float upright[3] = {0, 1, 0};
    run(f, kNoTurn, upright, 1);
    const float roll[3] = {0, 0, 90};  // around the screen's own axis: no change of heading
    run(f, roll, upright, 1);
    TEST_ASSERT_TRUE(std::fabs(angleDiff(f.heading(), 0)) < 0.5f);
    const float yaw[3] = {0, -90, 0};  // around the vertical
    run(f, yaw, upright, 1);
    TEST_ASSERT_FLOAT_WITHIN(1.0, 90.0, f.heading());
}

void test_the_course_pulls_the_heading_gently() {
    HeadingFilter f;
    f.course(350, 5.0f);
    f.course(10, 5.0f);  // through north, the short way
    TEST_ASSERT_FLOAT_WITHIN(0.01, 356.0, f.heading());
    for (int i = 0; i < 20; i++) f.course(10, 5.0f);
    TEST_ASSERT_FLOAT_WITHIN(0.1, 10.0, f.heading());
    TEST_ASSERT_FLOAT_WITHIN(0.001, 0.0, f.heldSeconds());
}

void test_the_sun_sets_north() {
    HeadingFilter f;
    f.sun(203);
    TEST_ASSERT_TRUE(f.known());
    TEST_ASSERT_TRUE(f.source() == HeadingFilter::Source::Sun);
    TEST_ASSERT_FLOAT_WITHIN(0.01, 203.0, f.heading());
    f.forget();
    TEST_ASSERT_FALSE(f.known());
}

void test_the_gyroscope_offset_is_learnt_at_rest() {
    HeadingFilter f;
    f.sun(90);
    const float offset[3] = {0.3f, -0.2f, 0.8f};  // what a BMI270 reads lying still
    run(f, offset, kFlat, 30);
    TEST_ASSERT_TRUE(f.still());
    float before = f.heading();
    run(f, offset, kFlat, 60);  // a minute later it has not drifted
    TEST_ASSERT_FLOAT_WITHIN(0.2, before, f.heading());
    // A real turn is still measured, with the offset taken off.
    const float turn[3] = {0.3f, -0.2f, 0.8f - 90.0f};
    run(f, turn, kFlat, 1);
    TEST_ASSERT_FLOAT_WITHIN(0.5, before + 90.0f, f.heading());
    TEST_ASSERT_FALSE(f.still());
}

void test_tilt_follows_the_accelerometer() {
    HeadingFilter f;
    run(f, kNoTurn, kFlat, 1);
    TEST_ASSERT_FLOAT_WITHIN(0.01, 0.0, f.tiltX());
    TEST_ASSERT_FLOAT_WITHIN(0.01, 0.0, f.tiltY());
    const float tilted[3] = {0.5f, 0, 0.866f};  // 30° over
    run(f, kNoTurn, tilted, 2);
    TEST_ASSERT_FLOAT_WITHIN(0.01, 0.5, f.tiltX());
    const float shaken[3] = {3, 3, 3};  // not gravity: ignored
    run(f, kNoTurn, shaken, 1);
    TEST_ASSERT_FLOAT_WITHIN(0.01, 0.5, f.tiltX());
}

int main() {
    UNITY_BEGIN();
    RUN_TEST(test_angles_wrap);
    RUN_TEST(test_bearing_to_a_point);
    RUN_TEST(test_cardinal_points);
    RUN_TEST(test_distances_read_well);
    RUN_TEST(test_the_sun_at_an_equinox);
    RUN_TEST(test_the_sun_at_a_solstice);
    RUN_TEST(test_no_heading_until_north_is_given);
    RUN_TEST(test_the_gyroscope_follows_a_turn);
    RUN_TEST(test_only_the_turn_around_the_vertical_counts);
    RUN_TEST(test_the_course_pulls_the_heading_gently);
    RUN_TEST(test_the_sun_sets_north);
    RUN_TEST(test_the_gyroscope_offset_is_learnt_at_rest);
    RUN_TEST(test_tilt_follows_the_accelerometer);
    return UNITY_END();
}