#include <cmath>
#include <cstdlib>
#include <iostream>
#include <odometry/odom_differential.h>

namespace
{
int g_failures = 0;

void expect_true(bool condition, const char *expr, const char *file, int line)
{
  if (!condition)
  {
    std::cerr << "FAIL: " << file << ":" << line << " " << expr << "\n";
    ++g_failures;
  }
}

void expect_near(double actual, double expected, double tolerance,
                 const char *expr, const char *file, int line)
{
  if (std::fabs(actual - expected) > tolerance)
  {
    std::cerr << "FAIL: " << file << ":" << line << " " << expr
              << " (actual=" << actual << ", expected=" << expected
              << ", tolerance=" << tolerance << ")\n";
    ++g_failures;
  }
}

#define EXPECT_TRUE(expr) expect_true((expr), #expr, __FILE__, __LINE__)
#define EXPECT_NEAR(actual, expected, tolerance) \
  expect_near((actual), (expected), (tolerance), #actual, __FILE__, __LINE__)

void test_initial_state_after_init()
{
  differential::Odometry odom(10);
  odom.setWheelParams(0.5, 0.09, 0.09);
  odom.init(100.0);

  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
  EXPECT_NEAR(odom.getY(), 0.0, 1e-9);
  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-9);
  EXPECT_NEAR(odom.getLinear(), 0.0, 1e-9);
  EXPECT_NEAR(odom.getAngular(), 0.0, 1e-9);
}

void test_straight_line_motion()
{
  differential::Odometry odom(1);
  const double wheel_radius = 0.09;
  const double wheel_separation = 0.5;
  odom.setWheelParams(wheel_separation, wheel_radius, wheel_radius);

  double t = 0.0;
  odom.init(t);

  const double wheel_vel = 2.0; // [rad/s]
  const double dt = 0.1;
  const int steps = 10;

  for (int i = 0; i < steps; ++i)
  {
    t += dt;
    EXPECT_TRUE(odom.updateWheel(wheel_vel, wheel_vel, t));
  }

  const double linear_vel = wheel_radius * wheel_vel;
  const double expected_x = linear_vel * dt * steps;

  EXPECT_NEAR(odom.getY(), 0.0, 1e-6);
  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-6);
  EXPECT_NEAR(odom.getX(), expected_x, 1e-6);
  EXPECT_NEAR(odom.getLinear(), linear_vel, 1e-3);
  EXPECT_NEAR(odom.getAngular(), 0.0, 1e-3);
}

void test_turn_motion()
{
  differential::Odometry odom(1);
  const double wheel_radius = 0.09;
  const double wheel_separation = 0.5;
  odom.setWheelParams(wheel_separation, wheel_radius, wheel_radius);

  double t = 0.0;
  odom.init(t);

  const double left_vel = 2.0;   // [rad/s]
  const double right_vel = 1.0;  // [rad/s]
  const double dt = 0.1;

  t += dt;
  EXPECT_TRUE(odom.updateWheel(left_vel, right_vel, t));

  const double left_lin = wheel_radius * left_vel;
  const double right_lin = wheel_radius * right_vel;
  const double expected_linear = (right_lin + left_lin) * 0.5;
  const double expected_angular = (right_lin - left_lin) / wheel_separation;
  const double expected_heading = expected_angular * dt;
  const double expected_x = expected_linear * dt;

  EXPECT_NEAR(odom.getHeading(), expected_heading, 1e-6);
  EXPECT_NEAR(odom.getX(), expected_x, 1e-6);
  EXPECT_NEAR(odom.getLinear(), expected_linear, 1e-3);
  EXPECT_NEAR(odom.getAngular(), expected_angular, 1e-3);
}

void test_update_rejects_small_dt()
{
  differential::Odometry odom(1);
  odom.setWheelParams(0.5, 0.09, 0.09);

  double t = 1.0;
  odom.init(t);

  const double linear_before = odom.getLinear();
  const double angular_before = odom.getAngular();
  EXPECT_TRUE(!odom.updateWheel(1.0, 1.0, t + 1e-5));
  EXPECT_NEAR(odom.getLinear(), linear_before, 1e-9);
  EXPECT_NEAR(odom.getAngular(), angular_before, 1e-9);
  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
}

void test_update_open_loop()
{
  differential::Odometry odom(1);
  odom.setWheelParams(0.5, 0.09, 0.09);

  double t = 0.0;
  odom.init(t);

  const double linear = 0.2;
  const double angular = 0.0;
  const double dt = 0.5;

  t += dt;
  odom.updateOdom(linear, angular, t);

  EXPECT_NEAR(odom.getLinear(), linear, 1e-9);
  EXPECT_NEAR(odom.getAngular(), angular, 1e-9);
  EXPECT_NEAR(odom.getX(), linear * dt, 1e-6);
  EXPECT_NEAR(odom.getY(), 0.0, 1e-6);
  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-6);
}

void test_zero_velocity_keeps_pose()
{
  differential::Odometry odom(1);
  odom.setWheelParams(0.5, 0.09, 0.09);
  odom.init(0.0);

  EXPECT_TRUE(odom.updateWheel(0.0, 0.0, 0.1));
  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
  EXPECT_NEAR(odom.getY(), 0.0, 1e-9);
  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-9);
  EXPECT_NEAR(odom.getLinear(), 0.0, 1e-9);
  EXPECT_NEAR(odom.getAngular(), 0.0, 1e-9);
}

void test_ticks_seed_then_straight()
{
  differential::Odometry odom(1);
  const double wheel_radius = 0.09;
  const double wheel_separation = 0.5;
  const double ticks_per_rev = 4096.0;
  odom.setWheelParams(wheel_separation, wheel_radius, wheel_radius);
  odom.setEncoderParams(ticks_per_rev);
  odom.init(0.0);

  // First sample only seeds previous ticks.
  EXPECT_TRUE(!odom.updateTicks(1000, 1000, 0.0));
  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);

  // One full wheel revolution on both sides → forward 2*pi*r
  const std::int64_t delta = static_cast<std::int64_t>(ticks_per_rev);
  EXPECT_TRUE(odom.updateTicks(1000 + delta, 1000 + delta, 1.0));

  const double expected_x = 2.0 * M_PI * wheel_radius;
  EXPECT_NEAR(odom.getY(), 0.0, 1e-6);
  EXPECT_NEAR(odom.getHeading(), 0.0, 1e-6);
  EXPECT_NEAR(odom.getX(), expected_x, 1e-6);
  EXPECT_NEAR(odom.getLinear(), expected_x / 1.0, 1e-3);
  EXPECT_NEAR(odom.getAngular(), 0.0, 1e-3);
}

void test_ticks_turn_in_place()
{
  differential::Odometry odom(1);
  const double wheel_radius = 0.09;
  const double wheel_separation = 0.5;
  const double ticks_per_rev = 4096.0;
  odom.setWheelParams(wheel_separation, wheel_radius, wheel_radius);
  odom.setEncoderParams(ticks_per_rev);
  odom.init(0.0);

  EXPECT_TRUE(!odom.updateTicks(0, 0, 0.0));

  // Left backward, right forward by the same arc → pure rotation.
  const double arc = 0.1; // [m]
  const std::int64_t delta = static_cast<std::int64_t>(
      (arc / (2.0 * M_PI * wheel_radius)) * ticks_per_rev);

  EXPECT_TRUE(odom.updateTicks(-delta, delta, 0.2));

  const double expected_heading = (2.0 * arc) / wheel_separation;
  EXPECT_NEAR(odom.getX(), 0.0, 1e-4);
  EXPECT_NEAR(odom.getY(), 0.0, 1e-4);
  EXPECT_NEAR(odom.getHeading(), expected_heading, 1e-3);
}

void test_ticks_rejects_unset_encoder_params()
{
  differential::Odometry odom(1);
  odom.setWheelParams(0.5, 0.09, 0.09);
  odom.init(0.0);

  EXPECT_TRUE(!odom.updateTicks(0, 0, 0.0));   // seed
  EXPECT_TRUE(!odom.updateTicks(10, 10, 0.1)); // CPR not set
  EXPECT_NEAR(odom.getX(), 0.0, 1e-9);
}

} // namespace

int main()
{
  test_initial_state_after_init();
  test_straight_line_motion();
  test_turn_motion();
  test_update_rejects_small_dt();
  test_update_open_loop();
  test_zero_velocity_keeps_pose();
  test_ticks_seed_then_straight();
  test_ticks_turn_in_place();
  test_ticks_rejects_unset_encoder_params();

  if (g_failures == 0)
  {
    std::cout << "All tests passed.\n";
    return EXIT_SUCCESS;
  }

  std::cerr << g_failures << " test(s) failed.\n";
  return EXIT_FAILURE;
}