#include <fstream>
#include <iostream>
#include <limits>
#include <list>
#include <string>
#include <vector>
#include <Eigen/Dense>

#include "asset.h"
#include "popup_3d.h"
#include "structs.h"

using namespace Eigen;
using namespace std;

namespace {

double ComputeRatio(const Vector2i& xs, const Vector2i& ys, const double x, const double y,
                    const Options::HeightAdjustment height_adjustment) {
  const double total = xs[1] - xs[0] + ys[1] - ys[0];
  switch (height_adjustment) {
  case Options::kNone: {
    return 1.0;
  }
  case Options::kTopLeft: {
    return (fabs(x - xs[0]) + fabs(y - ys[0])) / total;
  }
  case Options::kTopRight: {
    return (fabs(x - xs[1]) + fabs(y - ys[0])) / total;
  }
  case Options::kBottomLeft: {
    return (fabs(x - xs[0]) + fabs(y - ys[1])) / total;
  }
  case Options::kBottomRight: {
    return (fabs(x - xs[1]) + fabs(y - ys[1])) / total;
  }
  }
}

bool FilterHorizontalSurfaces(const Vector2i& xs, const Vector2i& ys, const int x, const int y,
                              const double wall_height,
                              const Options::HeightAdjustment height_adjustment,
                              const double lower_ratio,
                              const double upper_ratio,
                              vector<HorizontalSurface>& horizontal_surfaces) {
  const double ratio = ComputeRatio(xs, ys, x, y, height_adjustment);
  const double height_threshold = wall_height * (lower_ratio + (upper_ratio - lower_ratio) * ratio);

  const double kEpsilon = 0.00001;
  vector<HorizontalSurface> tmp;
  tmp.swap(horizontal_surfaces);
  for (const auto& horizontal_surface : tmp) {
    if (horizontal_surface.height <= height_threshold + kEpsilon)
      horizontal_surfaces.push_back(horizontal_surface);
  }
  return true;
}

bool FilterVerticalSurfaces(const Vector2i& xs, const Vector2i& ys,
                            const double wall_height,
                            const Options::HeightAdjustment height_adjustment,
                            const double lower_ratio,
                            const double upper_ratio,
                            vector<VerticalSurface>& vertical_surfaces) {
  vector<VerticalSurface> tmp;
  tmp.swap(vertical_surfaces);

  const double kEpsilon = 0.00001;
  for (auto& vertical_surface : tmp) {
    const double x = (vertical_surface.point0[0] + vertical_surface.point1[0]) / 2.0;
    const double y = (vertical_surface.point0[1] + vertical_surface.point1[1]) / 2.0;
    const double ratio = ComputeRatio(xs, ys, x, y, height_adjustment);
    int height_threshold =
      static_cast<int>(round(wall_height * (lower_ratio + (upper_ratio - lower_ratio) * ratio)));

    const int discretization = 20;
    height_threshold = height_threshold / discretization * discretization;

    if (vertical_surface.lower_height > height_threshold)
      continue;

    vertical_surface.upper_height = min(vertical_surface.upper_height, static_cast<double>(height_threshold));
    vertical_surfaces.push_back(vertical_surface);
  }

  return true;
}

bool Rotate(const Annotation& annotation, const double rotation_degree, Triangle& triangle) {
  Vector2d pivot(annotation.xs[0], annotation.ys[0]);
  const double rotation_radian = rotation_degree * M_PI / 180.0;
  Matrix2d rotation;
  rotation <<
      cos(rotation_radian), -sin(rotation_radian),
      sin(rotation_radian), cos(rotation_radian);

  for (int i = 0; i < 3; ++i) {
    Vector2d point(triangle.vertices[i][0], triangle.vertices[i][1]);
    const Vector2d new_point = rotation * (point - pivot) + pivot;

    triangle.vertices[i][0] = new_point[0];
    triangle.vertices[i][1] = new_point[1];
  }

  return true;
}

bool AddHorizontalFace(const int x, const int y, const int z,
                         const bool flip,
                         const double floor_uv_scale,
                         Triangle& triangle0, Triangle& triangle1) {
  triangle0.vertices[0] = Vector3d(x, y, z);
  triangle0.vertices[1] = Vector3d(x + 1, y, z);
  triangle0.vertices[2] = Vector3d(x + 1, y + 1, z);

  triangle0.uvs[0][0] = x * floor_uv_scale;
  triangle0.uvs[0][1] = y * floor_uv_scale;

  triangle0.uvs[1][0] = (x + 1) * floor_uv_scale;
  triangle0.uvs[1][1] = y * floor_uv_scale;

  triangle0.uvs[2][0] = (x + 1) * floor_uv_scale;
  triangle0.uvs[2][1] = (y + 1) * floor_uv_scale;

  triangle1.vertices[0] = Vector3d(x, y, z);
  triangle1.vertices[1] = Vector3d(x + 1, y + 1, z);
  triangle1.vertices[2] = Vector3d(x, y + 1, z);

  triangle1.uvs[0][0] = x * floor_uv_scale;
  triangle1.uvs[0][1] = y * floor_uv_scale;

  triangle1.uvs[1][0] = (x + 1) * floor_uv_scale;
  triangle1.uvs[1][1] = (y + 1) * floor_uv_scale;

  triangle1.uvs[2][0] = x * floor_uv_scale;
  triangle1.uvs[2][1] = (y + 1) * floor_uv_scale;

  if (flip) {
    swap(triangle0.vertices[0], triangle0.vertices[2]);
    swap(triangle1.vertices[0], triangle1.vertices[2]);

    swap(triangle0.uvs[0], triangle0.uvs[2]);
    swap(triangle1.uvs[0], triangle1.uvs[2]);
  }

  return true;
}

bool AddHorizontalFace(const int x, const int y, const int z,
                       const bool flip,
                       const int width,
                       const int height,
                       Triangle& triangle0, Triangle& triangle1) {
  triangle0.vertices[0] = Vector3d(x, y, z);
  triangle0.vertices[1] = Vector3d(x + 1, y, z);
  triangle0.vertices[2] = Vector3d(x + 1, y + 1, z);

  triangle0.uvs[0][0] = x / static_cast<double>(width);
  triangle0.uvs[0][1] = y / static_cast<double>(height);

  triangle0.uvs[1][0] = (x + 1) / static_cast<double>(width);
  triangle0.uvs[1][1] = y / static_cast<double>(height);;

  triangle0.uvs[2][0] = (x + 1) / static_cast<double>(width);
  triangle0.uvs[2][1] = (y + 1) / static_cast<double>(height);

  triangle1.vertices[0] = Vector3d(x, y, z);
  triangle1.vertices[1] = Vector3d(x + 1, y + 1, z);
  triangle1.vertices[2] = Vector3d(x, y + 1, z);

  triangle1.uvs[0][0] = x / static_cast<double>(width);
  triangle1.uvs[0][1] = y / static_cast<double>(height);

  triangle1.uvs[1][0] = (x + 1) / static_cast<double>(width);
  triangle1.uvs[1][1] = (y + 1) / static_cast<double>(height);

  triangle1.uvs[2][0] = x / static_cast<double>(width);
  triangle1.uvs[2][1] = (y + 1) / static_cast<double>(height);

  if (flip) {
    swap(triangle0.vertices[0], triangle0.vertices[2]);
    swap(triangle1.vertices[0], triangle1.vertices[2]);

    swap(triangle0.uvs[0], triangle0.uvs[2]);
    swap(triangle1.uvs[0], triangle1.uvs[2]);
  }

  return true;
}

bool AddVerticalFace(const Vector2i& point0, const Vector2i& point1,
                     const int floor_up_z, const int floor_bottom_z,
                     const int x_or_y, const double wall_uv_scale,
                     Mesh& mesh) {
  Triangle triangle0;
  triangle0.vertices[0] = Vector3d(point0[0], point0[1], floor_up_z);
  triangle0.vertices[1] = Vector3d(point1[0], point1[1], floor_up_z);
  triangle0.vertices[2] = Vector3d(point1[0], point1[1], floor_bottom_z);

  if (x_or_y == 0) {
    triangle0.uvs[0][0] = point0[0] * wall_uv_scale;
    triangle0.uvs[0][1] = floor_up_z * wall_uv_scale;

    triangle0.uvs[1][0] = point1[0] * wall_uv_scale;
    triangle0.uvs[1][1] = floor_up_z * wall_uv_scale;

    triangle0.uvs[2][0] = point1[0] * wall_uv_scale;
    triangle0.uvs[2][1] = floor_bottom_z * wall_uv_scale;
  } else {
    triangle0.uvs[0][0] = point0[1] * wall_uv_scale;
    triangle0.uvs[0][1] = floor_up_z * wall_uv_scale;

    triangle0.uvs[1][0] = point1[1] * wall_uv_scale;
    triangle0.uvs[1][1] = floor_up_z * wall_uv_scale;

    triangle0.uvs[2][0] = point1[1] * wall_uv_scale;
    triangle0.uvs[2][1] = floor_bottom_z * wall_uv_scale;
  }

  Triangle triangle1;
  triangle1.vertices[0] = Vector3d(point1[0], point1[1], floor_bottom_z);
  triangle1.vertices[1] = Vector3d(point0[0], point0[1], floor_bottom_z);
  triangle1.vertices[2] = Vector3d(point0[0], point0[1], floor_up_z);

  if (x_or_y == 0) {
    triangle1.uvs[0][0] = point1[0] * wall_uv_scale;
    triangle1.uvs[0][1] = floor_bottom_z * wall_uv_scale;

    triangle1.uvs[1][0] = point0[0] * wall_uv_scale;
    triangle1.uvs[1][1] = floor_bottom_z * wall_uv_scale;

    triangle1.uvs[2][0] = point0[0] * wall_uv_scale;
    triangle1.uvs[2][1] = floor_up_z * wall_uv_scale;
  } else {
    triangle1.uvs[0][0] = point1[1] * wall_uv_scale;
    triangle1.uvs[0][1] = floor_bottom_z * wall_uv_scale;

    triangle1.uvs[1][0] = point0[1] * wall_uv_scale;
    triangle1.uvs[1][1] = floor_bottom_z * wall_uv_scale;

    triangle1.uvs[2][0] = point0[1] * wall_uv_scale;
    triangle1.uvs[2][1] = floor_up_z * wall_uv_scale;
  }

  mesh.triangles.push_back(triangle0);
  mesh.triangles.push_back(triangle1);

  return true;
}

bool AddHorizontalFaceDoorWindow(const int x, const int y, const int z,
                                 const bool flip,
                                 const double floor_uv_scale,
                                 const Annotation& annotation,
                                 const double rotation_degree,
                                 Triangle& triangle0, Triangle& triangle1) {
  triangle0.vertices[0] = Vector3d(x, y, z);
  triangle0.vertices[1] = Vector3d(x + 1, y, z);
  triangle0.vertices[2] = Vector3d(x + 1, y + 1, z);

  triangle0.uvs[0][0] = x * floor_uv_scale;
  triangle0.uvs[0][1] = y * floor_uv_scale;

  triangle0.uvs[1][0] = (x + 1) * floor_uv_scale;
  triangle0.uvs[1][1] = y * floor_uv_scale;

  triangle0.uvs[2][0] = (x + 1) * floor_uv_scale;
  triangle0.uvs[2][1] = (y + 1) * floor_uv_scale;

  triangle1.vertices[0] = Vector3d(x, y, z);
  triangle1.vertices[1] = Vector3d(x + 1, y + 1, z);
  triangle1.vertices[2] = Vector3d(x, y + 1, z);

  triangle1.uvs[0][0] = x * floor_uv_scale;
  triangle1.uvs[0][1] = y * floor_uv_scale;

  triangle1.uvs[1][0] = (x + 1) * floor_uv_scale;
  triangle1.uvs[1][1] = (y + 1) * floor_uv_scale;

  triangle1.uvs[2][0] = x * floor_uv_scale;
  triangle1.uvs[2][1] = (y + 1) * floor_uv_scale;

  if (flip) {
    swap(triangle0.vertices[0], triangle0.vertices[2]);
    swap(triangle1.vertices[0], triangle1.vertices[2]);

    swap(triangle0.uvs[0], triangle0.uvs[2]);
    swap(triangle1.uvs[0], triangle1.uvs[2]);
  }

  Rotate(annotation, rotation_degree, triangle0);
  Rotate(annotation, rotation_degree, triangle1);

  return true;
}

bool AddVerticalFaceDoorWindow(const Vector2i& point0, const Vector2i& point1,
                               const int floor_up_z, const int floor_bottom_z,
                               const int x_or_y, const double wall_uv_scale,
                               const Annotation& annotation,
                               const double rotation_degree,
                               Mesh& mesh) {
  const int min_value = (x_or_y == 0) ? annotation.xs[0] : annotation.ys[0];
  const int max_value = (x_or_y == 0) ? annotation.xs[1] : annotation.ys[1];
  const double range = max(1, max_value - min_value);

  Triangle triangle0;
  triangle0.vertices[0] = Vector3d(point0[0], point0[1], floor_up_z);
  triangle0.vertices[1] = Vector3d(point1[0], point1[1], floor_up_z);
  triangle0.vertices[2] = Vector3d(point1[0], point1[1], floor_bottom_z);

  if (x_or_y == 0) {
    triangle0.uvs[0][0] = (point0[0] - min_value) / range;
    triangle0.uvs[0][1] = 1.0;

    triangle0.uvs[1][0] = (point1[0] - min_value) / range;
    triangle0.uvs[1][1] = 1.0;

    triangle0.uvs[2][0] = (point1[0] - min_value) / range;
    triangle0.uvs[2][1] = 0.0;
  } else {
    triangle0.uvs[0][0] = (point0[1] - min_value) / range;
    triangle0.uvs[0][1] = 1.0;

    triangle0.uvs[1][0] = (point1[1] - min_value) / range;
    triangle0.uvs[1][1] = 1.0;

    triangle0.uvs[2][0] = (point1[1] - min_value) / range;
    triangle0.uvs[2][1] = 0.0;
  }

  Triangle triangle1;
  triangle1.vertices[0] = Vector3d(point1[0], point1[1], floor_bottom_z);
  triangle1.vertices[1] = Vector3d(point0[0], point0[1], floor_bottom_z);
  triangle1.vertices[2] = Vector3d(point0[0], point0[1], floor_up_z);

  if (x_or_y == 0) {
    triangle1.uvs[0][0] = (point1[0] - min_value) / range;
    triangle1.uvs[0][1] = 0.0;

    triangle1.uvs[1][0] = (point0[0] - min_value) / range;
    triangle1.uvs[1][1] = 0.0;

    triangle1.uvs[2][0] = (point0[0] - min_value) / range;
    triangle1.uvs[2][1] = 1.0;
  } else {
    triangle1.uvs[0][0] = (point1[1] - min_value) / range;
    triangle1.uvs[0][1] = 0.0;

    triangle1.uvs[1][0] = (point0[1] - min_value) / range;
    triangle1.uvs[1][1] = 0.0;

    triangle1.uvs[2][0] = (point0[1] - min_value) / range;
    triangle1.uvs[2][1] = 1.0;
  }

  Rotate(annotation, rotation_degree, triangle0);
  Rotate(annotation, rotation_degree, triangle1);

  mesh.triangles.push_back(triangle0);
  mesh.triangles.push_back(triangle1);

  return true;
}

}  // namespace

bool FloorToFloorMesh(const int width,
                      const int height,
                      const std::vector<RasterArchitectureFloorplan::FloorType>& floor,
                      const int wall_radius,
                      const double wall_uv_scale,
                      const double floor_uv_scale,
                      const bool use_floorplan_image,
                      const int image_width,
                      const int image_height,
                      ArchitectureMesh& architecture_mesh) {
  Mesh& floor_bottom_mesh = architecture_mesh.floor_bottom;

  const int floor_top_z = 0;
  const int floor_bottom_z = - 2 * wall_radius + 1;
  const bool kFlip   = true;
  const bool kNoFlip = false;
  // Bottom.
  for (int y = 0; y < height; ++y) {
    for (int x = 0; x < width; ++x) {
      if (floor[y * width + x] != RasterArchitectureFloorplan::kNotFloor) {
        Triangle triangle0, triangle1;
        if (!AddHorizontalFace(x, y, floor_bottom_z, kFlip, floor_uv_scale, triangle0, triangle1))
          return false;

        floor_bottom_mesh.triangles.push_back(triangle0);
        floor_bottom_mesh.triangles.push_back(triangle1);
      }
    }
  }

  // Vertical.
  const int kX = 0;
  const int kY = 1;
  for (int y = 0; y < height - 1; ++y) {
    for (int x = 0; x < width - 1; ++x) {
      const int index = y * width + x;
      const int right_index = index + 1;
      if (floor[index] != RasterArchitectureFloorplan::kNotFloor &&
          floor[right_index] == RasterArchitectureFloorplan::kNotFloor) {
        if (!AddVerticalFace(Vector2i(x + 1, y + 1), Vector2i(x + 1, y), floor_top_z, floor_bottom_z,
                             kY, wall_uv_scale, floor_bottom_mesh))
          return false;
      } else if (floor[index] == RasterArchitectureFloorplan::kNotFloor &&
                 floor[right_index] != RasterArchitectureFloorplan::kNotFloor) {
        if (!AddVerticalFace(Vector2i(x + 1, y), Vector2i(x + 1, y + 1), floor_top_z, floor_bottom_z,
                             kY, wall_uv_scale, floor_bottom_mesh))
          return false;
      }

      const int bottom_index = index + width;
      if (floor[index] != RasterArchitectureFloorplan::kNotFloor &&
          floor[bottom_index] == RasterArchitectureFloorplan::kNotFloor) {
        if (!AddVerticalFace(Vector2i(x, y + 1), Vector2i(x + 1, y + 1), floor_top_z, floor_bottom_z,
                             kX, wall_uv_scale, floor_bottom_mesh))
          return false;
      } else if (floor[index] == RasterArchitectureFloorplan::kNotFloor &&
                 floor[bottom_index] != RasterArchitectureFloorplan::kNotFloor) {
        if (!AddVerticalFace(Vector2i(x + 1, y + 1), Vector2i(x, y + 1), floor_top_z, floor_bottom_z,
                             kX, wall_uv_scale, floor_bottom_mesh))
          return false;
      }
    }
  }

  // Top.
  for (int y = 0; y < height; ++y) {
    for (int x = 0; x < width; ++x) {
      if (floor[y * width + x] == RasterArchitectureFloorplan::kNotFloor)
        continue;

      Triangle triangle0, triangle1;
      if (use_floorplan_image) {
        if (!AddHorizontalFace(x, y, floor_top_z, kNoFlip, image_width, image_height, triangle0, triangle1))
          return false;
      } else {
        if (!AddHorizontalFace(x, y, floor_top_z, kNoFlip, floor_uv_scale, triangle0, triangle1))
          return false;
      }
      auto& mesh = GetFloorMesh(floor[y * width + x], architecture_mesh);
      mesh.triangles.push_back(triangle0);
      mesh.triangles.push_back(triangle1);
    }
  }

  return true;
}

bool FloorToWallDoorWindowMesh(const ArchitectureFloorplan& architecture_floorplan,
                               const RasterArchitectureFloorplan& raster_architecture_floorplan,
                               const Options& options,
                               ArchitectureMesh& architecture_mesh) {
  const auto& wall                 = raster_architecture_floorplan.wall;
  const auto& door                 = raster_architecture_floorplan.door;
  const auto& window               = raster_architecture_floorplan.window;
  const auto& door_annotation_id   = raster_architecture_floorplan.door_annotation_id;
  const auto& window_annotation_id = raster_architecture_floorplan.window_annotation_id;

  const int kBottomZ = 0;
  const int kX = 0;
  const int kY = 1;
  const bool kFlip   = true;
  const bool kNoFlip = false;
  const int kIrrelevant = -1;

  Vector2i xs, ys;
  if (!GetMinMaxXY(architecture_floorplan, xs[0], xs[1], ys[0], ys[1]))
    return false;

  for (int y = 0; y < raster_architecture_floorplan.height - 1; ++y) {
    for (int x = 0; x < raster_architecture_floorplan.width - 1; ++x) {
      const int index = y * raster_architecture_floorplan.width + x;

      { // Horizontal surface.
        if (wall[index] != RasterArchitectureFloorplan::kNotWall) {
          Triangle triangle0, triangle1;
          vector<HorizontalSurface> surfaces;

          if (door[index] != RasterArchitectureFloorplan::kInvalid) {
            // Door in a wall.
            surfaces.push_back(HorizontalSurface(options.wall_height, kNoFlip,
                                                 HorizontalSurface::kWall, kIrrelevant));
            surfaces.push_back(HorizontalSurface(options.door_height, kFlip,
                                                 HorizontalSurface::kWall, kIrrelevant));

            surfaces.push_back(HorizontalSurface(options.door_height, kNoFlip,
                                                 HorizontalSurface::kDoor, door_annotation_id[index]));
            surfaces.push_back(HorizontalSurface(kBottomZ,            kFlip,
                                                 HorizontalSurface::kDoor, door_annotation_id[index]));
          } else if (window[index] != RasterArchitectureFloorplan::kInvalid) {
            // Window in a wall.
            surfaces.push_back(HorizontalSurface(options.wall_height,         kNoFlip,
                                                 HorizontalSurface::kWall, kIrrelevant));
            surfaces.push_back(HorizontalSurface(options.window_upper_height, kFlip,
                                                 HorizontalSurface::kWall, kIrrelevant));
            surfaces.push_back(HorizontalSurface(options.window_lower_height, kNoFlip,
                                                 HorizontalSurface::kWall, kIrrelevant));
            surfaces.push_back(HorizontalSurface(kBottomZ,                    kFlip,
                                                 HorizontalSurface::kWall, kIrrelevant));

            surfaces.push_back(HorizontalSurface(options.window_upper_height, kNoFlip,
                                                 HorizontalSurface::kWindow, window_annotation_id[index]));
            surfaces.push_back(HorizontalSurface(options.window_lower_height, kFlip,
                                                 HorizontalSurface::kWindow, window_annotation_id[index]));
          } else {
            // Just a wall.
            surfaces.push_back(HorizontalSurface(options.wall_height, kNoFlip,
                                                 HorizontalSurface::kWall, kIrrelevant));
            surfaces.push_back(HorizontalSurface(kBottomZ,            kFlip,
                                                 HorizontalSurface::kWall, kIrrelevant));
          }
          if (!FilterHorizontalSurfaces(xs, ys, x, y, options.wall_height, options.height_adjustment,
                                        options.lower_ratio, options.upper_ratio, surfaces))
            return false;

          for (const auto& surface : surfaces) {
            Mesh& mesh = GetHorizontalSurfaceMesh(architecture_floorplan,
                                                  surface.mesh_type,
                                                  surface.annotation_id,
                                                  architecture_mesh);
            if (surface.mesh_type == HorizontalSurface::kWall) {
              if (!AddHorizontalFace(x, y, surface.height, surface.flip, options.wall_uv_scale,
                                     triangle0, triangle1))
                return false;
            } else if (surface.mesh_type == HorizontalSurface::kDoor) {
              if (!AddHorizontalFaceDoorWindow(x, y, surface.height, surface.flip, options.wall_uv_scale,
                                               architecture_floorplan.doors[surface.annotation_id],
                                               options.door_rotation_degree, triangle0, triangle1))
                return false;
            } else {
              if (!AddHorizontalFaceDoorWindow(x, y, surface.height, surface.flip, options.wall_uv_scale,
                                               architecture_floorplan.windows[surface.annotation_id],
                                               options.window_rotation_degree, triangle0, triangle1))
                return false;
            }

            mesh.triangles.push_back(triangle0);
            mesh.triangles.push_back(triangle1);
          }
        }
      }
      {  // Vertical surface.
        const int kRight = 0;
        const int kBottom = 1;
        vector<VerticalSurface> surfaces;
        if (!GetVerticalSurfaceRightBottom(raster_architecture_floorplan,
                                           options, x, y, kRight, surfaces))
          return false;
        if (!GetVerticalSurfaceRightBottom(raster_architecture_floorplan,
                                           options, x, y, kBottom, surfaces))
          return false;

        if (!FilterVerticalSurfaces(xs, ys, options.wall_height, options.height_adjustment,
                                    options.lower_ratio, options.upper_ratio, surfaces))
          return false;

        for (const auto& surface : surfaces) {
          if (surface.mesh_type == VerticalSurface::kWall) {
            AddVerticalFace(surface.point0, surface.point1,
                            surface.upper_height, surface.lower_height, surface.x_or_y,
                            options.wall_uv_scale,
                            GetVerticalSurfaceMesh(architecture_floorplan,
                                                   surface.mesh_type,
                                                   surface.wall_type,
                                                   surface.annotation_id,
                                                   architecture_mesh));
          } else if (surface.mesh_type == VerticalSurface::kDoor) {
            AddVerticalFaceDoorWindow(surface.point0, surface.point1,
                                      surface.upper_height, surface.lower_height, surface.x_or_y,
                                      options.door_uv_scale,
                                      architecture_floorplan.doors[surface.annotation_id],
                                      options.door_rotation_degree,
                                      GetVerticalSurfaceMesh(architecture_floorplan,
                                                             surface.mesh_type,
                                                             surface.wall_type,
                                                             surface.annotation_id,
                                                             architecture_mesh));
          } else {
            AddVerticalFaceDoorWindow(surface.point0, surface.point1,
                                      surface.upper_height, surface.lower_height, surface.x_or_y,
                                      options.window_uv_scale,
                                      architecture_floorplan.windows[surface.annotation_id],
                                      options.window_rotation_degree,
                                      GetVerticalSurfaceMesh(architecture_floorplan,
                                                             surface.mesh_type,
                                                             surface.wall_type,
                                                             surface.annotation_id,
                                                             architecture_mesh));
          }
        }
      }
    }
  }

  return true;
}

bool GetVerticalSurfaceRightBottom(
    const RasterArchitectureFloorplan& raster_architecture_floorplan,
    const Options& options,
    const int x, const int y,
    const int right_or_bottom,
    std::vector<VerticalSurface>& surfaces) {
  const int width  = raster_architecture_floorplan.width;
  const int height = raster_architecture_floorplan.height;

  const vector<RasterArchitectureFloorplan::WallType>& wall        = raster_architecture_floorplan.wall;
  const vector<RasterArchitectureFloorplan::DirectionType>& door   = raster_architecture_floorplan.door;
  const vector<RasterArchitectureFloorplan::DirectionType>& window = raster_architecture_floorplan.window;
  const vector<int>& door_annotation_id   = raster_architecture_floorplan.door_annotation_id;
  const vector<int>& window_annotation_id = raster_architecture_floorplan.window_annotation_id;

  const RasterArchitectureFloorplan::WallType kBathroomWall = RasterArchitectureFloorplan::kBathroomWall;
  const RasterArchitectureFloorplan::WallType kKitchenWall  = RasterArchitectureFloorplan::kKitchenWall;
  const RasterArchitectureFloorplan::WallType kDiningWall   = RasterArchitectureFloorplan::kDiningWall;
  const RasterArchitectureFloorplan::WallType kBedroomWall  = RasterArchitectureFloorplan::kBedroomWall;
  const RasterArchitectureFloorplan::WallType kWall         = RasterArchitectureFloorplan::kWall;
  const RasterArchitectureFloorplan::WallType kNotWall      = RasterArchitectureFloorplan::kNotWall;

  const RasterArchitectureFloorplan::DirectionType kVertical   = RasterArchitectureFloorplan::kVertical;
  const RasterArchitectureFloorplan::DirectionType kHorizontal = RasterArchitectureFloorplan::kHorizontal;
  const RasterArchitectureFloorplan::DirectionType kInvalid    = RasterArchitectureFloorplan::kInvalid;

  const int index = y * width + x;
  const int next_index = (right_or_bottom == 0) ? index + 1 : index + width;

  const double kBottomZ = 0.0;
  const int uv_x_or_y = (right_or_bottom == 0) ? 1 : 0;
  const Vector2i point0 = (right_or_bottom == 0) ? Vector2i(x + 1, y + 1) : Vector2i(x, y + 1);
  const Vector2i point1 = (right_or_bottom == 0) ? Vector2i(x + 1, y) : Vector2i(x + 1, y + 1);
  const int kIrrelevant = -1;
  //----------------------------------------------------------------------
  if (wall[index] != kNotWall && wall[next_index] == kNotWall) {
    if (door[index] != kInvalid) {           // door.
      const int x_or_y = (door[index] == kHorizontal) ? 0 : 1;
      surfaces.push_back(VerticalSurface(VerticalSurface::kDoor, wall[index], point0, point1,
                                         options.door_height, kBottomZ, x_or_y,
                                         door_annotation_id[index]));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[index], point0, point1,
                                         options.wall_height, options.door_height, uv_x_or_y,
                                         kIrrelevant));
    } else if (window[index] != kInvalid) {  // window.
      const int x_or_y = (window[index] == kHorizontal) ? 0 : 1;
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[index], point0, point1,
                                         options.wall_height, options.window_upper_height, uv_x_or_y,
                                         kIrrelevant));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWindow, wall[index], point0, point1,
                                         options.window_upper_height, options.window_lower_height, x_or_y,
                                         window_annotation_id[index]));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[index], point0, point1,
                                         options.window_lower_height, kBottomZ, uv_x_or_y,
                                         kIrrelevant));
    } else {                                 // wall.
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[index], point0, point1,
                                         options.wall_height, kBottomZ, uv_x_or_y,
                                         kIrrelevant));
    }
  } else if (wall[index] == kNotWall && wall[next_index] != kNotWall) {
    if (door[next_index] != kInvalid) {           // door.
      const int x_or_y = (door[next_index] == kHorizontal) ? 0 : 1;
      surfaces.push_back(VerticalSurface(VerticalSurface::kDoor, wall[next_index], point1, point0,
                                         options.door_height, kBottomZ, x_or_y,
                                         door_annotation_id[next_index]));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[next_index], point1, point0,
                                         options.wall_height, options.door_height, uv_x_or_y,
                                         kIrrelevant));
    } else if (window[next_index] != kInvalid) {  // window.
      const int x_or_y = (window[next_index] == kHorizontal) ? 0 : 1;
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[next_index], point1, point0,
                                         options.wall_height, options.window_upper_height, uv_x_or_y,
                                         kIrrelevant));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWindow, wall[next_index], point1, point0,
                                         options.window_upper_height, options.window_lower_height, x_or_y,
                                         window_annotation_id[next_index]));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[next_index], point1, point0,
                                         options.window_lower_height, kBottomZ, uv_x_or_y,
                                         kIrrelevant));
    } else {                                 // wall.
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[next_index], point1, point0,
                                         options.wall_height, kBottomZ, uv_x_or_y,
                                         kIrrelevant));
    }
  } else if (wall[index] != kNotWall && wall[next_index] != kNotWall) {
    if (door[index] != kInvalid && door[next_index] == kInvalid) {
      const int x_or_y = (door[index] == kHorizontal) ? 0 : 1;
      surfaces.push_back(VerticalSurface(VerticalSurface::kDoor, wall[index], point0, point1,
                                         options.door_height, kBottomZ, x_or_y,
                                         door_annotation_id[index]));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[index], point1, point0,
                                         options.door_height, kBottomZ, uv_x_or_y,
                                         kIrrelevant));
    } else if (door[index] == kInvalid && door[next_index] != kInvalid) {
      const int x_or_y = (door[next_index] == kHorizontal) ? 0 : 1;
      surfaces.push_back(VerticalSurface(VerticalSurface::kDoor, wall[next_index], point1, point0,
                                         options.door_height, kBottomZ, x_or_y,
                                         door_annotation_id[next_index]));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[next_index], point0, point1,
                                         options.door_height, kBottomZ, uv_x_or_y,
                                         kIrrelevant));
    } else if (window[index] != kInvalid && window[next_index] == kInvalid) {
      const int x_or_y = (window[index] == kHorizontal) ? 0 : 1;
      surfaces.push_back(VerticalSurface(VerticalSurface::kWindow, wall[index], point0, point1,
                                         options.window_upper_height, options.window_lower_height, x_or_y,
                                         window_annotation_id[index]));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[index], point1, point0,
                                         options.window_upper_height, options.window_lower_height, uv_x_or_y,
                                         kIrrelevant));
    } else if (window[index] == kInvalid && window[next_index] != kInvalid) {
      const int x_or_y = (window[next_index] == kHorizontal) ? 0 : 1;
      surfaces.push_back(VerticalSurface(VerticalSurface::kWindow, wall[next_index], point1, point0,
                                         options.window_upper_height, options.window_lower_height, x_or_y,
                                         window_annotation_id[next_index]));
      surfaces.push_back(VerticalSurface(VerticalSurface::kWall, wall[next_index], point0, point1,
                                         options.window_upper_height, options.window_lower_height, uv_x_or_y,
                                         kIrrelevant));
    }
  } else {
    // None.
  }

  return true;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/divides.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mtwo.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>

STF_CASE_TPL (" bs::saturated_(divides) signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::divides;

  // return type conformity test
  STF_EXPR_IS(bs::saturated_(divides)(T(), T()), T);

  // specific values tests
  STF_EQUAL(bs::saturated_(divides)(bs::Mone<T>(), bs::Mone<T>()), bs::One<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Valmax<T>(),bs::Mone<T>()), bs::Valmin<T>()+bs::One<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Valmax<T>(),bs::One<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Valmin<T>(),bs::Mone<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Mone<T>(), bs::Zero<T>()), bs::Valmin<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Mtwo<T>(), bs::Zero<T>()), bs::Valmin<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Valmin<T>(), bs::Zero<T>()), bs::Valmin<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::One<T>(), bs::Zero<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Two<T>(), bs::Zero<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Valmax<T>(), bs::Zero<T>()), bs::Valmax<T>());
} // end of test for signed_int_

STF_CASE_TPL (" bs::saturated_(divides) unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::divides;

  // return type conformity test
  STF_EXPR_IS(bs::saturated_(divides)(T(), T()), T);

  // specific values tests
  STF_EQUAL(bs::saturated_(divides)(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Valmax<T>(),bs::One<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::One<T>(), bs::Zero<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Two<T>(), bs::Zero<T>()), bs::Valmax<T>());
  STF_EQUAL(bs::saturated_(divides)(bs::Valmax<T>(), bs::Zero<T>()), bs::Valmax<T>());
} // end of test for unsigned_int_


#ifndef CLUSTERING_HPP
#define CLUSTERING_HPP

#include <unordered_set>

#include <boost/numeric/ublas/matrix.hpp>

#include "dmatrix.hpp"

using namespace graphs;

auto centroid(const Map& map, Locations locs) -> Building;

auto find_nearest_building(const Map& map, Location loc) -> Building;

struct Cluster {
    Cluster() = delete;

    explicit Cluster(size_t first, Building b, size_t id)
        : m_left(nullptr)
        , m_right(nullptr)
        , m_size(1)
        , m_first(first)
        , m_last(first)
        , m_id(id)
        , m_center(std::move(b)) {}

    explicit Cluster(Cluster& c1, Cluster& c2, Building b, size_t id)
        : m_left(&c1)
        , m_right(&c2)
        , m_size(c1.m_size + c2.m_size)
        , m_first(c1.m_first)
        , m_last(c2.m_last)
        , m_id(id)
        , m_center(std::move(b)) {}

    [[nodiscard]] auto first() const { return m_first; }
    [[nodiscard]] auto last() const { return m_last; }
    [[nodiscard]] auto size() const { return m_size; }
    [[nodiscard]] auto left() const { return m_left; }
    [[nodiscard]] auto right() const { return m_right; }
    [[nodiscard]] auto id() const { return m_id; }
    [[nodiscard]] auto centroid() const { return m_center; }

    bool operator<(const Cluster& other) const { return m_id < other.m_id; }
    bool operator==(const Cluster& other) const { return m_id == other.m_id; }

private:
    Cluster* m_left;    //  subclusters
    Cluster* m_right;   //
    size_t m_size; // cluster size
    size_t m_first; // index of the first cluster element
    size_t m_last; // index of the last cluster element
    size_t m_id; // cluster id
    Building m_center;
};

namespace std {
template<>
struct hash<Cluster> {
    size_t operator()(const Cluster& b) const { return b.id(); }
};
}

using Clusters = std::vector<Cluster>;

using boost::numeric::ublas::matrix;

struct ClusterStructure {
    ClusterStructure() = delete;

    ClusterStructure(const Map& map, Buildings&& buildings, DMatrix<Building>&& dm);

    auto merge_clusters(size_t id1, size_t id2) -> Cluster;

    void print_cluster_structure(std::ostream& out = std::cout, const Cluster* curr_cl = nullptr,
                                 int level = 0) const;

    auto get_elements(size_t id) const -> std::vector<Building>;

    [[nodiscard]] auto root() const { return m_root; }
    [[nodiscard]] auto clusters() const { return m_clusters; }

    void cluster_from_element(size_t ind, Building b);

    auto clusters_num() const { return m_clusters_num; }

private:
    const Cluster* m_root = nullptr;
    Buildings m_data;
    Clusters m_clusters {};
    DMatrix<Building> m_dm_buildings;
    matrix<uint64_t> m_dm_clusters;
    std::vector<int64_t> _m_next;
    size_t m_clusters_num;
    const Map& m_map;
};

#endif //CLUSTERING_HPP

/**
 *
 * @param cl_st
 * @param k
 * @return
 */
Clusters get_k_clusters(const ClusterStructure& cl_st, size_t k);

Maps clusters_to_maps(const Map& map, const Clusters& cls, const ClusterStructure& cl_st);


// Copyright (C) 2017-2019 Chris N. Richardson and Garth N. Wells
//
// This file is part of FEniCS-miniapp (https://www.fenicsproject.org)
//
// SPDX-License-Identifier:    MIT

#include "elasticity_problem.h"
#include "Elasticity.h"
#include <Eigen/Dense>
#include <dolfinx/common/Timer.h>
#include <dolfinx/fem/DirichletBC.h>
#include <dolfinx/fem/DofMap.h>
#include <dolfinx/fem/Form.h>
#include <dolfinx/fem/Function.h>
#include <dolfinx/fem/FunctionSpace.h>
#include <dolfinx/fem/assembler.h>
#include <dolfinx/fem/petsc.h>
#include <dolfinx/la/PETScMatrix.h>
#include <dolfinx/la/PETScVector.h>
#include <dolfinx/la/VectorSpaceBasis.h>
#include <dolfinx/la/utils.h>
#include <dolfinx/mesh/Geometry.h>
#include <dolfinx/mesh/Mesh.h>
#include <memory>
#include <utility>

namespace
{
// Function to compute the near nullspace for elasticity - it is made up
// of the six rigid body modes
dolfinx::la::VectorSpaceBasis
build_near_nullspace(const dolfinx::fem::FunctionSpace& V)
{
  // Create vectors for nullspace basis
  auto map = V.dofmap()->index_map;
  int bs = V.dofmap()->index_map_bs();
  const std::int32_t length_block = map->size_local() + map->num_ghosts();
  const std::int32_t length = bs * length_block;
  Eigen::Matrix<PetscScalar, Eigen::Dynamic, 6> basis
      = Eigen::Matrix<PetscScalar, Eigen::Dynamic, 6>::Zero(length, 6);

  // NOTE: The below will be simpler once Eigen 3.4 is released, see
  //
  // http://eigen.tuxfamily.org/dox-devel/group__TutorialSlicingIndexing.html

  // x0, x1, x2 translations
  for (int k = 0; k < 3; ++k)
  {
    for (std::int32_t i = 0; i < length_block; ++i)
      basis(bs * i + k, k) = 1.0;
  }

  // Rotations
  const Eigen::Array<double, Eigen::Dynamic, 3, Eigen::RowMajor> x
      = V.tabulate_dof_coordinates();
  auto& dofs = V.dofmap()->list().array();
  for (int i = 0; i < dofs.size(); ++i)
  {
    basis.col(3)(bs * dofs[i] + 0) = -x(dofs[i], 1);
    basis.col(3)(bs * dofs[i] + 1) = x(dofs[i], 0);

    basis.col(4)(bs * dofs[i] + 0) = x(dofs[i], 2);
    basis.col(4)(bs * dofs[i] + 2) = -x(dofs[i], 0);

    basis.col(5)(bs * dofs[i] + 2) = x(dofs[i], 1);
    basis.col(5)(bs * dofs[i] + 1) = -x(dofs[i], 2);
  }

  const std::int32_t size = map->size_local() * bs;
  const std::int64_t size_global = map->size_global() * bs;
  std::vector<std::shared_ptr<dolfinx::la::PETScVector>> basis_vec;
  for (int i = 0; i < 6; ++i)
  {
    Vec vec0, vec1;
    VecCreateMPIWithArray(V.mesh()->mpi_comm(), 3, size, size_global,
                          basis.col(i).data(), &vec0);
    VecDuplicate(vec0, &vec1);
    VecCopy(vec0, vec1);
    VecDestroy(&vec0);
    basis_vec.push_back(
        std::make_shared<dolfinx::la::PETScVector>(vec1, false));
  }

  // Create vector space and orthonormalize
  dolfinx::la::VectorSpaceBasis vector_space(basis_vec);
  vector_space.orthonormalize();
  if (!vector_space.is_orthonormal())
    throw std::runtime_error("Space not orthonormal");
  return vector_space;
}
} // namespace

std::tuple<dolfinx::la::PETScMatrix, dolfinx::la::PETScVector,
           std::shared_ptr<dolfinx::fem::Function<PetscScalar>>>
elastic::problem(std::shared_ptr<dolfinx::mesh::Mesh> mesh)
{
  dolfinx::common::Timer t0("ZZZ FunctionSpace");

  auto V = dolfinx::fem::create_functionspace(
      create_functionspace_form_Elasticity_a, "u", mesh);

  t0.stop();

  dolfinx::common::Timer t0a("ZZZ Create boundary conditions");

  // Define boundary condition
  auto u0 = std::make_shared<dolfinx::fem::Function<PetscScalar>>(V);
  std::fill(u0->x()->mutable_array().begin(), u0->x()->mutable_array().end(),
            0.0);

  const std::vector<std::int32_t> bdofs = dolfinx::fem::locate_dofs_geometrical(
      {*V}, [](auto& x) { return x.row(1) < 1.0e-8; });

  // Bottom (x[1] = 0) surface
  auto bc = std::make_shared<dolfinx::fem::DirichletBC<PetscScalar>>(u0, bdofs);

  t0a.stop();

  dolfinx::common::Timer t0b("ZZZ Create RHS function");

  // Define coefficients
  auto f = std::make_shared<dolfinx::fem::Function<PetscScalar>>(V);
  f->interpolate([](auto& x) {
    auto dx = x.row(0) - 0.5;
    auto dz = x.row(2) - 0.5;
    auto r = dx * dx + dz * dz;
    Eigen::Array<double, 3, Eigen::Dynamic, Eigen::RowMajor> values(3,
                                                                    x.cols());
    values.row(0) = -dz * r.sqrt() * x.row(1);
    values.row(1) = 1.0;
    values.row(2) = dx * r.sqrt() * x.row(1);
    return values;
  });

  t0b.stop();

  dolfinx::common::Timer t0c("ZZZ Create forms");

  // Define variational forms
  auto L = dolfinx::fem::create_form<PetscScalar>(create_form_Elasticity_L, {V},
                                                  {{"f", f}}, {}, {});
  auto a = dolfinx::fem::create_form<PetscScalar>(create_form_Elasticity_a,
                                                  {V, V}, {}, {}, {});
  t0c.stop();

  // Create matrices and vector, and assemble system
  dolfinx::la::PETScMatrix A(dolfinx::fem::create_matrix(*a), false);
  dolfinx::la::PETScVector b(*L->function_spaces()[0]->dofmap()->index_map,
                             L->function_spaces()[0]->dofmap()->index_map_bs());

  dolfinx::common::Timer t2("ZZZ Assemble matrix");
  dolfinx::fem::assemble_matrix(dolfinx::la::PETScMatrix::add_block_fn(A.mat()),
                                *a, {bc});
  dolfinx::fem::add_diagonal(dolfinx::la::PETScMatrix::add_fn(A.mat()), *V,
                             {bc});
  MatAssemblyBegin(A.mat(), MAT_FINAL_ASSEMBLY);
  MatAssemblyEnd(A.mat(), MAT_FINAL_ASSEMBLY);
  t2.stop();

  VecSet(b.vec(), 0.0);
  VecGhostUpdateBegin(b.vec(), INSERT_VALUES, SCATTER_FORWARD);
  VecGhostUpdateEnd(b.vec(), INSERT_VALUES, SCATTER_FORWARD);

  dolfinx::common::Timer t3("ZZZ Assemble vector");
  dolfinx::fem::assemble_vector_petsc(b.vec(), *L);
  dolfinx::fem::apply_lifting_petsc(b.vec(), {a}, {{bc}}, {}, 1.0);
  VecGhostUpdateBegin(b.vec(), ADD_VALUES, SCATTER_REVERSE);
  VecGhostUpdateEnd(b.vec(), ADD_VALUES, SCATTER_REVERSE);
  dolfinx::fem::set_bc_petsc(b.vec(), {bc}, nullptr);
  t3.stop();

  dolfinx::common::Timer t4("ZZZ Create near-nullspace");

  // Create Function to hold solution
  auto u = std::make_shared<dolfinx::fem::Function<PetscScalar>>(V);

  // Build near-nullspace and attach to matrix
  dolfinx::la::VectorSpaceBasis nullspace = build_near_nullspace(*V);
  A.set_near_nullspace(nullspace);

  t4.stop();

  return {std::move(A), std::move(b), u};
}


#include <map>
#include <string>
#include "math.h"
#include <cmath>
#include <vector>
#include <boost/algorithm/clamp.hpp>

#include "ros/ros.h"
#include "tf/transform_datatypes.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Pose2D.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

using namespace geometry_msgs;
using std::string;

namespace ros_helpers{ namespace geometry{

template <class T1>
inline double_t length_xy(const T1 &xy){
    return std::hypot(xy.x, xy.y);
}

template <class T2>
inline double_t angle_xy(const T2 &xy){
    return atan2(xy.y, xy.x);
}

inline Point vector3_to_point(const Vector3 &v3){
    Point p;
    p.x = v3.x;
    p.y = v3.y;
    p.z = v3.z;
    return p;
}

inline Vector3 point_to_vector3(const Point &p){
    Vector3 v;
    v.x = p.x;
    v.y = p.y;
    v.z = p.z;
    return v;
}

inline Vector3 vector_bw_points(const Point &p0, const Point &p1){
    Vector3 result;
    result.x = p1.x - p0.x;
    result.y = p1.y - p0.y;
    result.z = p1.z - p0.z;
    return result;
}

inline double dist(const Point &p0, const Point &p1){
    const float dx = p1.x - p0.x;
    const float dy = p1.y - p0.y;
    const float dz = p1.z - p0.z;
    return sqrt(dx*dx + dy*dy + dz*dz);
}

inline double angle_xy(const Point &p0, const Point &p1){
    const double dx = p1.x - p0.x;
    const double dy = p1.y - p0.y;
    return (double)atan2(dy, dx);
}

inline bool is_points_equal(const Point& p0, const Point& p1){
        if(p0.x != p1.x | p0.y != p0.y | p0.z != p1.z) return false;
        else return true;
    }

template <class XYZ>
inline bool is_equal(const XYZ &a, const XYZ &b){
        if(a.x != b.x | a.y != b.y | a.z != b.z) return false;
        else return true;
}
inline bool is_equal(const Quaternion &a, const Quaternion &b){
    if(a.x != b.x || a.y != b.y || a.z != b.z || a.w != a.w) return false;
    else return true;
}

inline bool is_empty(const Quaternion &q){
    if(q.x != 0 || q.y != 0 || q.z != 0 || q.w != 0) return false;
    else return true;
}

inline bool is_empty(const Point &p){
    if(p.x != 0 || p.y != 0 || p.z != 0) return false;
    else return true;
}

inline Point tf_point(const Point &point, const TransformStamped &tf){
    Point result;
    result.x = point.x + tf.transform.translation.x;
    result.y = point.y + tf.transform.translation.y;
    result.z = point.z + tf.transform.translation.z;
    return result;
}

inline Pose pose2d_to_pose(const Pose2D &pose2d){
    Pose pose;
    pose.position.x = pose2d.x;
    pose.position.y = pose2d.y;
    tf::Quaternion q_tf = tf::createQuaternionFromYaw(pose2d.theta);
    tf::quaternionTFToMsg(q_tf, pose.orientation);
    return pose;
}

template <class T>
inline void get_param(const string &name, T& xyz, const ros::NodeHandle *nh=NULL){
    std::map<string, double> map;
    ros_helpers::get_param(name, map, nh);

    // setting fields is optional - do nothing with exceptions
    try{xyz.x = map.at("x");}catch(const std::out_of_range &e){ros_helpers::error(e, "get_param(xyz"+name+".x)");}
    try{xyz.y = map.at("y");}catch(const std::out_of_range &e){ros_helpers::error(e, "get_param(xyz"+name+".y)");}
    try{xyz.z = map.at("z");}catch(const std::out_of_range &e){ros_helpers::warn(e.what(), "get_param("+name+".z)");}
}

inline void get_param(const string &name, geometry_msgs::Quaternion& quat){
    std::map<string, double> map;
    ros::param::get(name, map);
    // setting fields is optional - do nothing with exceptions
    try{quat.x = map.at("x");}catch(std::out_of_range){}
    try{quat.y = map.at("y");}catch(std::out_of_range){}
    try{quat.z = map.at("z");}catch(std::out_of_range){}
    try{quat.w = map.at("w");}catch(std::out_of_range){}
    if(quat.x==0 && quat.y==0 && quat.z==0) quat.w = 1; // ensure proper default init (all 0s is non-sensical)
}

// Retrieves a Pose2D msg. Theta in degrees.
inline void get_param(const string &name, geometry_msgs::Pose2D &pose_2d, const ros::NodeHandle *nh=NULL){
    std::map<string, double> map;
    ros_helpers::get_param(name, map, nh);
    try{pose_2d.x = map.at("x");}catch(const std::out_of_range &){ROS_WARN("%s.x not retrieved.", name.c_str());}
    try{pose_2d.y = map.at("y");}catch(const std::out_of_range &){ROS_WARN("%s.y not retrieved.", name.c_str());}
    try{pose_2d.theta = map.at("theta")*M_PI/180;}catch(const std::out_of_range &){ROS_WARN("%s.theta not retrieved.", name.c_str());}
}

// Calculates the point in x-y at a given distance and angle from a base point
inline Point point_from_dist_angle(const Point &start, const double_t &angle, const double_t &dist){
    Point result;
    result.x = start.x + dist*cos(angle);
    result.y = start.y + dist*sin(angle);
    return result;
}

// Calculates N Points spaced evenly b/w start and end Points
std::vector<Point> linspace_xy(const Point &start, const Point &end, const int &N){

    std::vector<Point> result; // build and return

    const double_t d_start_end = ros_helpers::geometry::dist(start, end);
    const double_t d_spacing = d_start_end/N;
    const double_t theta_start_end = ros_helpers::geometry::angle_xy(start, end);

    result.push_back(start);
    for(int i=0; i<N; i++){ // add points to result
        if(result.empty())
            result.push_back(ros_helpers::geometry::point_from_dist_angle(*std::unique_ptr<Point>{new Point()}, theta_start_end, d_spacing)); // TODO will this cause memory leak ?
        else
            result.push_back(ros_helpers::geometry::point_from_dist_angle(result.back(), theta_start_end, d_spacing));
    }
    return result;
}

// Returns the inverse of a TransformStamped
inline TransformStamped inverse_transform_stamped(const TransformStamped &tf_s){

    TransformStamped result; // build and return

    // reverse frame_ids
    result.child_frame_id = tf_s.header.frame_id;
    result.header.frame_id = tf_s.child_frame_id;

    result.header.stamp = ros::Time::now(); // update timestamp

    // inverse transform
    tf2::Transform tmp;
    tf2::convert(tf_s.transform, tmp); // geometry_msgs to tf2
    tmp = tmp.inverse(); // inverse
    tf2::convert(tmp, result.transform); // tf2 to geometry_msgs

    return result;
}

// Returns first and last points if angle b/w them < max_angle, otherwise original vector
inline std::vector<Point> approx_straight_points(const std::vector<Point> &points, const double &max_angle) {
    const double angle = ros_helpers::geometry::angle_xy(points.front(), points.back()); // angle b/w first and last point in x-y
    if(angle > max_angle){ // return just first and last points
        std::vector<Point> points_out;
        points_out.push_back(points.front());
        points_out.push_back(points.back());
        return points_out;
    }
    else return points;
}

// clamps the angle of a point in x-y in its coordinate frame b/w lower and upper bounds
inline Point clamp_point_angle(const Point &point_in, const double &lower_bound, const double &upper_bound){

    Point point_out; // build and return

    // calc angle
    Point origin; // (0,0,0)
    double angle = ros_helpers::geometry::angle_xy(origin, point_in);

    // clamp angle
    angle = boost::algorithm::clamp(angle, lower_bound, upper_bound);

    // rebuild point
    const double hypot = ros_helpers::geometry::dist(origin, point_in); // hypotenuse in x-y
    point_out.x = hypot * cos(angle);
    point_out.y = hypot * sin(angle);

    return point_out;
}

// Builds a PointStamped msg using the header and position of the PoseStamped msg
inline PointStamped pointstamped_from_posestamped(const PoseStamped &pose_stamped){
    PointStamped result;
    result.header = pose_stamped.header;
    result.point = pose_stamped.pose.position;
    return result;
}

// Returns a point dist metres from Point a along angle b/w Points a and b in x-y
inline Point extrap_interp_points(const Point &a, const Point &b, const double_t &dist){
    const double_t angle = ros_helpers::geometry::angle_xy(a, b);
    return ros_helpers::geometry::point_from_dist_angle(a, angle, dist);
}

// Builds a PointStamped msg from a Header and Point (factory).
inline geometry_msgs::PointStamped PointStamped(const std_msgs::Header &header, const geometry_msgs::Point &point){
    geometry_msgs::PointStamped result;
    result.header = header;
    result.point = point;
    return result;
}

// Scales a Point or Vector3
template <class XYZ>
inline XYZ scale(const XYZ &point, const double_t &scalar){
    XYZ result;
    result.x = scalar * point.x;
    result.y = scalar * point.y;
    result.z = scalar * point.z;
    return result;
}

// Adds two Points or two Vector3s
template <class XYZ>
inline XYZ add(const XYZ &a, const XYZ &b){
    XYZ result;
    result.x = a.x + b.x;
    result.y = a.y + b.y;
    result.z = a.z + b.z;
    return result;
}

// Subtracts two Points or two Vector3s (a-b)
template <class XYZ>
inline XYZ subtract(const XYZ &a, const XYZ &b){
    XYZ result;
    result.x = a.x - b.x;
    result.y = a.y - b.y;
    result.z = a.z - b.z;
    return result;
}

}} // ros_helpers::geometry


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <math.h>
#include <cassert>
#include <Eigen/Core>
#include "metro/log_sum_exp.hpp"

namespace metro {
	void rowwise_log_sum_exp( Eigen::MatrixXd const& data, Eigen::VectorXd* result ) {
		assert( result ) ;
		assert( result->size() == data.rows() ) ;

		for( int i = 0; i < result->size(); ++i ) {
			(*result)(i) = log_sum_exp( data.row(i) ) ;
		}
	}
	void rowwise_log_sum_exp( Eigen::MatrixXd const& data, Eigen::MatrixXd const& nonmissingness, Eigen::VectorXd* result ) {
		assert( result ) ;
		assert( result->size() == data.rows() ) ;
		assert( data.rows() == nonmissingness.rows() ) ;
		assert( data.cols() == nonmissingness.cols() ) ;

		for( int i = 0; i < result->size(); ++i ) {
			(*result)(i) = log_sum_exp( data.row(i), nonmissingness.row(i) ) ;
		}
	}
}


#include "Analysis.h"

#include "Processing.h"

#include <unordered_map>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

template<typename T>
std::common_type_t<T, double> vectorNthElement(std::vector<T> &data, int32_t n)
{
    assert(n >= 0 && n < data.size());
    std::nth_element(data.begin(), data.begin() + n, data.end());
    return data[n];
}

template<typename T>
std::common_type_t<T, double> vectorQuantile(std::vector<T> &data, double q = 0.5)
{
    assert(!data.empty());

    if(q >= 1.0)
        return *std::max_element(data.begin(), data.end());
    if(q <= 0)
        return *std::min_element(data.begin(), data.end());

    q = std::clamp(q, 0.0, 1.0);
    const double n = data.size() * q - 0.5;
    const int n1 = static_cast<int>(std::floor(n));
    const int n2 = static_cast<int>(std::ceil(n));
    const auto t = n - n1;
    std::nth_element(data.begin(), data.begin() + n1, data.end());
    std::nth_element(data.begin() + n1, data.begin() + n2, data.end());
    return lerp<double>(data[n1], data[n2], t);
}

template<arrow::Type::type id>
std::shared_ptr<arrow::Table> countValueTyped(const arrow::Column &column)
{
    using T = typename TypeDescription<id>::ObservedType;
    using ArrowType = typename TypeDescription<id>::ArrowType;
    using Builder = typename TypeDescription<id>::BuilderType;
    std::unordered_map<T, int64_t> valueCounts;

    iterateOver<id>(column,
        [&] (auto &&elem) { valueCounts[elem]++; },
        [] () {});

    auto valueBuilder = makeBuilder(std::static_pointer_cast<ArrowType>(column.field()->type()));
    arrow::Int64Builder countBuilder;

    valueBuilder->Reserve(valueCounts.size());
    countBuilder.Reserve(valueCounts.size());

    for(auto && [value, count] : valueCounts)
    {
        append(*valueBuilder, value);
        append(countBuilder, count);
    }

    if(column.null_count())
    {
        valueBuilder->AppendNull();
        countBuilder.Append(column.null_count());
    }

    auto valueArray = finish(*valueBuilder);
    auto countArray = finish(countBuilder);

    auto valueColumn = std::make_shared<arrow::Column>(arrow::field("value", column.type(), column.null_count()), valueArray);
    auto countColumn = std::make_shared<arrow::Column>(arrow::field("count", countArray->type(), false), countArray);

    return tableFromArrays({valueArray, countArray}, {"value", "count"});
}

template<typename T>
constexpr bool doesAggregatorAllowsType(AggregateFunction aggregator)
{
    // element counter doesn't care what element is
    if(aggregator == AggregateFunction::Length)
        return true;

    return std::is_arithmetic_v<T>;
}

template<typename T>
struct Minimum
{
    T accumulator = std::numeric_limits<T>::max();
    static constexpr const char *name = "min";
    static constexpr int32_t RequiredSampleCount = 1;
    void operator() (T elem) { accumulator = std::min<T>(accumulator, elem); }
    void operator() () {}
    auto get() { return accumulator; }
};

template<typename T>
struct Maximum
{
    T accumulator = std::numeric_limits<T>::min();
    static constexpr const char *name = "max";
    static constexpr int32_t RequiredSampleCount = 1;
    void operator() (T elem) { accumulator = std::max<T>(accumulator, elem); }
    void operator() () {}
    auto get() { return accumulator; }
};

// TODO naive implementation, look for something numerically better
template<typename T>
struct Mean
{
    int64_t count = 0;
    T accumulator = 0;
    static constexpr const char *name = "mean";
    static constexpr int32_t RequiredSampleCount = 1;
    void operator() (T elem) { accumulator += elem; count++; }
    void operator() () {}
    auto get() { return accumulator / (double)count; }
};

template<typename T>
struct Median
{
    std::vector<T> values;

    static constexpr const char *name = "median";
    static constexpr int32_t RequiredSampleCount = 1;
    void operator() (T elem) { values.push_back(elem); }
    void operator() () {}
    auto get() { return vectorQuantile(values, 0.5); }
};

template<typename T>
struct Variance
{
    boost::accumulators::accumulator_set<T, boost::accumulators::features<boost::accumulators::tag::variance>> accumulator;

    static constexpr const char *name = "variance";
    static constexpr int32_t RequiredSampleCount = 2;
    void operator() (T elem) { accumulator(elem); }
    void operator() () {}
    auto get() { return boost::accumulators::variance(accumulator); }
};

template<typename T>
struct StdDev : Variance<T>
{
    static constexpr const char *name = "std dev";
    auto get() { return std::sqrt(Variance<T>::get()); }
};

template<typename T>
struct Sum : Variance<T>
{
    T accumulator{};
    static constexpr const char *name = "sum";
    static constexpr int32_t RequiredSampleCount = 0;
    void operator() (T elem) { accumulator += elem; }
    void operator() () {}
    T get() { return accumulator; }
};

struct Length
{
    int64_t length = 0;
    static constexpr const char *name = "length";
    static constexpr int32_t RequiredSampleCount = 0;

    template<typename T>
    void operator()(T &&)
    {
        length++;
    }
    void operator()()
    {
        length++;
    }

    int64_t get() { return length; }
};

template<typename T>
struct First
{
    std::optional<T> value;
    static constexpr const char *name = "first";
    static constexpr int32_t RequiredSampleCount = 1;

    void operator()(T t)
    {
        if(!value)
            value = t;
    }
    void operator() () {}
    T get() { return value.value_or(T{}); }
};

template<typename T>
struct Last
{
    T value{};
    static constexpr const char *name = "last";
    static constexpr int32_t RequiredSampleCount = 1;

    void operator()(T t)
    {
        value = t;
    }
    void operator() () {}
    T get() { return value; }

};

template<typename T>
struct RSI
{
    static constexpr const char *name = "RSI";
    static constexpr int32_t RequiredSampleCount = 1;

    Mean<T> down, up;

    void operator()(T value)
    {
        up(std::max<T>(value, 0.0));
        down(std::min<T>(0.0, value));
    }
    void operator() () {}

    double get()
    {
        const auto upMean = up.get();
        const auto downMean = down.get();
        return 100.0 * upMean / (upMean - downMean);
    }

};

template<AggregateFunction aggr, typename T>
struct AggregatorFor {};

template<typename T> struct AggregatorFor<AggregateFunction::Minimum, T> { using type = Minimum<T>; };
template<typename T> struct AggregatorFor<AggregateFunction::Maximum, T> { using type = Maximum<T>; };
template<typename T> struct AggregatorFor<AggregateFunction::Mean   , T> { using type = Mean<T>   ; };
template<typename T> struct AggregatorFor<AggregateFunction::Length , T> { using type = Length    ; };
template<typename T> struct AggregatorFor<AggregateFunction::Median , T> { using type = Median<T> ; };
template<typename T> struct AggregatorFor<AggregateFunction::First  , T> { using type = First<T>  ; };
template<typename T> struct AggregatorFor<AggregateFunction::Last   , T> { using type = Last<T>   ; };
template<typename T> struct AggregatorFor<AggregateFunction::Sum    , T> { using type = Sum<T>    ; };
template<typename T> struct AggregatorFor<AggregateFunction::RSI    , T> { using type = RSI<T>    ; };
template<typename T> struct AggregatorFor<AggregateFunction::StdDev , T> { using type = StdDev<T> ; };

template<arrow::Type::type id, typename Processor>
auto calculateStatScalar(const arrow::Column &column, Processor &p)
{
    iterateOver<id>(column,
        [&] (auto elem) { p(toStorage(elem)); },
        [] {});

    return p.get();
}

// Helper for providing fast path for single-chunked column index accessing
template<typename F>
auto dispatchIndexable(const std::shared_ptr<arrow::Column> &column, F &&f)
{
    if(column->data()->num_chunks() == 1)
        return f(column->data()->chunk(0));
    else
        return f(column);
}

/// workaround for VS 15.9 regression, see function below
template<template <typename> typename Processor>
struct CalculateStatVisitor
{
    const arrow::Column &column;
    CalculateStatVisitor(const arrow::Column &column) : column(column) {}

    std::shared_ptr<arrow::Column> operator()(std::integral_constant<arrow::Type::type, arrow::Type::STRING> id) const
    {
        THROW("Operation {} not supported for type {}", Processor<double>::name, column.type()->ToString());
    }
    std::shared_ptr<arrow::Column> operator()(std::integral_constant<arrow::Type::type, arrow::Type::TIMESTAMP> id) const
    {
        THROW("Operation {} not supported for type {}", Processor<double>::name, column.type()->ToString());
    }

    template<typename IDType>
    std::shared_ptr<arrow::Column> operator()(IDType id) const
    {
        using T = typename TypeDescription<id.value>::ValueType;
        Processor<T> p;
        using ResultT = decltype(p.get());

        if (column.length() - column.null_count() <= 0)
            return toColumn(std::vector<std::optional<ResultT>>{std::nullopt}, p.name);

        const auto result = calculateStatScalar<id.value>(column, p);
        return toColumn(std::vector<ResultT>{result}, { p.name });
    }
};

template<template <typename> typename Processor>
std::shared_ptr<arrow::Column> calculateStat(const arrow::Column &column)
{
    // workaround for VS 15.9 regression, see https://developercommunity.visualstudio.com/content/problem/395993/159-regression-error-on-valid-c-code-constexpr-if.html
    // FIXME restore code commented out as soon as our CI uses VS 15.8 or 16.0
    return visitType(*column.type(), CalculateStatVisitor<Processor>(column));
//     return visitType(*column.type(), [&](auto id) -> std::shared_ptr<arrow::Column>
//     {
//         if constexpr(id.value != arrow::Type::STRING && id.value != arrow::Type::TIMESTAMP)
//         {
//             using T = typename TypeDescription<id.value>::ValueType;
//             Processor<T> p;
//             using ResultT = decltype(p.get());
//
//             if(column.length() - column.null_count() <= 0)
//                 return toColumn(std::vector<std::optional<ResultT>>{std::nullopt}, p.name);
//
//             const auto result = calculateStatScalar<id.value>(column, p);
//             return toColumn(std::vector<ResultT>{result}, { p.name });
//         }
//         else
//             THROW("Operation {} not supported for type {}", Processor<double>::name, column.type()->ToString());
//     });
}

std::shared_ptr<arrow::Column> calculateQuantile(const arrow::Column &column, double q, std::string name)
{
    // return calculateStat<Median>(column);
    auto v = toJustVector(column);
    return visit([&] (auto &vector) -> std::shared_ptr<arrow::Column>
    {
        using VectorType = std::decay_t<decltype(vector)>;
        using T = typename VectorType::value_type;
        if constexpr(std::is_arithmetic_v<T>)
        {
            auto result = vectorQuantile(vector, q);
            return scalarToColumn(result, name);
        }
        else
            throw std::runtime_error(name + " is allowed only for arithmetics type");
    }, v);
}

std::shared_ptr<arrow::Table> countValues(const arrow::Column &column)
{
    return visitType(*column.type(), [&] (auto id)
    {
        return countValueTyped<id.value>(column);
    });
}

std::shared_ptr<arrow::Column> calculateMin(const arrow::Column &column)
{
    return calculateStat<Minimum>(column);
}

std::shared_ptr<arrow::Column> calculateMax(const arrow::Column &column)
{
    return calculateStat<Maximum>(column);
}

std::shared_ptr<arrow::Column> calculateMean(const arrow::Column &column)
{
    return calculateStat<Mean>(column);
}

std::shared_ptr<arrow::Column> calculateMedian(const arrow::Column &column)
{
    return calculateQuantile(column, 0.5, "median");
}

std::shared_ptr<arrow::Column> calculateQuantile(const arrow::Column &column, double q)
{
    return calculateQuantile(column, q, "quantile " + std::to_string(q));
}

std::shared_ptr<arrow::Array> fromMemory(double *data, int32_t dataCount)
{
    arrow::DoubleBuilder builder;
    builder.AppendValues(data, dataCount);
    return finish(builder);
}

std::shared_ptr<arrow::Column> columnFromArray(std::shared_ptr<arrow::Array> array, std::string name)
{
    auto field = arrow::field(name, array->type(), array->null_count());
    return std::make_shared<arrow::Column>(field, array);
}

std::shared_ptr<arrow::Column> calculateVariance(const arrow::Column &column)
{
    return calculateStat<Variance>(column);
}

std::shared_ptr<arrow::Column> calculateRSI(const arrow::Column &column)
{
    return calculateStat<RSI>(column);
}

std::shared_ptr<arrow::Column> calculateStandardDeviation(const arrow::Column &column)
{
    return calculateStat<StdDev>(column);
}

std::shared_ptr<arrow::Column> calculateSum(const arrow::Column &column)
{
    return calculateStat<Sum>(column);
}

double calculateCorrelation(const arrow::Column &xCol, const arrow::Column &yCol)
{
    if(xCol.null_count() >= xCol.length() || yCol.null_count() >= yCol.length())
        return std::numeric_limits<double>::quiet_NaN();

    struct CorrelationStats
    {
        double sumX = 0;
        double sumY = 0;
        double sumXX = 0;
        double sumYY = 0;
        double sumXY = 0;
        int64_t n = 0;

        void addPair(double xVal, double yVal)
        {
            sumX += xVal;
            sumY += yVal;
            sumXX += xVal * xVal;
            sumYY += yVal * yVal;
            sumXY += xVal * yVal;
            n++;
        }

        double correlation() const
        {
            const auto num = n * sumXY - sumX * sumY;
            const auto den = std::sqrt(n * sumXX - (sumX*sumX)) * std::sqrt(n * sumYY - (sumY*sumY));
            return num / den;
        }
    };

    auto stats = visitType(*xCol.type(), [&] (auto id1)
    {
        return visitType(*yCol.type(), [&] (auto id2) -> CorrelationStats
        {
            if constexpr(id1.value != arrow::Type::STRING  &&  id2.value != arrow::Type::STRING)
            {
                CorrelationStats stats;
                iterateOverJustPairs<id1.value, id2.value>(xCol, yCol,
                    [&] (auto xVal, auto yVal)
                {
                    stats.addPair(toStorage(xVal), toStorage(yVal));
                });
                return stats;
            }
            else
                throw std::runtime_error("Correlation not supported on string types");
        });
    });

    return stats.correlation();
}

std::shared_ptr<arrow::Column> calculateCorrelation(const arrow::Table &table, const arrow::Column &column)
{
    if(table.num_rows() != column.length())
        throw std::runtime_error("cannot calculate correlation: mismatched column/table row counts");

    const auto N = table.num_columns();
    std::vector<double> correlationValues;
    correlationValues.resize(N);

    for(int i = 0; i < N; i++)
    {
        const auto columnI = table.column(i);
        const auto isSelfCompare = &column == columnI.get();
        correlationValues[i] = isSelfCompare
            ? 1.0
            : calculateCorrelation(column, *columnI);
    }

    return toColumn(correlationValues, column.name() + "_CORR");
}

std::shared_ptr<arrow::Table> calculateCorrelationMatrix(const arrow::Table &table)
{
    const auto N = table.num_columns();
    std::vector<std::vector<double>> correlationMatrix;
    correlationMatrix.resize(N);
    for(auto &correlationColumn : correlationMatrix)
        correlationColumn.resize(N);

    for(int i = 0; i < N; i++)
    {
        const auto ci = table.column(i);
        correlationMatrix[i][i] = 1.0;
        for(int j = i + 1; j < N; j++)
        {
            const auto cj = table.column(j);
            const auto correlation = calculateCorrelation(*ci, *cj);
            correlationMatrix[i][j] = correlation;
            correlationMatrix[j][i] = correlation;
        }
    }

    std::vector<std::shared_ptr<arrow::Column>> ret;
    for(int i = 0; i < N; i++)
    {
        auto c = toColumn(correlationMatrix.at(i), table.column(i)->name());
        ret.push_back(c);
    }

    return tableFromColumns(ret);
}

double autoCorrelation(const std::shared_ptr<arrow::Column> &column, int64_t lag /*= 1*/)
{
    auto shiftedColumn = shift(column, lag);
    auto debug = toVector<std::optional<int64_t>>(*shiftedColumn);
    return calculateCorrelation(*column, *shiftedColumn);
}

template<typename T>
struct AggregateBase
{
    virtual ~AggregateBase(){}

    virtual void operator()(T t) = 0;
    virtual void operator()() = 0;
    virtual std::optional<double> get(bool hadValidValue) = 0;
    virtual std::string name() = 0;
};

template<typename T, typename Aggregator>
struct AggregateBy : AggregateBase<T>
{
    Aggregator a;

    virtual void operator()(T t) override
    {
        a(t);
    }
    virtual void operator()() override
    {
        a();
    }
    virtual std::optional<double> get(bool hadValidValue) override
    {
        if(hadValidValue || !Aggregator::RequiredSampleCount)
            return a.get();
        return {};
    }
    virtual std::string name() override
    {
        return Aggregator::name;
    }
};

template<AggregateFunction aggr, typename T>
using AggregatorFor_t = typename AggregatorFor<aggr, T>::type;

template<typename T>
std::unique_ptr<AggregateBase<T>> makeAggregator(AggregateFunction a)
{
    return dispatchAggregateByEnum(a, [] (auto aggrC) -> std::unique_ptr<AggregateBase<T>>
    {
        using Aggregator = AggregatorFor_t<aggrC.value, T>;
        if constexpr(doesAggregatorAllowsType<T>(aggrC.value))
            return std::make_unique<AggregateBy<T, Aggregator>>();
        else
            THROW("wrong type for function id={}", (int)aggrC.value);
    });
}

std::string aggregateName(AggregateFunction a)
{
    // inefficient, but should not be called that often
    return makeAggregator<double>(a)->name();
}

template<typename T>
struct Aggregators
{
    bool hadValidValue = false;
    std::vector<std::unique_ptr<AggregateBase<T>>> aggregators;
    Aggregators(const std::vector<AggregateFunction> &toAggregate)
    {
        aggregators = transformToVector(toAggregate, [](auto aggrEnum) { return makeAggregator<T>(aggrEnum); });
    }
    void operator()(T t)
    {
        hadValidValue = true;
        for(auto &a : aggregators)
            (*a)(t);
    }
    void operator()()
    {
        for(auto &a : aggregators)
            (*a)();
    }
};

// Cannot be just lambda because of GCC-8 bug
// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=86740
template<typename ArrowType, typename T>
struct AbominableGroupingIterator
{
    int64_t row = 0;

    GroupedKeyInfo<ArrowType> &groups;
    std::vector<Aggregators<T>> &aggregators;

    AbominableGroupingIterator(GroupedKeyInfo<ArrowType> &groups, std::vector<Aggregators<T>> &aggregators)
            : groups(groups), aggregators(aggregators)
    {}

    template <typename U>
    void operator()(U value)
    {
        const auto groupId = groups.groupIds[row++];
        aggregators[groupId](value);
    }
    void operator()()
    {
        const auto groupId = groups.groupIds[row++];
        aggregators[groupId]();
    }
};

std::string to_string(AggregateFunction a)
{
    return dispatchAggregateByEnum(a, [] (auto aggrC) { return AggregatorFor_t<aggrC.value, double>::name; });
}

DFH_EXPORT std::shared_ptr<arrow::Table> abominableGroupAggregate(std::shared_ptr<arrow::Column> keyColumn, std::vector<std::pair<std::shared_ptr<arrow::Column>, std::vector<AggregateFunction>>> toAggregate)
{
    std::vector<std::shared_ptr<arrow::Column>> newColumns;

    visitDataType(keyColumn->type(), [&](auto type)
    {
        using ArrowType = ArrowTypeFromPtr<decltype(type)>;
        constexpr auto keyTypeID = idFromDataPointer<decltype(type)>;
        using KeyT = typename TypeDescription<keyTypeID>::ObservedType;
        if constexpr(keyTypeID == arrow::Type::LIST)
        {
            throw std::runtime_error("not implemented: grouping by column of list type");
        }
        else
        {
            GroupedKeyInfo<ArrowType> groups{*keyColumn};

            const auto groupCount = groups.groupCount();
            const auto hasNulls = groups.hasNulls;
            const auto afterLastGroup = groups.uniqueValues.size()+1;

            std::vector<KeyT> keyValues(afterLastGroup);
            for(auto [keyValue, groupId] : groups.uniqueValues)
                keyValues[groupId] = keyValue;

            // build column with unique key values
            {
                auto builder = makeBuilder(type);
                if(hasNulls)
                    builder->AppendNull();
                for(int group = 1; group < keyValues.size(); ++group)
                    append(*builder, keyValues[group]);

                auto arr = finish(*builder);
                newColumns.push_back(std::make_shared<arrow::Column>(keyColumn->field(), arr));
            }

            // build column for each (column, aggregate function) pair
            for(auto &colAggrs : toAggregate)
            {
                visitType(colAggrs.first->type()->id(), [&](auto id)
                {
                    auto [column, aggregates] = colAggrs;
                    using T = typename TypeDescription<id.value>::ObservedType;
                    std::vector<Aggregators<T>> aggregators;
                    aggregators.reserve(afterLastGroup);
                    for(int i = 0; i < afterLastGroup; i++)
                    {
                        try
                        {
                            aggregators.emplace_back(aggregates);
                        }
                        catch(std::exception &e)
                        {
                            THROW("cannot aggregate for column `{}` of type `{}`: {}", column->name(), column->type()->ToString(), e);
                        }
                    }

                    AbominableGroupingIterator<ArrowType, T> iterator{groups, aggregators};
                    iterateOver<id.value>(*column, iterator, iterator);

                    std::vector<arrow::DoubleBuilder> newColumnBuilders(aggregates.size());
                    for(auto &&newColumnBuilder : newColumnBuilders)
                        newColumnBuilder.Reserve(groupCount);

                    for(int64_t groupItr = !hasNulls; groupItr < afterLastGroup; ++groupItr)
                    {
                        auto &aggr = aggregators[groupItr];
                        for(int32_t i = 0; i < aggregates.size(); i++)
                        {
                            if(auto result = aggr.aggregators[i]->get(aggr.hadValidValue))
                                newColumnBuilders[i].Append(*result);
                            else
                                newColumnBuilders[i].AppendNull();
                        }
                    }
                    for(int32_t i = 0; i < aggregates.size(); i++)
                    {
                        auto arr = finish(newColumnBuilders[i]);
                        auto col = toColumn(arr, column->name() + "_"s + aggregateName(aggregates[i]));
                        newColumns.push_back(col);
                    };
                });
            }
        }
    });

    return tableFromColumns(newColumns);
}

template<class TD>
using IntervalType = typename TD::IntervalType;

template<arrow::Type::type id, typename Indexable, typename Index>
std::optional<typename TypeDescription<id>::ObservedType> getMaybeValue(const Indexable &indexable, Index index)
{
    if constexpr(std::is_same_v<Indexable, arrow::Column>)
    {
        return getMaybeValue<id>(*indexable.data(), index);
    }
    else if constexpr(std::is_same_v<Indexable, arrow::ChunkedArray>)
    {
        auto [chunk, chunkIndex] = locateChunk(indexable, index);
        return getMaybeValue<id>(*chunk, index);
    }
    else if constexpr(std::is_same_v<Indexable, arrow::Array>)
    {
        if(indexable.IsValid(index))
            return arrayValueAt<id>(indexable, index);
        else
            return std::nullopt;
    }
    else
        static_assert(always_false_v<Indexable>);
}

template<arrow::Type::type id, typename Indexable>
auto getJustValue(const Indexable &indexable, int64_t index)
{
    if constexpr(std::is_same_v<Indexable, arrow::Column>)
        return columnValueAt<id>(indexable, index);
    else if constexpr(std::is_same_v<Indexable, arrow::Array>)
        return arrayValueAt<id>(indexable, index);
    else
        static_assert(always_false_v<Indexable>);
}

template<typename Indexable>
std::vector<int64_t> collectRollingWindowPositionsT(const Indexable &indexable, DynamicField interval)
{
    const auto N = indexable.length();
    std::vector<int64_t> ret(N);

    visitType4(indexable.type(), [&](auto id)
    {
        if constexpr(is_detected_v<IntervalType, TypeDescription<id.value>>)
        {
            using TD = TypeDescription<id.value>;
            using Array = typename TD::Array;
            using IntervalType = IntervalType<TD>;

            if(!holds_alternative<IntervalType>(interval))
                THROW("wrong interval type: `index {}`, expected: `{}`", interval.index(), typeid(IntervalType));

            const auto intervalT = get<IntervalType>(interval);
            int64_t left = 0;

            for(int64_t right = 0; right < N; ++right)
            {
                const auto rightValue = getJustValue<id.value>(indexable, right);
                const auto leftValue = rightValue - intervalT;

                while(left < right && getJustValue<id.value>(indexable, left) <= leftValue)
                    left++;

                ret[right] = 1 + right - left;
            }
        }
        else
            throw std::runtime_error("interval windows not supported for type " + indexable.type()->ToString());
    });

    return ret;
}

// because of GCC-8 bug this cannot be lambda
// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=86740
template <typename Indexable, arrow::Type::type id>
struct FunctionOverWindowCalculator
{
    const Indexable &indexable;
    int64_t startIndex;
    int64_t windowsWidth;

    template <AggregateFunction f>
    std::optional<double> operator()(std::integral_constant<AggregateFunction, f> aggrC) const
    {
        using T = typename TypeDescription<id>::ValueType;
        using Aggregator = AggregatorFor_t<aggrC.value, T>;
        Aggregator aggregator;

        auto validCount = 0;
        for(int64_t row = startIndex - windowsWidth + 1; row <= startIndex; ++row)
        {
            const auto value = getMaybeValue<id>(indexable, row);
            if(value)
            {
                ++validCount;
                aggregator(*value);
            }
            else
                aggregator();
        }
        if(validCount >= Aggregator::RequiredSampleCount)
            return aggregator.get();
        else
            return std::nullopt;
    }
};

template<typename Indexable>
std::optional<double> calculateFunctionOnWindow(const Indexable &indexable, int64_t startIndex, int64_t windowsWidth, AggregateFunction f)
{
    return visitType4(indexable.type(), [&] (auto id) -> std::optional<double>
    {
        if constexpr(id.value == arrow::Type::INT64 || id.value == arrow::Type::DOUBLE)
        {
            FunctionOverWindowCalculator<Indexable, id.value> calculator{indexable, startIndex, windowsWidth};
            return dispatchAggregateByEnum(f, calculator);
        }
        else
            throw std::runtime_error("rolling statistics not supported for type " + indexable.type()->ToString());
    });
}

std::vector<int64_t> collectRollingIntervalSizes(std::shared_ptr<arrow::Column> keyColumn, DynamicField interval)
{
    try
    {
        return dispatchIndexable(keyColumn, [&] (auto &&indexable)
        {
            return collectRollingWindowPositionsT(*indexable, interval);
        });
    }
    catch(std::exception &e)
    {
        THROW("failed to collect window widths for column `{}` of type `{}`: {}", keyColumn->name(), keyColumn->type()->ToString(), e);
    }
}

void requireSameSize(const arrow::Column &lhs, const arrow::Column &rhs)
{
    auto lhsN = lhs.length();
    auto rhsN = rhs.length();

    if(lhsN != rhsN)
        THROW("Column length mismatch: `{}` has {} rows, `{}` has {} rows", lhs.name(), lhsN, rhs.name(), rhsN);
}

std::shared_ptr<arrow::Table> rollingInterval(std::shared_ptr<arrow::Column> keyColumn, DynamicField interval, std::vector<std::pair<std::shared_ptr<arrow::Column>, std::vector<AggregateFunction>>> toAggregate)
{
    std::vector<std::shared_ptr<arrow::Column>> newColumns{ keyColumn };

    const auto N = keyColumn->length();
    const auto windowWidths = collectRollingIntervalSizes(keyColumn, interval);
    for(auto && [col, funcs] : toAggregate)
    {
        requireSameSize(*keyColumn, *col);
        dispatchIndexable(col, [&, &col=col, &funcs=funcs](auto &&indexable)
        {
            const auto &name = col->name();
            for(auto fun : funcs)
            {
                try
                {
                    arrow::DoubleBuilder builder;
                    builder.Reserve(N);

                    for(int64_t row = 0; row < N; ++row)
                    {
                        auto value = calculateFunctionOnWindow(*indexable, row, windowWidths[row], fun);
                        append(builder, value);
                    }

                    auto arr = finish(builder);
                    newColumns.push_back(toColumn(arr, name + "_" + to_string(fun)));  // TODO pretty name
                }
                catch(std::exception &e)
                {
                    THROW("failed to calculate `{}` on column `{}`: {}", to_string(fun), name, e);
                }
            }
        });
    }

    return tableFromColumns(newColumns);
}


#include <blitzml/dataset/sparse_binary_dataset.h>

#include <blitzml/base/math_util.h>
#include <iostream>
using std::vector;

namespace BlitzML {

SparseBinaryColumn::SparseBinaryColumn(const index_t *indices,
                                       index_t nnz, index_t length)
    : Column(length, nnz), indices(indices) { }

value_t SparseBinaryColumn::inner_product(const vector<value_t> &vec) const {
  value_t result = 0.;
  const index_t* i;
  const index_t* end = indices + nnz();
  for (i = indices; i != end; ++i) {
    result += vec[*i];
  }
  return result;
}

value_t SparseBinaryColumn::weighted_inner_product(
    const vector<value_t> &vec,
    const vector<value_t> &weights) const {
  value_t result = 0.;
  const index_t* i;
  const index_t* end = indices + nnz();
  for (i = indices; i != end; ++i) {
    result += vec[*i] * weights[*i];
  }
  return result;
}

value_t SparseBinaryColumn::weighted_norm_sq(
    const vector<value_t> &weights) const {
  return inner_product(weights);
}

void SparseBinaryColumn::add_multiple(vector<value_t> &target,
                                        value_t scalar) const {
  const index_t* i;
  const index_t* end = indices + nnz();
  for (i = indices; i != end; ++i) {
    target[*i] += scalar;
  }
}

void SparseBinaryColumn::weighted_add_multiple(
    vector<value_t> &target, vector<value_t> &weights, value_t scalar) const {
  for (const index_t* i = indices; i != indices + nnz(); ++i) {
    target[*i] += scalar * weights[*i];
  }
}

value_t SparseBinaryColumn::sum() const {
  return static_cast<value_t>(nnz());
}

value_t SparseBinaryColumn::mean() const {
  return static_cast<value_t>(nnz()) / length();
}

value_t SparseBinaryColumn::l2_norm_sq() const {
  return static_cast<value_t>(nnz());
}

SparseBinaryDataset::SparseBinaryDataset(
    const index_t *indices, const size_t *indptr,
    index_t height, index_t width, size_t nnz,
    const value_t *b, index_t length_b)
    : Dataset(height, width, nnz, b, length_b) {
  A_cols.clear();
  A_cols.reserve(width);
  for (index_t j = 0; j < width; ++j) {
    size_t offset = indptr[j];
    index_t col_nnz = static_cast<index_t>(indptr[j + 1] - offset);
    const index_t *col_indices = indices + offset;
    SparseBinaryColumn *col =
              new SparseBinaryColumn(col_indices, col_nnz, height);
    A_cols.push_back(col);
  }
}

SparseBinaryDataset::~SparseBinaryDataset() {
  for (size_t ind = 0; ind < A_cols.size(); ++ind) {
    delete A_cols[ind];
  }
}

bool SparseBinaryDataset::any_columns_overlapping_in_submatrix(
    index_t first_column_submatrix, index_t end_column_submatrix) const {
  vector<bool> seen_row(height, false);
  for (index_t j = first_column_submatrix; j < end_column_submatrix; ++j) {
    const SparseBinaryColumn* col = A_cols[j];
    for (const index_t* i = col->indices_begin();
         i != col->indices_end(); ++i) {
      if (seen_row[*i]) {
        return true;
      } else {
        seen_row[*i] = true;
      }
    }
  }
  return false;
}

}


/******************************************************************************
 *
 *  filename: Material.h
 *  author  : Do Won Cha
 *
 *****************************************************************************/

#pragma once

#ifndef _RAY_MATERIAL_
#define _RAY_MATERIAL_

#include <Eigen/Core>

using namespace Eigen;

namespace raytracer
{

class Material
{
public:
  Material(Vector4f ambient,
           Vector4f diffuse,
           Vector4f specular,
           float specular_power = 0.0f,
           float reflection = 0.0f) :
    ambient_(ambient),
    diffuse_(diffuse),
    specular_(specular),
    specular_power_(specular_power),
    reflectivity_(reflection)
  { }

  // Ambient getter and setter
  Vector4f ambient() const { return ambient_; }
  void set_ambient(Vector4f ambient)   { ambient_ = ambient; }

  // Diffuse getter and setter
  Vector4f diffuse() const { return diffuse_; }
  void set_diffuse(Vector4f diffuse) { diffuse_ = diffuse; }

  // Specular getter and setter
  Vector4f specular() const { return specular_; }
  void set_specular(Vector4f specular) { specular_ = specular; }

  // Specular power getter and setters
  float specular_power() const { return specular_power_; }
  void  set_specular_power(float specular_power) { specular_power_ = specular_power; }

  // Reflection getter and setters
  float reflectivity() const { return reflectivity_; }
  void  set_reflectivity(float reflectivity) { reflectivity_ = reflectivity; }

  std::string name() { return name_; }
  void set_name(std::string name) { name_ = name; }
private:
  std::string name_;
  Vector4f ambient_;
  Vector4f diffuse_;
  Vector4f specular_;
  float specular_power_, reflectivity_;
};

} // end of namespace raytracer

#endif    // RAY_MATERIAL Header guard


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/extensions/nsphere/nsphere.hpp>
#include <boost/geometry/multi/core/point_type.hpp>

template <typename G, typename Expected>
void test_geometry()
{
    BOOST_CHECK_EQUAL(typeid(typename bg::point_type<G>::type).name(),
        typeid(Expected).name());
}

template <typename P>
void test_all()
{
    test_geometry<bg::model::nsphere<P, double> , P>();
}

int test_main(int, char* [])
{
    test_all<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();

    return 0;
}