#ifndef ORIGINCAR_BASE_WALL_FIT_CORE_HPP_
#define ORIGINCAR_BASE_WALL_FIT_CORE_HPP_

#include <map>
#include <string>
#include <vector>

namespace origincar_wall
{

struct WallPoint
{
  double x;
  double y;
};

struct Pose2D
{
  double x;
  double y;
  double theta;
};

struct WallBounds
{
  double x_min;
  double x_max;
  double y_min;
  double y_max;
};

struct WallLine
{
  std::string name;
  double a;
  double b;
  double c;
  std::string color;
};

struct ReferencePoint
{
  std::string label;
  WallPoint image;
  WallPoint map;
};

struct CoordinateFrame
{
  std::string origin;
  std::string origin_note;
  double width_m;
  double height_m;
  int image_width_px;
  int image_height_px;
  int pixels_per_meter;
  WallPoint p_image_px;
  std::string odom_zero_reference;
  WallPoint odom_zero_map;
};

struct WallFitConfig
{
  CoordinateFrame coordinate_frame;
  std::vector<WallLine> walls;
  std::map<std::string, ReferencePoint> reference_points;
  std::string generated_at;
};

struct MatchedWall
{
  std::string name;
  int point_count;
  double observed;
  double target;
  double correction;
  double mean_error;
};

struct LocalizationQuality
{
  int wall_count;
  double mean_error;
  bool has_mean_error;
};

struct LocalizationResult
{
  bool ok;
  Pose2D pose;
  std::vector<WallPoint> scan_map;
  std::vector<MatchedWall> matched_walls;
  LocalizationQuality quality;
  std::string message;
};

struct WallFitCapture
{
  std::string key;
  WallPoint map;
  Pose2D odom_pose;
  std::vector<WallPoint> scan_points;
};

WallFitConfig defaultWallFitConfig();
WallBounds wallBounds(const WallFitConfig &config);
double normalizeAngle(double angle);
Pose2D predictPoseFromOdomDelta(const Pose2D &previous_wall_pose,
                                const Pose2D &previous_odom_pose,
                                const Pose2D &current_odom_pose);
std::vector<WallPoint> transformScanPointsToMap(const std::vector<WallPoint> &scan_points,
                                                const Pose2D &pose);
std::vector<WallPoint> transformScanPointsToPoseFrame(const std::vector<WallPoint> &scan_points,
                                                      const Pose2D &laser_pose);
std::vector<WallPoint> mapPointsToScan(const std::vector<WallPoint> &map_points,
                                       const Pose2D &pose);
LocalizationResult localizeFromScan(const std::vector<WallPoint> &scan_points,
                                    const WallFitConfig &config,
                                    const Pose2D &predicted_pose);
WallFitConfig fitWallConfigFromCaptures(const std::vector<WallFitCapture> &captures,
                                        const WallFitConfig &base_config);
WallFitConfig loadWallFitConfig(const std::string &path);
void saveWallFitConfig(const std::string &path, const WallFitConfig &config);
std::string wallFitConfigToJson(const WallFitConfig &config);

}  // namespace origincar_wall

#endif  // ORIGINCAR_BASE_WALL_FIT_CORE_HPP_
