#include "origincar_base/wall_kalman_filter.hpp"

#include <cmath>
#include <iostream>
#include <stdexcept>
#include <string>

namespace
{
constexpr double kPi = 3.14159265358979323846;

void requireNear(double actual, double expected, double tolerance, const std::string &message)
{
  if (std::fabs(actual - expected) > tolerance)
  {
    throw std::runtime_error(message + " actual=" + std::to_string(actual) +
                             " expected=" + std::to_string(expected));
  }
}

void require(bool condition, const std::string &message)
{
  if (!condition)
  {
    throw std::runtime_error(message);
  }
}

void testPredictUsesWheelVelocityAndImuYawRate()
{
  origincar_wall::WallKalmanFilter filter({0.56, 0.18, 0.0});

  filter.predict(1.0, 0.0, 0.2, 0.5);
  const auto pose = filter.pose();

  requireNear(pose.x, 1.06, 1e-9, "predicted x");
  requireNear(pose.y, 0.18, 1e-9, "predicted y");
  requireNear(pose.theta, 0.1, 1e-9, "predicted theta");
}

void testScanUpdateTrustsLowNoiseMoreThanHighNoise()
{
  origincar_wall::WallKalmanFilter weak_filter({0.0, 0.0, 0.0});
  origincar_wall::WallKalmanFilter strong_filter({0.0, 0.0, 0.0});
  weak_filter.predict(1.0, 0.0, 0.0, 1.0);
  strong_filter.predict(1.0, 0.0, 0.0, 1.0);

  weak_filter.correct({2.0, 0.0, 0.0}, {1.0, 1.0, 1.0});
  strong_filter.correct({2.0, 0.0, 0.0}, {0.01, 0.01, 0.01});

  require(strong_filter.pose().x > weak_filter.pose().x, "low scan noise should pull x closer to scan");
  requireNear(strong_filter.pose().x, 2.0, 0.08, "strong scan update x");
}

void testYawCorrectionUsesWrappedInnovation()
{
  origincar_wall::WallKalmanFilter filter({0.0, 0.0, 179.0 * kPi / 180.0});

  filter.correct({0.0, 0.0, -179.0 * kPi / 180.0}, {0.01, 0.01, 0.01});

  require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary");
}

void testScanPolicyGradesByWallCountAndMeanError()
{
  const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
  const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0};

  const auto high = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, scan_pose, {4, 0.04, true}, false);
  require(high.accept, "4 walls with low error should be accepted");
  require(high.trust == origincar_wall::WallScanTrust::High, "4 walls with low error should be high trust");
  requireNear(high.noise.x, 0.015, 1e-12, "high trust x noise");

  const auto medium = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, scan_pose, {3, 0.08, true}, false);
  require(medium.accept, "3 walls with medium error should be accepted");
  require(medium.trust == origincar_wall::WallScanTrust::Medium, "3 walls with medium error should be medium trust");
  requireNear(medium.noise.x, 0.045, 1e-12, "medium trust x noise");

  const auto weak = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, scan_pose, {3, 0.12, true}, false);
  require(weak.accept, "3 walls with weak error should be accepted as weak");
  require(weak.trust == origincar_wall::WallScanTrust::Weak, "3 walls with weak error should be weak trust");
  requireNear(weak.noise.x, 0.10, 1e-12, "weak trust x noise");
}

void testScanPolicyRejectsPoorOrJumpingMeasurements()
{
  const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};

  const auto too_few_walls = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, {1.01, 1.0, 0.0}, {2, 0.04, true}, false);
  require(!too_few_walls.accept, "fewer than 3 walls should be rejected");

  const auto too_noisy = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, {1.01, 1.0, 0.0}, {4, 0.16, true}, false);
  require(!too_noisy.accept, "mean error above 0.15 should be rejected");

  const auto stationary_jump = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, {1.06, 1.0, 0.0}, {4, 0.03, true}, true);
  require(!stationary_jump.accept, "stationary scan jump above 0.04m should be rejected");
}

void testScanPolicyDowngradesWhileStationary()
{
  const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
  const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0};

  const auto downgraded_high = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, scan_pose, {4, 0.04, true}, true);
  require(downgraded_high.accept, "stationary high trust scan should still be accepted");
  require(downgraded_high.trust == origincar_wall::WallScanTrust::Medium,
          "stationary high trust scan should downgrade to medium");

  const auto downgraded_medium = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, scan_pose, {3, 0.08, true}, true);
  require(downgraded_medium.accept, "stationary medium trust scan should still be accepted");
  require(downgraded_medium.trust == origincar_wall::WallScanTrust::Weak,
          "stationary medium trust scan should downgrade to weak");

  const auto downgraded_weak = origincar_wall::chooseWallScanUpdatePolicy(
      filter_pose, scan_pose, {3, 0.12, true}, true);
  require(!downgraded_weak.accept, "stationary weak trust scan should be rejected");
}
}  // namespace

int main()
{
  try
  {
    testPredictUsesWheelVelocityAndImuYawRate();
    testScanUpdateTrustsLowNoiseMoreThanHighNoise();
    testYawCorrectionUsesWrappedInnovation();
    testScanPolicyGradesByWallCountAndMeanError();
    testScanPolicyRejectsPoorOrJumpingMeasurements();
    testScanPolicyDowngradesWhileStationary();
  }
  catch (const std::exception &error)
  {
    std::cerr << "wall_kalman_filter_test failed: " << error.what() << std::endl;
    return 1;
  }
  return 0;
}
