#include <unity.h>

#include <cstdio>
#include <string>
#include <vector>

#include "nmea_parser.h"

using namespace roro;
using namespace roro::gnss;

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

// "$<body>*<checksum>": the checksum is computed here, so scenarios stay readable.
static std::string nmea(const std::string& body) {
    uint8_t sum = 0;
    for (char c : body) sum ^= static_cast<uint8_t>(c);
    char tail[4];
    std::snprintf(tail, sizeof tail, "*%02X", sum);
    return "$" + body + tail;
}

static void feedLine(NmeaParser& p, const std::string& line) {
    std::string withEnd = line + "\r\n";
    p.feed(withEnd.data(), withEnd.size());
}

void test_checksum_is_verified() {
    NmeaParser p;
    TEST_ASSERT_TRUE(p.sentence("$GNVTG,,,,,,,,,N*2E"));  // captured from the device
    TEST_ASSERT_FALSE(p.sentence("$GNVTG,,,,,,,,,N*2F"));
    TEST_ASSERT_FALSE(p.sentence("$GNVTG,,,,,,,,,N"));
    TEST_ASSERT_FALSE(p.sentence("GNVTG,,,,,,,,,N*2E"));
    TEST_ASSERT_EQUAL_UINT32(3, p.badSentences());
    TEST_ASSERT_EQUAL_UINT32(1, p.goodSentences());
}

void test_no_fix_as_captured_indoors() {
    NmeaParser p;
    feedLine(p, "$GNRMC,200231.00,V,,,,,,,041026,,,N,V*1A");
    feedLine(p, "$GNGGA,200231.00,,,,,0,00,25.5,,,,,,*48");
    feedLine(p, "$GNGSA,A,1,,,,,,,,,,,,,25.5,25.5,25.5,1*01");
    feedLine(p, "$GPGSV,1,1,01,25,41,108,17,1*58");
    feedLine(p, "$GLGSV,1,1,00,1*78");
    const GnssState& s = p.state();
    TEST_ASSERT_TRUE(s.fix == FixType::None);
    TEST_ASSERT_FALSE(s.positionValid);
    TEST_ASSERT_FALSE(s.timeValid);  // a time without a Fix isn't trusted
    TEST_ASSERT_EQUAL(1, static_cast<int>(s.satellites.size()));
    TEST_ASSERT_TRUE(s.satellites[0].system == Constellation::Gps);
    TEST_ASSERT_EQUAL(25, s.satellites[0].prn);
    TEST_ASSERT_EQUAL(41, s.satellites[0].elevation);
    TEST_ASSERT_EQUAL(108, s.satellites[0].azimuth);
    TEST_ASSERT_EQUAL(17, s.satellites[0].snr);
    TEST_ASSERT_FALSE(s.satellites[0].used);
}

void test_rmc_with_a_fix_gives_position_time_speed_course() {
    NmeaParser p;
    feedLine(p, nmea("GNRMC,123519.00,A,5050.8136,N,00421.1532,E,10.0,84.4,041026,,,A,V"));
    const GnssState& s = p.state();
    TEST_ASSERT_TRUE(s.positionValid);
    TEST_ASSERT_FLOAT_WITHIN(1e-6, 50.0 + 50.8136 / 60.0, s.latitude);
    TEST_ASSERT_FLOAT_WITHIN(1e-6, 4.0 + 21.1532 / 60.0, s.longitude);
    TEST_ASSERT_TRUE(s.timeValid);
    TEST_ASSERT_EQUAL(2026, s.year);
    TEST_ASSERT_EQUAL(10, s.month);
    TEST_ASSERT_EQUAL(4, s.day);
    TEST_ASSERT_EQUAL(12, s.hour);
    TEST_ASSERT_EQUAL(35, s.minute);
    TEST_ASSERT_EQUAL(19, s.second);
    TEST_ASSERT_FLOAT_WITHIN(0.01, 18.52, s.speedKmh);  // 10 knots
    TEST_ASSERT_TRUE(s.courseValid);
    TEST_ASSERT_FLOAT_WITHIN(0.01, 84.4, s.courseDeg);
}

void test_southern_and_western_hemispheres_are_negative() {
    NmeaParser p;
    feedLine(p, nmea("GNRMC,000000.00,A,3352.0000,S,15112.6000,W,0.0,,010126,,,A,V"));
    TEST_ASSERT_FLOAT_WITHIN(1e-6, -(33.0 + 52.0 / 60.0), p.state().latitude);
    TEST_ASSERT_FLOAT_WITHIN(1e-6, -(151.0 + 12.6 / 60.0), p.state().longitude);
    TEST_ASSERT_FALSE(p.state().courseValid);  // empty course field
}

void test_utc_seconds_from_the_fix_time() {
    NmeaParser p;
    feedLine(p, nmea("GNRMC,123519.00,A,5050.8136,N,00421.1532,E,0.0,,041026,,,A,V"));
    TEST_ASSERT_EQUAL_INT64(1791117319LL, p.state().utcSeconds());  // 2026-10-04 12:35:19 UTC
}

void test_gga_gives_fix_quality_altitude_satellites_and_hdop() {
    NmeaParser p;
    feedLine(p, nmea("GNGGA,123519.00,5050.8136,N,00421.1532,E,1,09,0.9,61.4,M,46.9,M,,"));
    feedLine(p, nmea("GNGSA,A,3,05,13,15,18,23,,,,,,,,1.6,0.9,1.3,1"));
    const GnssState& s = p.state();
    TEST_ASSERT_TRUE(s.positionValid);
    TEST_ASSERT_EQUAL(9, s.satellitesUsed);
    TEST_ASSERT_FLOAT_WITHIN(0.01, 0.9, s.hdop);
    TEST_ASSERT_TRUE(s.altitudeValid);
    TEST_ASSERT_FLOAT_WITHIN(0.01, 61.4, s.altitudeM);
    TEST_ASSERT_TRUE(s.fix == FixType::ThreeD);
}

void test_fix_type_comes_from_gsa_mode() {
    NmeaParser p;
    feedLine(p, nmea("GNGSA,A,2,05,13,15,,,,,,,,,,2.1,1.9,1.0,1"));
    TEST_ASSERT_TRUE(p.state().fix == FixType::TwoD);
    feedLine(p, nmea("GNGSA,A,1,,,,,,,,,,,,,25.5,25.5,25.5,1"));
    TEST_ASSERT_TRUE(p.state().fix == FixType::None);
}

void test_losing_the_fix_drops_the_position() {
    NmeaParser p;
    feedLine(p, nmea("GNRMC,123519.00,A,5050.8136,N,00421.1532,E,0.0,,041026,,,A,V"));
    TEST_ASSERT_TRUE(p.state().positionValid);
    feedLine(p, nmea("GNRMC,123520.00,V,,,,,,,041026,,,N,V"));
    TEST_ASSERT_FALSE(p.state().positionValid);
    TEST_ASSERT_FALSE(p.state().timeValid);
}

void test_gsv_sequence_spans_several_messages() {
    NmeaParser p;
    feedLine(p, nmea("GPGSV,2,1,05,05,60,120,40,13,45,200,35,15,30,300,30,18,20,050,25,1"));
    TEST_ASSERT_EQUAL(0, static_cast<int>(p.state().satellites.size()));  // committed at the last message
    feedLine(p, nmea("GPGSV,2,2,05,23,10,010,,1"));
    const auto& sats = p.state().satellites;
    TEST_ASSERT_EQUAL(5, static_cast<int>(sats.size()));
    TEST_ASSERT_EQUAL(-1, sats[4].snr);  // not tracked: empty SNR
}

void test_each_constellation_keeps_its_own_satellites() {
    NmeaParser p;
    feedLine(p, nmea("GPGSV,1,1,01,05,60,120,40,1"));
    feedLine(p, nmea("GLGSV,1,1,01,70,30,90,33,1"));
    feedLine(p, nmea("GAGSV,1,1,01,11,20,45,28,7"));
    feedLine(p, nmea("GBGSV,1,1,01,24,50,270,36,1"));
    feedLine(p, nmea("GQGSV,1,1,01,02,70,180,41,1"));
    const auto& sats = p.state().satellites;
    TEST_ASSERT_EQUAL(5, static_cast<int>(sats.size()));
    int seen[8] = {};
    for (auto& s : sats) seen[static_cast<int>(s.system)]++;
    TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::Gps)]);
    TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::Glonass)]);
    TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::Galileo)]);
    TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::BeiDou)]);
    TEST_ASSERT_EQUAL(1, seen[static_cast<int>(Constellation::Qzss)]);
}

void test_a_satellite_on_two_bands_counts_once_with_the_stronger_signal() {
    NmeaParser p;
    feedLine(p, nmea("GPGSV,1,1,01,05,60,120,30,1"));  // L1
    feedLine(p, nmea("GPGSV,1,1,01,05,60,120,42,8"));  // L5
    const auto& sats = p.state().satellites;
    TEST_ASSERT_EQUAL(1, static_cast<int>(sats.size()));
    TEST_ASSERT_EQUAL(42, sats[0].snr);
}

void test_an_empty_gsv_clears_that_constellation() {
    NmeaParser p;
    feedLine(p, nmea("GLGSV,1,1,01,70,30,90,33,1"));
    TEST_ASSERT_EQUAL(1, static_cast<int>(p.state().satellites.size()));
    feedLine(p, "$GLGSV,1,1,00,1*78");  // captured
    TEST_ASSERT_EQUAL(0, static_cast<int>(p.state().satellites.size()));
}

void test_gsa_marks_the_satellites_used_per_system() {
    NmeaParser p;
    feedLine(p, nmea("GPGSV,1,1,02,05,60,120,40,13,45,200,35,1"));
    feedLine(p, nmea("GBGSV,1,1,01,05,50,270,36,1"));  // BeiDou 5, not GPS 5
    feedLine(p, nmea("GNGSA,A,3,05,,,,,,,,,,,,1.6,0.9,1.3,1"));  // GPS: 5 used
    feedLine(p, nmea("GNGSA,A,3,,,,,,,,,,,,,1.6,0.9,1.3,4"));    // BeiDou: none
    for (auto& s : p.state().satellites) {
        bool expected = s.system == Constellation::Gps && s.prn == 5;
        TEST_ASSERT_EQUAL(expected, s.used);
    }
}

void test_split_feeds_and_noise_are_handled() {
    NmeaParser p;
    std::string a = "garbage\r\n" + nmea("GNGGA,123519.00,5050.8136,N,00421.1532,E,1,09,0.9,61.4,M,46.9,M,,");
    std::string b = "\r\n";
    p.feed(a.data(), 20);
    p.feed(a.data() + 20, a.size() - 20);
    TEST_ASSERT_FALSE(p.state().positionValid);  // no line end yet
    p.feed(b.data(), b.size());
    TEST_ASSERT_TRUE(p.state().positionValid);
}

void test_an_overlong_line_is_dropped() {
    NmeaParser p;
    std::string junk(300, 'x');
    junk += "\r\n";
    p.feed(junk.data(), junk.size());
    feedLine(p, nmea("GNGGA,123519.00,5050.8136,N,00421.1532,E,1,09,0.9,61.4,M,46.9,M,,"));
    TEST_ASSERT_TRUE(p.state().positionValid);
}

void test_every_complete_line_reaches_the_echo() {
    NmeaParser p;
    std::vector<std::string> seen;
    p.onLine = [&](const std::string& l) { seen.push_back(l); };
    std::string data = "$GNVTG,,,,,,,,,N*2E\r\nbroken\r\n$GN";
    p.feed(data.data(), data.size());
    TEST_ASSERT_EQUAL(2, static_cast<int>(seen.size()));
    TEST_ASSERT_EQUAL_STRING("$GNVTG,,,,,,,,,N*2E", seen[0].c_str());
    TEST_ASSERT_EQUAL_STRING("broken", seen[1].c_str());
}

void test_constellation_names() {
    TEST_ASSERT_EQUAL_STRING("GPS", constellationName(Constellation::Gps));
    TEST_ASSERT_EQUAL_STRING("Galileo", constellationName(Constellation::Galileo));
    TEST_ASSERT_EQUAL_STRING("BeiDou", constellationName(Constellation::BeiDou));
}

int main() {
    UNITY_BEGIN();
    RUN_TEST(test_checksum_is_verified);
    RUN_TEST(test_no_fix_as_captured_indoors);
    RUN_TEST(test_rmc_with_a_fix_gives_position_time_speed_course);
    RUN_TEST(test_southern_and_western_hemispheres_are_negative);
    RUN_TEST(test_utc_seconds_from_the_fix_time);
    RUN_TEST(test_gga_gives_fix_quality_altitude_satellites_and_hdop);
    RUN_TEST(test_fix_type_comes_from_gsa_mode);
    RUN_TEST(test_losing_the_fix_drops_the_position);
    RUN_TEST(test_gsv_sequence_spans_several_messages);
    RUN_TEST(test_each_constellation_keeps_its_own_satellites);
    RUN_TEST(test_a_satellite_on_two_bands_counts_once_with_the_stronger_signal);
    RUN_TEST(test_an_empty_gsv_clears_that_constellation);
    RUN_TEST(test_gsa_marks_the_satellites_used_per_system);
    RUN_TEST(test_split_feeds_and_noise_are_handled);
    RUN_TEST(test_an_overlong_line_is_dropped);
    RUN_TEST(test_every_complete_line_reaches_the_echo);
    RUN_TEST(test_constellation_names);
    return UNITY_END();
}