#include <unity.h>

#include <vector>

#include "geo.h"

using kraftort::geo::Place;

void test_haversine_basics() {
  const double d0 = kraftort::geo::haversineMeters(47.0, 8.0, 47.0, 8.0);
  TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, static_cast<float>(d0));

  const double d1 = kraftort::geo::haversineMeters(47.3769, 8.5417, 47.4979, 8.7241);
  TEST_ASSERT_TRUE(d1 > 18000.0);
  TEST_ASSERT_TRUE(d1 < 21000.0);
}

void test_bearing_and_led_index() {
  const double bearing = kraftort::geo::initialBearingDeg(47.0, 8.0, 48.0, 8.0);
  TEST_ASSERT_FLOAT_WITHIN(1.0f, 0.0f, static_cast<float>(bearing));

  const int idx = kraftort::geo::bearingToLedIndex(90.0, 0.0, 54);
  TEST_ASSERT_TRUE(idx >= 13 && idx <= 14);
}

void test_nearest_place() {
  std::vector<Place> places = {
      {"A", 47.0, 8.0, 100.0f},
      {"B", 47.5, 8.5, 100.0f},
  };
  double distance = 0.0;
  const std::size_t idx = kraftort::geo::findNearestPlaceIndex(places, 47.02, 8.02, &distance);
  TEST_ASSERT_EQUAL_UINT32(0u, static_cast<uint32_t>(idx));
  TEST_ASSERT_TRUE(distance < 5000.0);
}

int runUnityTests(void) {
  UNITY_BEGIN();
  RUN_TEST(test_haversine_basics);
  RUN_TEST(test_bearing_and_led_index);
  RUN_TEST(test_nearest_place);
  return UNITY_END();
}

#ifdef ARDUINO
#include <Arduino.h>
void setup() { delay(1000); runUnityTests(); }
void loop() {}
#else
int main() { return runUnityTests(); }
#endif