#include <opencv2/opencv.hpp>

#include <uc/logger.h>
#include <uc/command_line.h>
#include <uc/timer.h>
#include <uc/math_common.h>
#include <uc/dimensions.h>
#include <uc/math_geometric.h>

#include "draw_utils.h"

using namespace dimensions::literals;

std::vector<std::array<math::Vector3d, 4>> planes(const std::vector<math::Vector2d>& vector,
                                                  const dimensions::Length& dist_to_ceiling,
                                                  const dimensions::Length& dist_to_floor) {
  std::vector<std::array<math::Vector3d, 4>> output_planes;
  for (size_t i = 0; i < vector.size(); ++i) {
    const auto& p0 = vector[i == 0 ? vector.size() - 1 : i - 1];
    const auto& p1 = vector[i];
    output_planes.emplace_back(std::array<math::Vector3d, 4>{{{p0.x, p0.y, dist_to_ceiling.m()},
                                                              {p1.x, p1.y, dist_to_ceiling.m()},
                                                              {p1.x, p1.y, dist_to_floor.m()},
                                                              {p0.x, p0.y, dist_to_floor.m()}}});
  }
  return output_planes;
}

struct Portal {
  Portal() = default;
  Portal(const math::Vector2d& top_left, const math::Vector2d& bottom_right) :
      top_left(top_left), bottom_right(bottom_right) {}
  math::Vector2d top_left;
  math::Vector2d bottom_right;

  math::Vector2d layoutTopLeft(const math::Vector2d& layout_size) const {
    return {top_left.x * layout_size.x, top_left.y * layout_size.y};
  }
  math::Vector2d layoutBottomRight(const math::Vector2d& layout_size) const {
    return {bottom_right.x * layout_size.x, bottom_right.y * layout_size.y};
  }

  std::array<math::Vector3d, 4> plane(const std::vector<std::array<math::Vector3d, 4>>& planes,
                                      const math::Vector2d& layout_size) {
    const auto layout_tl = layoutTopLeft(layout_size);
    const auto layout_br = layoutBottomRight(layout_size);
    auto tl_plane = layoutPointOnPlane(layout_tl, planes, layout_size);
    auto br_plane = layoutPointOnPlane(layout_br, planes, layout_size);
    math::Vector3d tr_plane = {br_plane.x, br_plane.y, tl_plane.z};
    math::Vector3d bl_plane = {tl_plane.x, tl_plane.y, br_plane.z};
    return {tl_plane, tr_plane, br_plane, bl_plane};
  }

  math::Vector4d rect(const std::array<math::Vector3d, 4>& wall, const math::Vector2d& layout_size) {
    const auto layout_tl = layoutTopLeft(layout_size);
    const auto layout_br = layoutBottomRight(layout_size);
    const auto plane = planeFromTriangle(wall[0], wall[1], wall[2]);
    const auto plane_size = planeSizeFromTriangle(wall[0], wall[1], wall[2]);
    const auto tl_plane = math::Vector2d(pix2PointOnPlane(plane, plane_size, layout_tl, layout_size));
    const auto br_plane = math::Vector2d(pix2PointOnPlane(plane, plane_size, layout_br, layout_size));
    return {tl_plane.x, tl_plane.y, fabs(br_plane.x - tl_plane.x), fabs(br_plane.y - tl_plane.y)};
  }
};

math::Vector2d getRectOverlapOffset(const math::Vector4d& rect1, const math::Vector4d& rect2) {
  // fl, fw, fr, ft, fh, fb: First.Left, Width, Right, Top, Height, Bottom
  // s* - similar parameters for the second rectangle
  const auto fl = rect1.x;
  const auto fw = rect1.z;
  const auto fr = fl + fw;
  const auto ft = rect1.y;
  const auto fb = ft + rect1.w;
  const auto fh = rect1.w;

  const auto sl = rect2.x;
  const auto sw = rect2.z;
  const auto sr = sl + sw;
  const auto st = rect2.y;
  const auto sb = st + rect2.w;
  const auto sh = rect2.w;

  const auto dcy = (sb + st) - (fb + ft); // doubled center y-difference
  const auto dcx = (sl + sr) - (fl + fr);
  if (std::abs(dcx) >= fw + sw || std::abs(dcy) >= fh + sh) { // no intersection
    return {0, 0};
  }

  auto dx = fw + sw - std::abs(dcx); // doubled  needed x-shift absolute value
  auto dy = fh + sh - std::abs(dcy);

  if (dx > dy) {
    if (dcy > 0) {
      dy = -dy / 2;
    } else {
      dy = dy / 2; // needed y-shift accounting for direction
    }
    dx = 0;
  } else {
    if (dcy > 0) {
      dx = -dx / 2;
    } else {
      dx = dx / 2;
    }
    dy = 0;
  }

  // Result: dx, dy pair to shift the second rectangle
  return {dx, dy};
}

std::array<math::Vector2d, 4> rectToArray(const math::Vector4d& rect) {
  return {{{rect.x, rect.y}, {rect.x + rect.z, rect.y}, {rect.x + rect.z, rect.y + rect.w}, {rect.x, rect.y + rect.w}}};
}

int main(int argc, char** argv) {
  CMDLine cmd;

  if (cmd.parse(argc, argv) == false) {
    cmd.help();
    return 0;
  }

  math::Vector2d layout_size{2000, 1000};

  dimensions::Length ceiling = 1.0598699949175356_m;
  dimensions::Length floor = -1.2196247597843446_m;

  std::vector<math::Vector2d> contour = {{-1.2792625427246094, -1.5333192348480225},
                                         {1.8199341297149658, -1.491775393486023},
                                         {1.785354495048523, 1.7804408073425293},
                                         {-1.3336228132247925, 1.73493230342865}};
  const auto contour_planes = planes(contour, ceiling, floor);

  Portal portal_one({0.10979843974529249, 0.39079923902773445}, {0.15709831606137092, 0.7091479793600469});
  Portal portal_two({0.15201005025125622, 0.35050251256281406}, {0.20351758793969862, 0.7316167535108522});
  Portal portal_three({0.12836970474967904, 0.3427471116816431}, {0.23876765083440324, 0.35686777920410784});

  auto output = drawPanRoom(contour_planes, layout_size);

  cv::imshow("Output", output);
  cv::waitKey(0);

  const auto portal_one_tl = portal_one.layoutTopLeft(layout_size);
  const auto portal_one_br = portal_one.layoutBottomRight(layout_size);
  const auto portal_two_tl = portal_two.layoutTopLeft(layout_size);
  const auto portal_two_br = portal_two.layoutBottomRight(layout_size);
  const auto portal_three_tl = portal_two.layoutTopLeft(layout_size);
  const auto portal_three_br = portal_two.layoutBottomRight(layout_size);

  std::array<math::Vector3d, 4> wall_plane;
  bool portal_plane_found =
      closestPlaneAtLayoutLine(wall_plane, portal_one_tl, portal_one_br, contour_planes, layout_size);
  if (portal_plane_found) {
    drawPanPlane(output, wall_plane, layout_size, {255, 0, 0});
  }
  cv::imshow("Output", output);
  cv::waitKey(0);

  cv::Scalar portal_one_color{0, 255, 0};
  cv::Scalar portal_two_color{127, 127, 0};
  cv::Scalar portal_three_color{127, 127, 200};

  const auto portal_one_plane = portal_one.plane(contour_planes, layout_size);
  const auto portal_two_plane = portal_two.plane(contour_planes, layout_size);
  const auto portal_three_plane = portal_three.plane(contour_planes, layout_size);
  drawPanPlane(output, portal_one_plane, layout_size, portal_one_color);
  drawPanPlane(output, portal_two_plane, layout_size, portal_two_color);
  drawPanPlane(output, portal_three_plane, layout_size, portal_three_color);
  cv::imshow("Output", output);
  cv::waitKey(0);

  cv::Mat plane_output(1000, 1000, CV_8UC3);
  plane_output = cv::Scalar(255, 255, 255);

  const auto portal_one_rect = portal_one.rect(wall_plane, layout_size);
  const auto portal_two_rect = portal_two.rect(wall_plane, layout_size);
  const auto portal_three_rect = portal_three.rect(wall_plane, layout_size);

  drawRect(plane_output, portal_one_rect, portal_one_color, 300);
  drawRect(plane_output, portal_two_rect, portal_two_color, 300);
  drawRect(plane_output, portal_three_rect, portal_three_color, 300);

  cv::imshow("Plane Output", plane_output);
  cv::waitKey(0);

  const auto portal_one_array = rectToArray(portal_one_rect);
  const auto plane = planeFromTriangle(wall_plane[0], wall_plane[1], wall_plane[2]);

  math::Vector4d portal_two_new_rect;
  bool portals_two_overlap = isRectangleIntersection(portal_one_rect, portal_two_rect);
  if (portals_two_overlap) {
    const auto two_overlap_offset = getRectOverlapOffset(portal_one_rect, portal_two_rect);
    const auto portal_two_array = rectToArray(portal_two_rect);
    bool change_tl = true;
    portal_two_new_rect.z = portal_two_rect.z - two_overlap_offset.x;
    portal_two_new_rect.w = portal_two_rect.w - two_overlap_offset.y;
    portal_two_new_rect.x = change_tl ? portal_two_rect.x + two_overlap_offset.x : portal_two_rect.x;
    portal_two_new_rect.y = change_tl ? portal_two_rect.y + two_overlap_offset.y : portal_two_rect.y;

    drawRect(plane_output, portal_two_new_rect, {0, 0, 255}, 300, 1);

    cv::imshow("Plane Output", plane_output);
    cv::waitKey(0);

    const auto portal_two_tl_new =
        pointOnPlane2Pix(plane, {portal_two_new_rect.x, portal_two_new_rect.y, 0}, layout_size);
    const auto portal_two_br_new = pointOnPlane2Pix(
        plane, {portal_two_new_rect.x + portal_two_new_rect.z, portal_two_new_rect.y + portal_two_new_rect.w, 0},
        layout_size);
    Portal portal_two_new(portal_two_tl_new / layout_size, portal_two_br_new / layout_size);
    const auto portal_two_new_plane = portal_two_new.plane(contour_planes, layout_size);
    drawPanPlane(output, portal_two_new_plane, layout_size, {0, 0, 255}, 1);
    cv::imshow("Output", output);
    cv::waitKey(0);
  }

  math::Vector4d portal_three_new_rect;
  bool portals_three_overlap = isRectangleIntersection(portal_one_rect, portal_three_rect);
  if (portals_three_overlap) {
    const auto three_overlap_offset = getRectOverlapOffset(portal_one_rect, portal_three_rect);
    const auto portal_three_array = rectToArray(portal_three_rect);

    bool change_tl = false;
    const auto tl_dist_to_rect = pointToRectangleDistance(portal_three_array[0], portal_one_rect);
    const auto tr_dist_to_rect = pointToRectangleDistance(portal_three_array[1], portal_one_rect);
    const auto br_dist_to_rect = pointToRectangleDistance(portal_three_array[2], portal_one_rect);
    const auto bl_dist_to_rect = pointToRectangleDistance(portal_three_array[3], portal_one_rect);

    if (tl_dist_to_rect < br_dist_to_rect || bl_dist_to_rect > tr_dist_to_rect) {
      change_tl = true;
    }

    portal_three_new_rect.z = portal_three_rect.z - three_overlap_offset.x;
    portal_three_new_rect.w = portal_three_rect.w - three_overlap_offset.y;
    portal_three_new_rect.x = change_tl ? portal_three_rect.x + three_overlap_offset.x : portal_three_rect.x;
    portal_three_new_rect.y = change_tl ? portal_three_rect.y + three_overlap_offset.y : portal_three_rect.y;

    drawRect(plane_output, portal_three_new_rect, {0, 0, 255}, 300, 1);

    cv::imshow("Plane Output", plane_output);
    cv::waitKey(0);

    const auto portal_three_tl_new =
        pointOnPlane2Pix(plane, {portal_three_new_rect.x, portal_three_new_rect.y, 0}, layout_size);
    const auto portal_three_br_new = pointOnPlane2Pix(
        plane,
        {portal_three_new_rect.x + portal_three_new_rect.z, portal_three_new_rect.y + portal_three_new_rect.w, 0},
        layout_size);
    Portal portal_three_new(portal_three_tl_new / layout_size, portal_three_br_new / layout_size);
    const auto portal_three_new_plane = portal_three_new.plane(contour_planes, layout_size);
    drawPanPlane(output, portal_three_new_plane, layout_size, {0, 0, 255}, 1);
    cv::imshow("Output", output);
    cv::waitKey(0);
  }

  return 0;
}