#include "origincar_base/wall_fit_core.hpp"

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

namespace
{
constexpr double kPi = 3.14159265358979323846;

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

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));
  }
}

std::vector<origincar_wall::WallPoint> makeWallPoints(const origincar_wall::WallBounds &bounds)
{
  std::vector<origincar_wall::WallPoint> points;
  for (double y : {0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2})
  {
    points.push_back({bounds.x_min, y});
    points.push_back({bounds.x_max, y});
  }
  for (double x : {0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2})
  {
    points.push_back({x, bounds.y_min});
    points.push_back({x, bounds.y_max});
  }
  return points;
}

void testReferencePointsUseLowerLeftMapFrame()
{
  const auto config = origincar_wall::defaultWallFitConfig();
  requireNear(config.reference_points.at("P").map.x, 0.56, 1e-9, "P x");
  requireNear(config.reference_points.at("P").map.y, 0.18, 1e-9, "P y");
  requireNear(config.reference_points.at("task_release").map.x, 4.5, 1e-9, "task x");
  requireNear(config.reference_points.at("task_release").map.y, 1.81, 1e-9, "task y");
  requireNear(config.reference_points.at("passage").map.x, 2.5, 1e-9, "passage x");
  requireNear(config.reference_points.at("passage").map.y, 2.6, 1e-9, "passage y");
  requireNear(config.reference_points.at("clinic").map.x, 2.8, 1e-9, "clinic x");
  requireNear(config.reference_points.at("clinic").map.y, 3.77, 1e-9, "clinic y");
}

void testPredictPoseFromOdomDeltaAdvancesWallPose()
{
  const origincar_wall::Pose2D previous_wall_pose{1.0, 2.0, 170.0 * kPi / 180.0};
  const origincar_wall::Pose2D previous_odom_pose{0.56, 0.18, 175.0 * kPi / 180.0};
  const origincar_wall::Pose2D current_odom_pose{0.86, 0.08, -175.0 * kPi / 180.0};

  const auto predicted =
      origincar_wall::predictPoseFromOdomDelta(previous_wall_pose, previous_odom_pose, current_odom_pose);

  requireNear(predicted.x, 1.3, 1e-9, "predicted x");
  requireNear(predicted.y, 1.9, 1e-9, "predicted y");
  requireNear(predicted.theta, -kPi, 1e-9, "predicted theta");
}

void testWallBoundsCanExtendOutsideMapImage()
{
  auto config = origincar_wall::defaultWallFitConfig();
  config.walls = {
      {"left", 1.0, 0.0, 0.08, "#2ca02c"},
      {"right", -1.0, 0.0, 5.12, "#ff7f0e"},
      {"bottom", 0.0, 1.0, 0.05, "#1f77b4"},
      {"top", 0.0, -1.0, 5.2, "#d62728"},
  };

  const auto bounds = origincar_wall::wallBounds(config);

  requireNear(bounds.x_min, -0.08, 1e-9, "x_min");
  requireNear(bounds.x_max, 5.12, 1e-9, "x_max");
  requireNear(bounds.y_min, -0.05, 1e-9, "y_min");
  requireNear(bounds.y_max, 5.2, 1e-9, "y_max");
}

void testLocalizeFromScanCorrectsXYAndTheta()
{
  const auto config = origincar_wall::defaultWallFitConfig();
  const auto points = makeWallPoints({0.0, 5.0, 0.0, 5.0});
  const origincar_wall::Pose2D true_pose{1.6, 1.5, 8.0 * kPi / 180.0};
  const origincar_wall::Pose2D predicted_pose{1.55, 1.45, 0.0};
  const auto scan = origincar_wall::mapPointsToScan(points, true_pose);

  const auto result = origincar_wall::localizeFromScan(scan, config, predicted_pose);

  require(result.ok, "localization should be ok");
  requireNear(result.pose.x, true_pose.x, 0.08, "localized x");
  requireNear(result.pose.y, true_pose.y, 0.08, "localized y");
  requireNear(result.pose.theta, true_pose.theta, 1.1 * kPi / 180.0, "localized theta");
}

void testFitWallConfigFromCapturesUsesOuterPointsBeyondMapEdges()
{
  const auto base_config = origincar_wall::defaultWallFitConfig();
  const origincar_wall::WallBounds true_bounds{-0.08, 5.12, -0.05, 5.2};
  const auto points = makeWallPoints(true_bounds);
  std::vector<origincar_wall::WallFitCapture> captures;
  for (const auto &entry : base_config.reference_points)
  {
    origincar_wall::WallFitCapture capture;
    capture.key = entry.first;
    capture.map = entry.second.map;
    capture.odom_pose = {entry.second.map.x, entry.second.map.y, 0.0};
    capture.scan_points = origincar_wall::mapPointsToScan(points, capture.odom_pose);
    captures.push_back(capture);
  }

  const auto config = origincar_wall::fitWallConfigFromCaptures(captures, base_config);
  const auto bounds = origincar_wall::wallBounds(config);

  requireNear(bounds.x_min, true_bounds.x_min, 0.04, "fit x_min");
  requireNear(bounds.x_max, true_bounds.x_max, 0.04, "fit x_max");
  requireNear(bounds.y_min, true_bounds.y_min, 0.04, "fit y_min");
  requireNear(bounds.y_max, true_bounds.y_max, 0.04, "fit y_max");
}

void testSaveWallFitAtomicRoundTripsJson()
{
  const std::string path = "/tmp/origincar_wall_fit_core_test.json";
  const auto config = origincar_wall::defaultWallFitConfig();
  origincar_wall::saveWallFitConfig(path, config);
  const auto loaded = origincar_wall::loadWallFitConfig(path);
  std::remove(path.c_str());

  require(loaded.coordinate_frame.origin == "map_lower_left", "loaded origin");
  requireNear(loaded.reference_points.at("P").map.x, 0.56, 1e-9, "loaded P x");
  requireNear(loaded.reference_points.at("P").map.y, 0.18, 1e-9, "loaded P y");
}
}  // namespace

int main()
{
  try
  {
    testReferencePointsUseLowerLeftMapFrame();
    testPredictPoseFromOdomDeltaAdvancesWallPose();
    testWallBoundsCanExtendOutsideMapImage();
    testLocalizeFromScanCorrectsXYAndTheta();
    testFitWallConfigFromCapturesUsesOuterPointsBeyondMapEdges();
    testSaveWallFitAtomicRoundTripsJson();
  }
  catch (const std::exception &error)
  {
    std::cerr << "wall_fit_core_test failed: " << error.what() << std::endl;
    return 1;
  }
  return 0;
}
