#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");
}
}  // namespace

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