/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, INRIA
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Justin Carpentier */

#define BOOST_TEST_MODULE FCL_HEIGHT_FIELDS
#include <boost/test/included/unit_test.hpp>
#include <boost/filesystem.hpp>

#include "fcl_resources/config.h"

#include <hpp/fcl/collision.h>
#include <hpp/fcl/hfield.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/mesh_loader/assimp.h>
#include <hpp/fcl/mesh_loader/loader.h>

#include <hpp/fcl/collision.h>

#include "utility.h"
#include <iostream>

using namespace hpp::fcl;

template <typename BV>
void test_constant_hfields(const Eigen::DenseIndex nx,
                           const Eigen::DenseIndex ny,
                           const FCL_REAL min_altitude,
                           const FCL_REAL max_altitude) {
  const FCL_REAL x_dim = 1., y_dim = 2.;
  const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);

  HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude);

  BOOST_CHECK(hfield.getXDim() == x_dim);
  BOOST_CHECK(hfield.getYDim() == y_dim);

  const VecXf& x_grid = hfield.getXGrid();
  BOOST_CHECK(x_grid[0] == -x_dim / 2.);
  BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.);

  const VecXf& y_grid = hfield.getYGrid();
  BOOST_CHECK(y_grid[0] == y_dim / 2.);
  BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.);

  // Test local AABB
  hfield.computeLocalAABB();

  for (Eigen::DenseIndex i = 0; i < nx; ++i) {
    for (Eigen::DenseIndex j = 0; j < ny; ++j) {
      Vec3f point(x_grid[i], y_grid[j], heights(j, i));
      BOOST_CHECK(hfield.aabb_local.contain(point));
    }
  }

  // Test clone
  {
    HeightField<BV>* hfield_clone = hfield.clone();
    hfield_clone->computeLocalAABB();
    BOOST_CHECK(*hfield_clone == hfield);

    delete hfield_clone;
  }

  // Build equivalent object
  const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude);
  const Transform3f box_placement(
      Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.));

  // Test collision
  const Sphere sphere(1.);
  static const Transform3f IdTransform;

  const Box box(Vec3f::Ones());

  Transform3f M_sphere, M_box;

  // No collision case
  {
    const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude);
    M_sphere.setTranslation(
        Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
    M_box.setTranslation(
        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
    CollisionRequest request;

    CollisionResult result;
    collide(&hfield, IdTransform, &sphere, M_sphere, request, result);

    BOOST_CHECK(!result.isCollision());

    CollisionResult result_check_sphere;
    collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere,
            request, result_check_sphere);

    BOOST_CHECK(!result_check_sphere.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request,
            result_check_box);

    BOOST_CHECK(!result_check_box.isCollision());
  }

  // Collision case
  {
    const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude);
    M_sphere.setTranslation(
        Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
    M_box.setTranslation(
        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
    CollisionRequest
        request;  //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));

    CollisionResult result;
    collide(&hfield, IdTransform, &sphere, M_sphere, request, result);

    BOOST_CHECK(result.isCollision());

    CollisionResult result_check;
    collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere,
            request, result_check);

    BOOST_CHECK(result_check.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request,
            result_check_box);

    BOOST_CHECK(result_check_box.isCollision());
  }

  // Update height
  hfield.updateHeights(
      MatrixXf::Constant(ny, nx, max_altitude / 2.));  // We change nothing

  // No collision case
  {
    const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude);
    M_sphere.setTranslation(
        Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision));
    M_box.setTranslation(
        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision));
    CollisionRequest request;

    CollisionResult result;
    collide(&hfield, IdTransform, &sphere, M_sphere, request, result);

    BOOST_CHECK(!result.isCollision());

    CollisionResult result_check_sphere;
    collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere,
            request, result_check_sphere);

    BOOST_CHECK(!result_check_sphere.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request,
            result_check_box);

    BOOST_CHECK(!result_check_box.isCollision());
  }

  // Collision case
  {
    const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude);
    M_sphere.setTranslation(
        Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
    M_box.setTranslation(
        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
    CollisionRequest
        request;  //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));

    CollisionResult result;
    collide(&hfield, IdTransform, &sphere, M_sphere, request, result);

    BOOST_CHECK(!result.isCollision());

    CollisionResult result_check;
    collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere,
            request, result_check);

    BOOST_CHECK(result_check.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request,
            result_check_box);

    BOOST_CHECK(result_check_box.isCollision());
  }

  // Restore height
  hfield.updateHeights(
      MatrixXf::Constant(ny, nx, max_altitude));  // We change nothing

  // Collision case
  {
    const FCL_REAL eps_collision = -0.1 * (max_altitude - min_altitude);
    M_sphere.setTranslation(
        Vec3f(0., 0., max_altitude + sphere.radius + eps_collision));
    M_box.setTranslation(
        Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_collision));
    CollisionRequest
        request;  //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));

    CollisionResult result;
    collide(&hfield, IdTransform, &sphere, M_sphere, request, result);

    BOOST_CHECK(result.isCollision());

    CollisionResult result_check;
    collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere,
            request, result_check);

    BOOST_CHECK(result_check.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request,
            result_check_box);

    BOOST_CHECK(result_check_box.isCollision());
  }
}

BOOST_AUTO_TEST_CASE(building_constant_hfields) {
  const FCL_REAL max_altitude = 1., min_altitude = 0.;

  test_constant_hfields<OBBRSS>(2, 2, min_altitude,
                                max_altitude);  // Simple case
  test_constant_hfields<OBBRSS>(20, 2, min_altitude, max_altitude);
  test_constant_hfields<OBBRSS>(100, 100, min_altitude, max_altitude);
  //  test_constant_hfields<OBBRSS>(1000,1000,min_altitude,max_altitude);

  test_constant_hfields<AABB>(2, 2, min_altitude, max_altitude);  // Simple case
  test_constant_hfields<AABB>(20, 2, min_altitude, max_altitude);
  test_constant_hfields<AABB>(100, 100, min_altitude, max_altitude);
}

BOOST_AUTO_TEST_CASE(hfield_with_square_hole) {
  const Eigen::DenseIndex nx = 100, ny = 100;

  typedef AABB BV;
  const MatrixXf X =
      Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1);
  const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx);

  const FCL_REAL dim_square = 0.5;

  const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole =
      (X.array().abs() < dim_square) && (Y.array().abs() < dim_square);

  const MatrixXf heights =
      MatrixXf::Ones(ny, nx) - hole.cast<double>().matrix();

  const HeightField<BV> hfield(2., 2., heights, -10.);

  Sphere sphere(0.48);
  const Transform3f sphere_pos(Vec3f(0., 0., 0.5));
  const Transform3f hfield_pos;

  const CollisionRequest request;

  CollisionResult result;
  {
    collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result);

    BOOST_CHECK(!result.isCollision());
  }

  sphere.radius = 0.51;

  {
    CollisionResult result;
    const Sphere sphere2(0.51);
    collide(&hfield, hfield_pos, &sphere2, sphere_pos, request, result);

    BOOST_CHECK(result.isCollision());
  }
}


#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <iterator>
#include <map>
#include <set>
#include <sstream>
#include <string>
#include <vector>

#include <Eigen/Core>

#include "igl/AABB.h"
#include "igl/adjacency_list.h"
#include "igl/adjacency_matrix.h"
#include "igl/ambient_occlusion.h"
#include "igl/components.h"
#include "igl/cotmatrix.h"
#include "igl/decimate.h"
#include "igl/dijkstra.h"
#include "igl/embree/ambient_occlusion.h"
#include "igl/embree/EmbreeIntersector.h"
#include "igl/embree/unproject_onto_mesh.h"
#include "igl/gaussian_curvature.h"
#include "igl/invert_diag.h"
#include "igl/jet.h"
#include "igl/per_vertex_attribute_smoothing.h"
#include "igl/per_vertex_normals.h"
#include "igl/polygon_mesh_to_triangle_mesh.h"
#include "igl/principal_curvature.h"
#include "igl/ray_mesh_intersect.h"
#include "igl/readOFF.h"
#include "igl/unproject_onto_mesh.h"
#include "igl/viewer/Viewer.h"
#include "igl/writeOFF.h"

#include "GLFW/glfw3.h"

#include "dijkstra.hxx"

const double mOCC_MAX_RAY_DIST = 10.0;
const int mOCC_NUM_RAYS = 20;
const double mDECIMATE_PERC = 0.6;

enum SelectionType { CURVATURE, DARKNESS };

// Split a string into words.
std::vector<std::string> split(const std::string &input) {
  using namespace std;
  istringstream iss(input);
  vector<string> tokens{istream_iterator<string>{iss},
                        istream_iterator<string>{}};
  return tokens;
}

std::string prettyprint(const std::string &input, int width, int trailing_spaces) {
  using namespace std;
  vector<string> words = split(input);

  string ret;
  int cur_width = 0;
  for (const string &s : words) {
    if (cur_width + s.size() > width) {
      ret += "\n";
      for (int i = 0; i < trailing_spaces; ++i) {
        ret += " ";
      }
      cur_width = 0;
    }
    ret += s + " ";
    cur_width += s.size() + 1;
  }

  return ret;
}

void buildChoiceSide(std::vector<std::string> &prompts,
                     std::vector<SelectionType> &types,
                     std::string side) {
  using namespace std;
  prompts.push_back(string("Insert points along the squamous suture [patient ")
                    + side + "]");
  types.push_back(DARKNESS);
  types.push_back(CURVATURE);
  prompts.push_back(string("Insert points from the squamous x lambdoid sutures "
                           "to the tip of the mastoid process [patient ")
                    + side + "]");
  types.push_back(CURVATURE);
  prompts.push_back(string("Insert points from the tip of the mastoid process "
                           "along the zygomatic bone to the top of the skull "
                           "[patient ")
                    + side + "]");
}

void buildChoiceTop(std::vector<std::string> &prompts,
                    std::vector<SelectionType> &types) {
  using namespace std;

  prompts.push_back("Insert points along the coronal suture, "
                    "starting at the sphenoid bone on one side, "
                    "and ending at the sphenoid bone on the other side.");
  types.push_back(DARKNESS);
  prompts.push_back("Insert points along the sagittal suture.");
  types.push_back(DARKNESS);
  prompts.push_back("Insert points along the lambdoid suture");
  types.push_back(DARKNESS);
}

void buildChoiceFront(std::vector<std::string> &prompts,
                      std::vector<SelectionType> &types) {
  prompts.push_back("Insert points along the chin, starting at the jaw attachment "
                    "point on one side, ending on the other side");
  types.push_back(CURVATURE);
  prompts.push_back("Insert points along the outside of the left eye");
  types.push_back(CURVATURE);
  prompts.push_back("Insert points along the outside of the right eye");
  types.push_back(CURVATURE);
  prompts.push_back("Insert points along the outside of the nose");
  types.push_back(CURVATURE);
}

void buildChoices(std::vector<std::string> &prompts,
                  std::vector<SelectionType> &types) {
  buildChoiceTop(prompts, types);
  buildChoiceSide(prompts, types, "left");
  buildChoiceSide(prompts, types, "right");
  buildChoiceFront(prompts, types);
}

void printChoice(int ptnum, const std::vector<std::string> &prompts) {
  if (ptnum >= prompts.size()) {
    printf("Press 'w' to write points to an output file\n");
  } else {
    std::string msg = prettyprint(prompts[ptnum], 74, 8);
    printf("%2d/%2d -- %s\n", ptnum + 1, prompts.size(), msg.c_str());
  }
}

// Select all points between a and b, given mesh V,F with edge weights defined by w.
// Will output indices to new_points.
void selectPointsBetween(int a, int b,
                         const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
                         const std::vector<std::vector<int> > &VV,
                         const std::vector<std::vector<double> > &w,
                         std::vector<int> &new_points) {
  // Call the Dijkstra's code.
  Eigen::VectorXd min_distance;
  Eigen::VectorXi previous;
  std::set<int> targets;
  targets.insert(b);
  int vertex_found =
      from_igl::dijkstra_compute_paths(a, targets, VV, w, min_distance,previous);
      //igl::dijkstra_compute_paths(a, targets, VV, min_distance,previous);
  if (vertex_found == -1) {
    // ERROR
    printf("ERROR: No path found!!\n");
  } else {
    igl::dijkstra_get_shortest_path_to(vertex_found, previous, new_points);
  }
}

void getMeanCurvature(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
                      Eigen::VectorXd &VCs) {
  Eigen::MatrixXd PD1,PD2;
  Eigen::VectorXd PV1,PV2;
  //igl::principal_curvature(V,F,PD1,PD2,PV1,PV2, 5);
  igl::principal_curvature(V,F,PD1,PD2,PV1,PV2, 8);
  //mean_curve = 0.5 * (PV1 + PV2);
  VCs = PV1.cwiseProduct(PV1) + PV2.cwiseProduct(PV2);
  VCs = VCs.cwiseMin(0.1);
  VCs /= VCs.maxCoeff();

  Eigen::VectorXd ones = Eigen::VectorXd::Constant(VCs.rows(), 1, 1);
  VCs = ones - VCs;

  /*
  // Get the laplacian
  Eigen::SparseMatrix<double> L,M,Minv;
  //   from the cotangent matrix
  igl::cotmatrix(V,F, L);
  //   and the mass matrix
  igl::massmatrix(V,F, igl::MASSMATRIX_TYPE_VORONOI, M);
  igl::invert_diag(M, Minv);

  Eigen::MatrixXd HN = -Minv*(L * V);
  // Mean curviture is this value.
  Eigen::VectorXd VC = HN.rowwise().norm();

  // Smooth this out a bit.
  igl::per_vertex_attribute_smoothing(VC,F, VCs);
  // Cutoff at 1 (to avoid overpowering by high curvature)
  VCs = VCs.cwiseMin(1.0);

  // Normalize
  VCs /= VCs.maxCoeff();
  // Invert so high curvature has low weight.
  Eigen::VectorXd ones = Eigen::VectorXd::Constant(VCs.rows(), 1, 1);
  // Sqrt before switching
  VCs = VCs.cwiseSqrt();
  VCs = ones - VCs;
  //std::cout << VCs;
  */

}

void printHelp(int ptnum) {
  printf("Press 'u' to undo previous selection\n");
  printf("Press '[' or ']' to adjust scene lighting\n");
  printf("Press 'c' to show colors for curvature\n");
  printf("Press 'd' to switch back to current selected points\n");
  printf("Press ' ' (space) to complete the current task\n");
  printf("Press '?' to repeat this message (and the requested user input).\n");
}

int main(int argc, char* argv[]) {
  if (argc < 3) {
    fprintf(stderr, "Used for selecting points on the surface of a mesh. Will write the\n"
                    "corresponding vertices to output.off\n");
    fprintf(stderr, "usage: %s <input.off> <output.off> [decimate perc]\n",
            argv[0]);
    return -1;
  }

  Eigen::MatrixXd V,V2;
  Eigen::MatrixXi F,F2;
  igl::embree::EmbreeIntersector ei;

  printf("Reading in mesh...\n");
  // Read in the input file.
  igl::readOFF(argv[1], V, F);

  if (argc == 4) {
    float dec_perc = atof(argv[3]);
    int new_faces = dec_perc * F.rows();
    printf("Decimating by %f to %d faces...\n", dec_perc, new_faces);
    // First things first: decimate the mesh
    Eigen::VectorXi J;
    igl::decimate(V,F, new_faces, V2,F2,J);
    printf("Decimated from V:%d,F:%d to V:%d,F:%d\n",
           V.rows(), F.rows(), V2.rows(), F2.rows());
    V = V2; F = F2;
  }

  // Colors are all white to start.
  Eigen::MatrixXd VC = Eigen::MatrixXd::Constant(V.rows(),3,1);
  // Initialize this thing so we can get the vertices.
  ei.init(V.cast<float>(), F);

  // Ambient occlusion
  Eigen::VectorXd AO;
  Eigen::MatrixXd N;
  igl::per_vertex_normals(V,F,N);
  // Compute ambient occlusion factor using embree
  printf("Building AABB tree...\n");
  igl::AABB<Eigen::MatrixXd, 3> aabb;
  aabb.init(V, F);
  const auto & shoot_ray = [&aabb,&V,&F](
      const Eigen::Vector3f& _s,
      const Eigen::Vector3f& dir)->bool
  {
    Eigen::Vector3f s = _s+1e-4*dir;
    igl::Hit hit;
    if (aabb.intersect_ray(V,F, s.cast<double>().eval(),dir.cast<double>().eval(), hit)) {
      return hit.t < mOCC_MAX_RAY_DIST;
    }
    return false;
  };

  printf("Now setting ambient occlusion vertices to improve contrast (patience)...\n");
  //igl::embree::ambient_occlusion(V,F,V,N,300,AO);
  igl::ambient_occlusion(shoot_ray, V,N, mOCC_NUM_RAYS,AO);
  AO = 1.0 - AO.array();
  for (int i = 0; i < V.rows(); ++i) {
    VC.row(i) *= AO(i);
  }

  printf("Constructing adjacency list...\n");
  // Need the adjacency list
  std::vector<std::vector<int> > VV;
  igl::adjacency_list(F, VV);

  std::vector<std::vector<double> > darkWeight;
  darkWeight.resize(VV.size());
  //std::vector<std::vector<double> > curveWeight;
  //curveWeight.resize(VV.size());
  for (int i = 0; i < VV.size(); ++i) {
    for (int j = 0; j < VV[i].size(); ++j) {
      // Just use the value of the ambient occlusion.
      darkWeight[i].push_back(AO(VV[i][j]));
      //curveWeight[i].push_back(1.0 - AO(VV[i][j]));
    }
  }

  // Also caluculate the curvature values.
  printf("Calculating mean curvature...\n");
  Eigen::VectorXd mean_curve;
  getMeanCurvature(V,F, mean_curve);
  std::vector<std::vector<double> > curveWeight;
  curveWeight.resize(VV.size());
  double maxCurve = 0;
  double minCurve = 0;
  for (int i = 0; i < VV.size(); ++i) {
    for (int j = 0; j < VV[i].size(); ++j) {
      // Difference in gausssian curvature
      //double diff = std::abs(VCs(i) - VCs(j));
      double diff = mean_curve(VV[i][j]);
      curveWeight[i].push_back(diff);
      maxCurve = std::max(diff, maxCurve);
      minCurve = std::min(diff, minCurve);
    }
  }

  // Create the colors for mean curviture to use
  Eigen::MatrixXd curve_colors;
  igl::jet(mean_curve, true, curve_colors);

  // Prompts and types
  std::vector<std::string> prompts;
  std::vector<SelectionType> types;
  buildChoices(prompts, types);

  int point_num = 0;
  std::vector<int> all_points;  // will contain all points, once the entire mesh has been selected.

  std::vector<int> selected_points;
  std::vector<int> additional_points;

  igl::viewer::Viewer viewer;
  viewer.callback_key_down = [&](igl::viewer::Viewer& viewer, unsigned char key, int modifier)->bool {
    printf("key: '%c' modifier: '%d'\n", key, modifier);
    switch(key) {
      case ']':
        viewer.core.lighting_factor += 0.1;
        printf("Turning lighting up to %f\n", viewer.core.lighting_factor);
        break;
      case '[':
        viewer.core.lighting_factor -= 0.1;
        printf("Turning lighting down to %f\n", viewer.core.lighting_factor);
        break;
      case 'U':
      {
        printf("Undoing last point...\n");
        // Undo the last choice
        int last = selected_points.back();
        printf("Undoing last choice... %d\n", last);
        selected_points.pop_back();
        VC.row(last) << 1,1,1;
        VC.row(last) *= AO(last);
        // Also remove points from additional_points
        int this_p = -1;
        while ( this_p != last && additional_points.size() > 0 ) {
          // Get the last point.
          this_p = additional_points.back();
          // Set the color
          VC.row(this_p) << 1,1,1;
          VC.row(this_p) *= AO(this_p);
          // Remove the point.
          additional_points.pop_back();
        }

        // Need to add in any colors we might have removed
        for (int i : additional_points) {
          VC.row(i) << 1,0,0;
        }
        for (int i : selected_points) {
          VC.row(i) << 1,0,0;
        }

        // Set the colors back to what they were.
        viewer.data.set_colors(VC);
        printChoice(point_num, prompts);
        break;
      }
      case ' ':
      {
        printf("Finished with current curve...Moving on to next\n");
        // Add all the points to all_points
        all_points.insert(all_points.end(),
                          additional_points.begin(),
                          additional_points.end());
        additional_points.clear();
        selected_points.clear();

        point_num++;
        printChoice(point_num, prompts);
        break;
      }
      case 'C':
        printf("Setting curvature colors...\n");
        viewer.data.set_colors(curve_colors);
        break;
      case 'D':
        printf("Setting colors to highlight darkness...\n");
        viewer.data.set_colors(VC);
        break;
      case 'W':
      {
        printf("Writing output to %s\n", argv[2]);
        // Write to output file
        FILE* ofs = fopen(argv[2], "w");
        fprintf(ofs, "OFF\n");
        fprintf(ofs, "%d 0 0\n", all_points.size());
        for (int idx : all_points) {
          fprintf(ofs, "%f %f %f\n", V(idx, 0), V(idx, 1), V(idx, 2));
        }
        fclose(ofs);
        // You can quit now.
        printf("Finished! Please close the program.\n");
        break;
      }
      default:
        // Question mark ('?')
        if (key == '/' && modifier == GLFW_MOD_SHIFT) {
          printHelp(selected_points.size());
          printChoice(point_num, prompts);
        }
    }

    // Make sure this stays the same.
    viewer.core.lighting_factor =
        std::min(std::max(viewer.core.lighting_factor,0.f),1.f);
    return false;
  };
  viewer.callback_mouse_down =
      [&](igl::viewer::Viewer &viewer, int, int)->bool
      {
        // fid will be the face ID. Need to get the vertex ID
        int fid, vid;
        // Cast a ray in the view direction starting from the mouse position
        double x = viewer.current_mouse_x;
        double y = viewer.core.viewport(3) - viewer.current_mouse_y;
        if(igl::embree::unproject_onto_mesh(Eigen::Vector2f(x,y), F,
                                            viewer.core.view * viewer.core.model,
                                            viewer.core.proj,
                                            viewer.core.viewport,
                                            ei,
                                            fid, vid)) {

          selected_points.push_back(vid);
          VC.row(vid) << 1,0,0;

          // Update all the colors
          //Eigen::MatrixXd vcol;
          //igl::jet(VC, false, vcol);
          if (selected_points.size() > 1) {
            std::vector<int> path;
            if (types[point_num] == DARKNESS) {
              printf("Using light shading...\n");
              selectPointsBetween(selected_points[selected_points.size() - 2],
                                  selected_points[selected_points.size() - 1],
                                  V,F, VV, darkWeight,
                                  path);
            } else {
              printf("Using curvature...\n");
              selectPointsBetween(selected_points[selected_points.size() - 2],
                                  selected_points[selected_points.size() - 1],
                                  V,F, VV, curveWeight,
                                  path);
            }
            printf("Setting colors of %d...\n", path.size());
            for (int i = 0; i < path.size(); ++i) {
              VC.row(path[i]) << 1,0,0;
            }
            additional_points.insert(additional_points.end(), path.begin(), path.end());
          }

          viewer.data.set_colors(VC);

          return true;
        }
        return false;
      };

  printHelp(0);
  viewer.data.set_mesh(V, F);
  viewer.data.set_colors(curve_colors);
  viewer.core.show_lines = false;
  viewer.launch();
}


#include "MeshProcessingConnections_EEK.h"
#include "MeshProcessingConnections.h"

#include <CGAL/Polyhedron_3.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/boost/graph/Face_filtered_graph.h>
#include <boost/property_map/property_map.hpp>
#include <boost/iterator/function_output_iterator.hpp>

void* MeshProcessingConnections_EEK_Create()
{
	return MeshProcessingConnections<EEK>::NewMeshProcessingConnections();
}

void MeshProcessingConnections_EEK_Release(void* ptr)
{
	MeshProcessingConnections<EEK>::DeleteMeshProcessingConnections(ptr);
}

//Polyhedron

int MeshProcessingConnections_EEK_ConnectedComponents_PH(void* meshPtr)
{
	return MeshProcessingConnections<EEK>::ConnectedComponents_PH(meshPtr);
}

int MeshProcessingConnections_EEK_ConnectedComponent_PH(void* ptr, void* meshPtr, int index)
{
	return MeshProcessingConnections<EEK>::ConnectedComponent_PH(ptr, meshPtr, index);
}

void MeshProcessingConnections_EEK_GetConnectedComponentsFaceIndex_PH(void* ptr, void* meshPtr, int* indices, int count)
{
	MeshProcessingConnections<EEK>::GetConnectedComponentFaceIndex_PH(ptr, meshPtr, indices, count);
}

int MeshProcessingConnections_EEK_SplitConnectedComponents_PH(void* ptr, void* meshPtr)
{
	return MeshProcessingConnections<EEK>::SplitConnectedComponents_PH(ptr, meshPtr);
}

void MeshProcessingConnections_EEK_GetSplitConnectedComponents_PH(void* ptr, void** meshPtrs, int count)
{
	MeshProcessingConnections<EEK>::GetSplitConnectedComponents_PH(ptr, meshPtrs, count);
}

int MeshProcessingConnections_EEK_KeepLargeConnectedComponents_PH(void* meshPtr, int threshold_value)
{
	return MeshProcessingConnections<EEK>::KeepLargeConnectedComponents_PH(meshPtr, threshold_value);
}

int MeshProcessingConnections_EEK_KeepLargestConnectedComponents_PH(void* meshPtr, int nb_components_to_keep)
{
	return MeshProcessingConnections<EEK>::KeepLargestConnectedComponents_PH(meshPtr, nb_components_to_keep);
}

//Surface Mesh

int MeshProcessingConnections_EEK_ConnectedComponents_SM(void* meshPtr)
{
	return MeshProcessingConnections<EEK>::ConnectedComponents_SM(meshPtr);
}

int MeshProcessingConnections_EEK_ConnectedComponent_SM(void* ptr, void* meshPtr, int index)
{
	return MeshProcessingConnections<EEK>::ConnectedComponent_SM(ptr, meshPtr, index);
}

void MeshProcessingConnections_EEK_GetConnectedComponentsFaceIndex_SM(void* ptr, void* meshPtr, int* indices, int count)
{
	MeshProcessingConnections<EEK>::GetConnectedComponentFaceIndex_SM(ptr, meshPtr, indices, count);
}

int MeshProcessingConnections_EEK_SplitConnectedComponents_SM(void* ptr, void* meshPtr)
{
	return MeshProcessingConnections<EEK>::SplitConnectedComponents_SM(ptr, meshPtr);
}

void MeshProcessingConnections_EEK_GetSplitConnectedComponents_SM(void* ptr, void** meshPtrs, int count)
{
	MeshProcessingConnections<EEK>::GetSplitConnectedComponents_SM(ptr, meshPtrs, count);
}

int MeshProcessingConnections_EEK_KeepLargeConnectedComponents_SM(void* meshPtr, int threshold_value)
{
	return MeshProcessingConnections<EEK>::KeepLargeConnectedComponents_SM(meshPtr, threshold_value);
}

int MeshProcessingConnections_EEK_KeepLargestConnectedComponents_SM(void* meshPtr, int nb_components_to_keep)
{
	return MeshProcessingConnections<EEK>::KeepLargestConnectedComponents_SM(meshPtr, nb_components_to_keep);
}


#ifndef THYME_ENVIRONMENTS_ROVERS_ISENSOR
#define THYME_ENVIRONMENTS_ROVERS_ISENSOR

#include <Eigen/Dense>
#include <rovers/core/detail/pack.hpp>

namespace rovers {

/*
 *
 * sensor interface for bindings
 *
 */
class ISensor {
   public:
    [[nodiscard]] virtual Eigen::MatrixXd scan(const AgentPack& pack) const = 0;
    virtual ~ISensor() = default;
};

}  // namespace rovers
#endif


// メイン部分(solve関数)は一番下
// https://github.com/taiton-k/kyoupuro-template

#define USE_ACL
#define USE_BOOST
//#define USE_BOOST_GRAPH
//#define USE_BOOST_MULTIPRECISION
//#define USE_BOOST_GEOMETRY

#ifndef LOCAL
#pragma GCC optimize("Ofast,unroll-loops")
#pragma GCC target("avx,avx2")
#endif

#include <bits/stdc++.h>
#include <unistd.h>

#ifdef USE_ACL
#include <atcoder/all>
#endif

#ifdef USE_BOOST
#include <boost/range/algorithm.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/algorithm_ext.hpp>

#ifdef USE_BOOST_GRAPH

#include <boost/graph/directed_graph.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/bipartite.hpp>

namespace taiton {

template<class Graph>
using vertex_t = typename boost::graph_traits<Graph>::vertex_descriptor;

template<class Graph>
using edge_t = typename boost::graph_traits<Graph>::edge_descriptor;

template<class Graph>
void get_distance(const Graph& g,std::vector<int>& dist,typename Graph::vertex_descriptor s = 0){
        boost::breadth_first_search(g,s,boost::visitor(boost::make_bfs_visitor(boost::record_distances(dist.data(),boost::on_tree_edge{}))));
}

}

#endif

#ifdef USE_BOOST_GEOMETRY

#include <boost/geometry.hpp>
#if BOOST_VERSION < 107800
#include <boost/geometry/algorithms/detail/azimuth.hpp>
#endif //BOOST_VERSION < 107800

namespace taiton{

#if BOOST_VERSION < 107800
template<class Point1,class Point2>
inline long double azimuth(const Point1 &p1,const Point2 &p2){
        return azimuth<long double>(p1,p2);
}

template<typename T>
class point3_t : public boost::geometry::model::point<T,3,boost::geometry::cs::cartesian> {
        using Base = boost::geometry::model::point<T,3,boost::geometry::cs::cartesian>;
public:
        using Base::Base;

        constexpr inline T x() noexcept {
                return this->template get<0>();
        }

        constexpr inline T y() noexcept {
                return this->template get<1>();
        }

        constexpr inline T z() noexcept {
                return this->template get<2>();
        }
};

#else //BOOST_VERSION < 107800

template<typename T>
using point3_t = boost::geometry::model::d3::point_xyz<T>;

#endif //BOOST_VERSION < 107800

template<typename T>
using point2_t = boost::geometry::model::d2::point_xy<T>;

template<class Point>
using line_t = boost::geometry::model::linestring<Point>;

template<class Point>
using segment_t = boost::geometry::model::segment<Point>;

template<class Point>
using box_t = boost::geometry::model::box<Point>;

template<class Point>
using polygon_t = boost::geometry::model::polygon<Point>;

template<class Point>
inline Point rotate(Point geo,double deg){
        boost::geometry::transform(geo,geo,boost::geometry::strategy::transform::rotate_transformer<boost::geometry::degree,long double,2,2>{deg});
        return geo;
}

template<class Point>
inline Point rotate(Point geo,const Point& org,double deg){
        geo.template set<0>(geo.template get<0>() - org.template get<0>());
        geo.template set<1>(geo.template get<1>() - org.template get<1>());

        geo = rotate(geo,deg);

        geo.template set<0>(geo.template get<0>() + org.template get<0>());
        geo.template set<1>(geo.template get<1>() + org.template get<1>());

        return geo;
}

} // namespace taiton

#endif

#ifdef USE_BOOST_MULTIPRECISION

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

namespace taiton {

namespace mp = boost::multiprecision;
using cint = mp::cpp_int;
using lfloat = mp::number<mp::cpp_dec_float<30>>;

}

#endif

#endif

#include <string>
#include <array>
#include <sstream>
#include <unistd.h>

#include <initializer_list>
#include <type_traits>

using swallow = std::initializer_list<bool>;

template<class T>
struct is_container {
        template<class U>
        static constexpr auto check(U&&)
                -> decltype(std::begin(std::declval<U>()),std::true_type{});

        static constexpr auto check(...)
                -> std::false_type;

        static constexpr bool value = decltype(check(std::declval<T>()))::value;
};

template<class T>
constexpr bool is_container_v = is_container<T>::value;

template<class T>
struct is_multi_dimensional_container {
        template<class U>
        static constexpr auto check(U&&)
                -> decltype(std::begin(std::declval<typename U::value_type>()),std::true_type{});

        static constexpr auto check(...)
                -> std::false_type;

        static constexpr bool value = decltype(check(std::declval<T>()))::value;
};

template<class T>
constexpr bool is_multi_dimensional_container_v
        = is_multi_dimensional_container<T>::value;

template<class T>
constexpr bool is_one_dimensional_container_v
        = is_container_v<T> and !is_multi_dimensional_container_v<T>;

template<class T>
using is_one_dimensional_container
        = std::conditional_t<is_one_dimensional_container_v<T>,std::true_type,std::false_type>;

namespace taiton {

template<size_t BufferSize>
class FastInput {

public:

        template<typename ...Args>
        inline void operator () (Args& ...args) noexcept {
                static_cast<void>(swallow{(input(args),false)...});
        }

        inline void update() noexcept {
                read_buf();
        }

private:

        inline void read_char(char& c) noexcept {
                if(counter_ == readsize_){
                        read_buf();
                }

                c = buffer_[counter_];
                ++counter_;
        }

        inline void read_buf() noexcept {
                readsize_ = read(0,buffer_,BufferSize);
                counter_ = 0;
        }

        inline void input(std::string& s) noexcept {
                char c;
                read_char(c);

                while(c == ' ' or c == '\n'){
                        read_char(c);
                }while(!(c == ' ' or c == '\n')){
                        s += c;
                        read_char(c);
                }
        }

        inline void input(char& c) noexcept {
                read_char(c);
                while(c == ' ' or c == '\n'){
                        read_char(c);
                }
        }

        template<typename T,size_t N>
        inline void input(std::array<T,N>& a) noexcept {
                for(auto&& i : a){
                        input(i);
                }
        }

        template<typename T,typename U>
        inline void input(std::pair<T,U>& p) noexcept {
                input(p.first);
                input(p.second);
        }

#ifdef USE_BOOST_GEOMETRY
        template<typename T>
        inline void input(point2_t<T>& p) noexcept {
                T x,y;
                input(x);
                input(y);

                p.x(x);
                p.y(y);
        }
#endif

        template<typename T>
        inline void input(T& a) noexcept {
                if constexpr (std::is_integral_v<T>) {
                        a = 0;
                        char c;
                        bool minus = false;

                        do{
                                read_char(c);
                        }while(c < '0' and '9' < c and
                        c != '-' and
                        c !='+');

                        if(c == '-'){
                                minus = true;
                        }else{
                                a += c-'0';
                        }

                        read_char(c);
                        while('0' <= c and c <= '9'){
                                a *= 10;
                                a += c-'0';
                                read_char(c);
                        }

                        if(minus){
                                a *= -1;
                        }
                }else if constexpr (is_container_v<T>) {
                        for(auto&& i : a){
                                input(i);
                        }
                }else{
                        std::string s;
                        input(s);

                        std::istringstream iss(s);
                        iss >> a;
                }
        }

        static inline char buffer_[BufferSize];

        size_t readsize_ = 0;

        size_t counter_ = 0;

};
inline FastInput<1024*1024> input;

}

#include <string>
#include <array>
#include <sstream>
#include <iomanip>
#include <cstring>
#include <unistd.h>

namespace taiton {

class FastOutput {

public:

        virtual void write_char(char) noexcept = 0;
        virtual void write_chars(const char*,size_t) noexcept = 0;

        inline void output(const char c) noexcept {
                write_char(c);
        }

        inline virtual void output(const char* s) noexcept {
                while(*s != '\0'){
                        write_char(*s);
                        ++s;
                }
        }

        inline void output(const std::string& s) noexcept {
                write_chars(s.c_str(),s.size());
        }

        template<typename T,typename U>
        inline void output(const std::pair<T,U>& p) noexcept {
                output(p.first);
                output(' ');
                output(p.second);
        }

        template<typename T,size_t N>
        inline void output(std::array<T,N>& a) noexcept {
                for(auto&& i : a){
                        output(i);
                }
        }

#ifdef USE_ACL
        template<int M>
        void output(atcoder::static_modint<M> x) noexcept {
                output(x.val());
        }
#endif

        template<typename T>
        inline void output(const T& a) noexcept {
                if constexpr (std::is_integral_v<T>) {
                        T x = a;
                        if(x == 0){
                                write_char('0');
                        }else{
                                if(x < 0){
                                        write_char('-');
                                        x = -x;
                                }
                                static char num[19];
                                int i;
                                for(i=0;x!=0;++i){
                                        num[i] = '0' + x % 10;
                                        x /= 10;
                                }
                                for(;i>0;--i){
                                        write_char(num[i-1]);
                                }
                        }
                }else if constexpr (std::is_floating_point_v<T>) {
                        std::ostringstream oss;
                        oss << std::fixed << std::setprecision(12) << a;
                        output(oss.str());
                }else if constexpr (is_one_dimensional_container_v<T>) {
                        for(auto&& i : a){
                                output(i);
                                output(' ');
                        }
                        output('\n');
                }else if constexpr (is_multi_dimensional_container_v<T>) {
                        for(auto& i : a){
                                for(auto&& j : i){
                                        output(j);
                                        output(' ');
                                }
                                output('\n');
                        }
                }else{
                        std::ostringstream oss;
                        oss << a;
                        output(oss.str());
                }
        }

};

template<size_t BufferSize>
class FastStdOut : private FastOutput {

public:

        inline ~FastStdOut() noexcept {
                flush();
        }

        template<typename... Args>
        inline void operator () (const Args&... args) noexcept {
                static_cast<void>(swallow{(output(args),false)...});
        }

        inline void flush() noexcept {
                ssize_t state = write(1,buffer_,counter_);
                if(state == -1){
                        // Todo : print error state;
                        exit(errno);
                }
                counter_ = 0;
        }

private:

        inline void write_char(char c) noexcept override {
                if(counter_ == BufferSize){
                        flush();
                }
                buffer_[counter_] = c;
                ++counter_;
        }

        inline void write_chars(const char *c,size_t s) noexcept override {
                if(s != 0){
                        if(counter_ + s >= BufferSize){
                                //memcpy(buffer_+counter_,c,BufferSize-counter_);
                                flush();
                                //s -= BufferSize-counter_;
                        }
                        memcpy(buffer_+counter_,c,s);
                        counter_ += s;
                }
        }

        static inline char buffer_[BufferSize];

        size_t counter_;

};
inline FastStdOut<1024*1024> print;

#ifdef LOCAL

class FastStdErr : private FastOutput {

public:

        template<typename ...Args>
        inline void operator () (const Args& ...args) noexcept {
                output("\e[1m\e[33m");
                static_cast<void>(swallow{(output(args),false)...});
                output("\e[0m");
        }

private:

        inline void write_char(char c) noexcept override {
                ssize_t state;
                state = write(2,&c,1);
                if(state == -1){
                        std::exit(errno);
                }
        }

        inline void write_chars(const char *s,size_t len) noexcept override {
                if(s != 0){
                        ssize_t state = write(2,s,len);
                        if(state == -1){
                                std::exit(errno);
                        }
                }
        }

};
inline FastStdErr debug;

#else // #ifdef LOCAL

constexpr inline void debug(...) noexcept {}

#endif // #ifdef LOCAL

}

#include <cmath>

namespace taiton {

#ifdef USE_ACL
template<int M,int O>
constexpr inline bool operator < (const atcoder::static_modint<M>& a,const atcoder::static_modint<O>& b) noexcept {
        return a.val() < b.val();
}
template<int M,int O>
constexpr inline bool operator > (const atcoder::static_modint<M>& a,const atcoder::static_modint<O>& b) noexcept {
        return a.val() > b.val();
}
template<int M,int O>
constexpr inline bool operator <= (const atcoder::static_modint<M>& a,const atcoder::static_modint<O>& b) noexcept {
        return a.val() <= b.val();
}
template<int M,int O>
constexpr inline bool operator >= (const atcoder::static_modint<M>& a,const atcoder::static_modint<O>& b) noexcept {
        return a.val() >= b.val();
}
#endif

constexpr int ctoi (char c) noexcept {
        return c - '0';
}

template<typename T>
constexpr inline int ilog10(T x) noexcept {
        int res = 0;
        while(x >= 10){
                x /= 10;
                ++res;
        }
        return res;
}

template<typename T>
constexpr inline int ilog2(T x) noexcept {
        int res = 0;
        while(x >= 2){
                x /= 2;
                ++res;
        }
        return res;
}

constexpr inline int64_t ipow(int64_t x,int64_t y) noexcept {
        int64_t res=1;

        while(y!=0){
                if(y % 2 == 1){
                        res *= x;
                }
                y /= 2;
                x *= x;
        }

        return res;
}

template<typename T>
constexpr inline int64_t sum(T a) noexcept {
        return a*(a+1)/2;
}
template<typename T>
constexpr inline int64_t sum(T a,T b) noexcept {
        int64_t res = sum(b)-sum(a-1);

        if(res >= 0){
                return res;
        }else{
                return -res;
        }
}

template<int M = 1000000007>
constexpr inline int mod_sum(int64_t a) noexcept {
        return ((a % M) * ((a+1) % M) / 2) % M;
}
template<int M = 1000000007>
constexpr inline int mod_sum(int64_t a,int64_t b) noexcept {
        if(a > b){
                std::swap(a,b);
        }

        int res = mod_sum<M>(b) - mod_sum<M>(a-1);

        return res;
}

template<typename T>
constexpr inline double rad2deg(const T& rad) noexcept {
        return rad * (180.0 / M_PI);
}

template<typename T>
constexpr inline double deg2rad(const T& deg) noexcept {
        return deg / (180.0 / M_PI);
}

template<typename T,typename U>
constexpr inline bool chmax(T& a,const U& b) noexcept {
        if(a < b){
                a = b;
                return true;
        }else{
                return false;
        }
}

template<typename T>
constexpr inline bool chmin(T& a,const T& b) noexcept {
        if(a > b){
                a = b;
                return true;
        }else{
                return false;
        }
}

template<typename To,typename From>
constexpr inline To cast(From&& a) noexcept {
        return static_cast<To>(a);
}

[[noreturn]] inline void yes() noexcept {
        print("Yes",'\n');
        std::exit(0);
}
[[noreturn]] inline void no() noexcept {
        print("No",'\n');
        std::exit(0);
}
inline void yorn(bool flag) noexcept {
        print(flag ? "Yes" : "No",'\n');
}

struct {
        template<typename T>
        constexpr operator T () {
                return std::numeric_limits<T>::min();
        }
} minusInf;

struct {
        template<typename T>
        constexpr operator T () {
                return std::numeric_limits<T>::max();
        }
        auto operator - () {
                return minusInf;
        }
} inf;

constexpr char spc = ' ';
constexpr char lend = '\n';

}

#include <vector>
#include <string>
#include <set>
#include <array>
#include <deque>
#include <utility>

namespace taiton {

using ll = int64_t;
using uint = uint32_t;
using ull = uint64_t;
using ldouble = long double;
using str = std::string;

//template<typename T>using vec = std::vector<T>;
template<typename T>
class vec : public std::vector<T> {
        using Base = std::vector<T>;
public:
        vec():Base(){}
        vec(size_t size):Base(size){}
        vec(size_t size,const T& init):Base(size,init){}
        vec(const std::initializer_list<T>& list):Base(list){}
        template<class Iterator>
        vec(Iterator beg,Iterator end):Base(beg,end){}
};
using ivec = vec<int>;
using llvec = vec<ll>;
using bvec = vec<bool>;
using fvec = vec<float>;
using dvec = vec<double>;
using ldvec = vec<ldouble>;
using cvec = vec<char>;
using svec = vec<str>;
template<typename T,typename U>using pvec = vec<std::pair<T,U>>;
template<typename ...Args>using tvec = vec<std::tuple<Args...>>;

template<typename T>
class vvec : public std::vector<std::vector<T>> {
        using Base = std::vector<std::vector<T>>;
public:
        vvec():Base(){}
        vvec(size_t n):Base(n){}
        vvec(size_t h,size_t w):Base(h,std::vector<T>(w)){}
        vvec(size_t h,size_t w,const T& init):Base(h,std::vector<T>(w,init)){}
};
using ivvec = vvec<int>;
using llvvec = vvec<ll>;
using bvvec = vvec<bool>;
using fvvec = vvec<float>;
using dvvec = vvec<double>;
using ldvvec = vvec<ldouble>;
using cvvec = vvec<char>;
using svvec = vvec<str>;
template<typename T,typename U>using pvvec = vvec<std::pair<T,U>>;
template<typename ...Args>using tvvec = vvec<std::tuple<Args...>>;

using iset = std::set<int>;
using llset = std::set<ll>;
using bset = std::set<bool>;
using fset = std::set<float>;
using dset = std::set<double>;
using ldset = std::set<ldouble>;
using cset = std::set<char>;
using sset = std::set<str>;
template<typename T,typename U>using pset = std::set<std::pair<T,U>>;

template<typename T>using mset = std::multiset<T>;
using imset = mset<int>;
using llmset = mset<ll>;
using bmset = mset<bool>;
using fmset = mset<float>;
using dmset = mset<double>;
using ldmset = mset<ldouble>;
using cmset = mset<char>;
using smset = mset<str>;

template<typename T>using deq = std::deque<T>;
using ideq = deq<int>;
using lldeq = deq<ll>;
using bdeq = deq<bool>;
using fdeq = deq<float>;
using ddeq = deq<double>;
using lddeq = deq<ldouble>;
using cdeq = deq<char>;
using sdeq = deq<str>;

template<typename T,size_t N>using arr = std::array<T,N>;
template<size_t N>using iarr = arr<int,N>;
template<size_t N>using llarr = arr<ll,N>;
template<size_t N>using barr = arr<bool,N>;
template<size_t N>using farr = arr<float,N>;
template<size_t N>using darr = arr<double,N>;
template<size_t N>using ldarr = arr<ldouble,N>;
template<size_t N>using carr = arr<char,N>;
template<size_t N>using sarr = arr<str,N>;

template<typename T>using initlist = std::initializer_list<T>;

#ifdef USE_ACL
using mint = atcoder::modint1000000007;
using mint2 = atcoder::modint998244353;
using mivec = vec<mint>;
using mi2vec = vec<mint2>;
#endif

}

#define rep_overload(i,n,m, REP, ...) REP
#define rep_0(n) for(int repeatCounter=static_cast<int>(n);repeatCounter;--repeatCounter)
#define rep_1(i,n) for(int i=0,repeatCounter=static_cast<int>(n);i < repeatCounter;++i)
#define rep_2(i,a,n) for(int i=a,repeatCounter=static_cast<int>(n);i < repeatCounter;++i)
#define rep(...) rep_overload(__VA_ARGS__,rep_2,rep_1,rep_0)(__VA_ARGS__)
#define drep_0(n) for(int i=0;i < static_cast<int>(n);++i)
#define drep_1(i,n) for(int i=0;i < static_cast<int>(n);++i)
#define drep_2(i,a,n) for(int i=static_cast<int>(n);i < static_cast<int>(n);++i)
#define drep(...) rep_overload(__VA_ARGS__,drep_2,drep_1,drep_0)(__VA_ARGS__) // 'd' means dynamic
#define fore(p,arr) for(auto& p : arr)

#define all(a) a.begin(),a.end()

void solve(void);
int main(void){

        solve();

        return 0;
}

using namespace std;
using namespace taiton;
#ifdef USE_BOOST
using namespace boost;
#ifdef USE_BOOST_GRAPH
using namespace boost::graph;
#endif
#ifdef USE_BOOST_GEOMETRY
using namespace boost::geometry;
#endif
#ifdef USE_BOOST_MULTIPRECISION
using namespace boost::multiprecision;
#endif
#endif
#ifdef USE_ACL
using namespace atcoder;
#endif

void solve(void){

}


/// \file
/// Maintainer: Felice Serena
///
///

#include "read_png.h"

#include <boost/test/unit_test.hpp>
#include <sstream>

using namespace MouseTrack;

void check_matrix(PictureI expected, PictureI received) {
  BOOST_CHECK(expected.rows() == received.rows());
  BOOST_CHECK(expected.cols() == received.cols());

  for (int i = 0; i < expected.rows(); i += 1) {
    for (int j = 0; j < expected.cols(); j += 1) {
      std::stringstream ss;
      ss << "i: " << i << " j: " << j << " expected: " << expected(i, j)
         << " pic: " << received(i, j) << "\n";
      BOOST_CHECK_MESSAGE(expected(i, j) == received(i, j), ss.str());
    }
  }
}

BOOST_AUTO_TEST_CASE(read_png_grey1x1) {
  // Note: make sure to run ./lib/tests to pass this test (from the build
  // directory which in turn is located in the project root)
  const PictureI pic = read_png("../lib/generic/read_png.test/grey1x1.png");
  PictureI expected(1, 1);
  expected(0, 0) = 128;

  check_matrix(expected, pic);
}

BOOST_AUTO_TEST_CASE(read_png_grey2x2) {
  // Note: make sure to run ./lib/tests to pass this test (from the build
  // directory which in turn is located in the project root)
  const PictureI pic = read_png("../lib/generic/read_png.test/grey2x2.png");
  PictureI expected(2, 2);
  expected(0, 0) = 26;
  expected(1, 0) = 77;
  expected(0, 1) = 51;
  expected(1, 1) = 102;

  check_matrix(expected, pic);
}

BOOST_AUTO_TEST_CASE(read_png_grey2x3) {
  // Note: make sure to run ./lib/tests to pass this test (from the build
  // directory which in turn is located in the project root)
  const PictureI pic = read_png("../lib/generic/read_png.test/grey2x3.png");
  PictureI expected(2, 3);
  expected(0, 0) = 26;
  expected(1, 0) = 102;
  expected(0, 1) = 51;
  expected(1, 1) = 128;
  expected(0, 2) = 77;
  expected(1, 2) = 153;

  check_matrix(expected, pic);
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#pragma once

#include "TestUtils.hpp"

#include <armnn/ArmNN.hpp>

#include <Graph.hpp>
#include <layers/ArgMinMaxLayer.hpp>
#include <layers/BatchToSpaceNdLayer.hpp>
#include <layers/SpaceToDepthLayer.hpp>
#include <layers/PreluLayer.hpp>
#include <layers/StackLayer.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/test/unit_test.hpp>

void ArgMinMaxInferOutputShapeImpl(const armnn::ArgMinMaxDescriptor       descriptor,
                                   const std::vector<armnn::TensorShape>& inputShapes,
                                   std::vector<armnn::TensorShape>&       outputShapes)
{
    armnn::Graph graph;
    auto argMinMaxLayer = graph.AddLayer<armnn::ArgMinMaxLayer>(descriptor, "argMinMax");
    outputShapes = argMinMaxLayer->InferOutputShapes(inputShapes);
}

void ArgMinMaxInferOutputShape4dTest()
{
    armnn::Graph graph;
    armnn::ArgMinMaxDescriptor descriptor;
    descriptor.m_Axis = 2;

    const std::vector<armnn::TensorShape> inputShapes
    {
        { 1, 3, 2, 4 }
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(ArgMinMaxInferOutputShapeImpl(descriptor, inputShapes, outputShapes));

    armnn::TensorShape expectedOutputShape( { 1, 3, 4 } );
    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShape);
}

void ArgMinMaxInferOutputShape3dTest()
{
    armnn::Graph graph;
    armnn::ArgMinMaxDescriptor descriptor;
    descriptor.m_Axis = 0;

    const std::vector<armnn::TensorShape> inputShapes
    {
        { 1, 3, 2 }
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(ArgMinMaxInferOutputShapeImpl(descriptor, inputShapes, outputShapes));

    armnn::TensorShape expectedOutputShape( { 3, 2 } );
    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShape);
}

void ArgMinMaxInferOutputShape2dTest()
{
    armnn::Graph graph;
    armnn::ArgMinMaxDescriptor descriptor;
    descriptor.m_Axis = 1;

    const std::vector<armnn::TensorShape> inputShapes
    {
        { 3, 2 }
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(ArgMinMaxInferOutputShapeImpl(descriptor, inputShapes, outputShapes));

    armnn::TensorShape expectedOutputShape( { 3 } );
    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShape);
}

void ArgMinMaxInferOutputShape1dTest()
{
    armnn::Graph graph;
    armnn::ArgMinMaxDescriptor descriptor;
    descriptor.m_Axis = 0;

    const std::vector<armnn::TensorShape> inputShapes
    {
        { 5 }
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(ArgMinMaxInferOutputShapeImpl(descriptor, inputShapes, outputShapes));

    armnn::TensorShape expectedOutputShape( { 1 } );
    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShape);
}

void BatchToSpaceInferOutputShapeTest()
{
    armnn::Graph graph;

    armnn::BatchToSpaceNdDescriptor descriptor;
    descriptor.m_BlockShape = {2, 2};
    descriptor.m_Crops = {{0, 0}, {2, 0}};
    descriptor.m_DataLayout = armnn::DataLayout::NHWC;

    armnn::BatchToSpaceNdLayer* const batchToSpaceLayer =
        graph.AddLayer<armnn::BatchToSpaceNdLayer>(descriptor, "batchToSpace");

    std::vector<armnn::TensorShape> shapes;
    const std::vector<unsigned int> theDimSizes = {8, 1, 3, 1};
    armnn::TensorShape shape(4, theDimSizes.data());
    shapes.push_back(shape);

    const std::vector<unsigned int> expectedDimSizes = {2, 2, 4, 1};
    armnn::TensorShape expectedShape(4, expectedDimSizes.data());

    BOOST_CHECK(expectedShape == batchToSpaceLayer->InferOutputShapes(shapes).at(0));
}

void SpaceToDepthInferOutputShapeTest()
{
    armnn::Graph graph;

    armnn::SpaceToDepthDescriptor descriptor;
    descriptor.m_BlockSize  = 2;
    descriptor.m_DataLayout = armnn::DataLayout::NHWC;

    armnn::SpaceToDepthLayer* const spaceToDepthLayer =
        graph.AddLayer<armnn::SpaceToDepthLayer>(descriptor, "spaceToDepth");

    std::vector<armnn::TensorShape> shapes;
    const std::vector<unsigned int> dimSizes{ 1, 16, 8, 3 };
    armnn::TensorShape shape(4, dimSizes.data());
    shapes.push_back(shape);

    const std::vector<unsigned int> expectedDimSizes{ 1, 8, 4, 12 };
    armnn::TensorShape expectedShape(4, expectedDimSizes.data());

    BOOST_CHECK(expectedShape == spaceToDepthLayer->InferOutputShapes(shapes).at(0));
}

void PreluInferOutputShapeImpl(const std::vector<armnn::TensorShape>& inputShapes,
                               std::vector<armnn::TensorShape>&       outputShapes)
{
    armnn::Graph graph;
    armnn::PreluLayer* const preluLayer = graph.AddLayer<armnn::PreluLayer>("prelu");
    outputShapes = preluLayer->InferOutputShapes(inputShapes);
}

void PreluInferOutputShapeSameDimsTest()
{
    const std::vector<armnn::TensorShape> inputShapes
    {
        { 5, 1, 1, 7 }, // Input shape
        { 5, 4, 3, 1 }  // Alpha shape
    };

    const std::vector<armnn::TensorShape> expectedOutputShapes
    {
        { 5, 4, 3, 7 }  // Output shape
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(PreluInferOutputShapeImpl(inputShapes, outputShapes));

    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShapes[0]);
}

void PreluInferOutputShapeInputBiggerTest()
{
    const std::vector<armnn::TensorShape> inputShapes
    {
        { 4, 1, 4, 8 }, // Input shape
        { 5, 4, 1 }     // Alpha shape
    };

    const std::vector<armnn::TensorShape> expectedOutputShapes
    {
        { 4, 5, 4, 8 } // Output shape
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(PreluInferOutputShapeImpl(inputShapes, outputShapes));

    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShapes[0]);
}

void PreluInferOutputShapeAlphaBiggerTest()
{
    const std::vector<armnn::TensorShape> inputShapes
    {
        { 4, 1, 2 },   // Input shape
        { 5, 4, 3, 1 } // Alpha shape
    };

    const std::vector<armnn::TensorShape> expectedOutputShapes
    {
        { 5, 4, 3, 2 } // Output shape
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(PreluInferOutputShapeImpl(inputShapes, outputShapes));

    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShapes[0]);
}

void PreluInferOutputShapeNoMatchTest()
{
    const std::vector<armnn::TensorShape> inputShapes
    {
        { 4, 1, 2 },   // Input shape
        { 5, 4, 3, 1 } // Alpha shape
    };

    const std::vector<armnn::TensorShape> expectedOutputShapes
    {
        { 5, 7, 3, 2 } // Output shape
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(PreluInferOutputShapeImpl(inputShapes, outputShapes));

    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] != expectedOutputShapes[0]);
}

void CreatePreluLayerHelper(armnn::Graph& graph,
                            const armnn::TensorShape& inputShape,
                            const armnn::TensorShape& alphaShape,
                            const armnn::TensorShape& outputShape)
{
    // Creates the PReLU layer
    armnn::Layer* const preluLayer = graph.AddLayer<armnn::PreluLayer>("prelu");

    // Creates extra layers
    armnn::Layer* const input  = graph.AddLayer<armnn::InputLayer> (0, "input");
    armnn::Layer* const alpha  = graph.AddLayer<armnn::InputLayer> (1, "alpha");
    armnn::Layer* const output = graph.AddLayer<armnn::OutputLayer>(0, "output");

    // Connects up
    armnn::TensorInfo inputTensorInfo (inputShape,  armnn::DataType::Float32);
    armnn::TensorInfo alphaTensorInfo (alphaShape,  armnn::DataType::Float32);
    armnn::TensorInfo outputTensorInfo(outputShape, armnn::DataType::Float32);
    Connect(input, preluLayer,  inputTensorInfo,  0, 0);
    Connect(alpha, preluLayer,  alphaTensorInfo,  0, 1);
    Connect(preluLayer, output, outputTensorInfo, 0, 0);
}

void PreluValidateTensorShapesFromInputsMatchTest()
{
    armnn::Graph graph;

    // Creates the PReLU layer
    CreatePreluLayerHelper(graph, { 1, 4, 1, 2 }, { 5, 4, 3, 1 }, { 5, 4, 3, 2 });

    // Graph::InferTensorInfos calls Layer::ValidateTensorShapesFromInputs
    BOOST_CHECK_NO_THROW(graph.InferTensorInfos());
}

void PreluValidateTensorShapesFromInputsNoMatchTest()
{
    armnn::Graph graph;

    // Creates the PReLU layer
    CreatePreluLayerHelper(graph, { 1, 4, 1, 2 }, { 5, 4, 3, 1 }, { 5, 7, 3, 2 });

    // Graph::InferTensorInfos calls Layer::ValidateTensorShapesFromInputs
    BOOST_CHECK_THROW(graph.InferTensorInfos(), armnn::LayerValidationException);
}

void StackInferOutputShapeImpl(const armnn::StackDescriptor           descriptor,
                               const std::vector<armnn::TensorShape>& inputShapes,
                               std::vector<armnn::TensorShape>&       outputShapes)
{
    armnn::Graph graph;
    armnn::StackLayer* const stackLayer = graph.AddLayer<armnn::StackLayer>(descriptor, "stack");
    outputShapes = stackLayer->InferOutputShapes(inputShapes);
}

void StackInferOutputShapeFromInputsMatchTest()
{
    armnn::Graph graph;

    armnn::StackDescriptor descriptor;
    descriptor.m_Axis = 1;
    descriptor.m_NumInputs = 3;
    descriptor.m_InputShape = armnn::TensorShape
    (
        { 4, 2 }  // Defined input shape
    );

    const std::vector<armnn::TensorShape> inputShapes
    {
        { 4, 2 }, // Actual input shapes
        { 4, 2 },
        { 4, 2 }
    };

    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(StackInferOutputShapeImpl(descriptor, inputShapes, outputShapes));

    armnn::TensorShape expectedOutputShape
    (
        { 4, 3, 2 }
    );
    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShape);
}

void StackInferOutputShapeFromInputsNoMatchTest()
{
    armnn::Graph graph;

    armnn::StackDescriptor descriptor;
    descriptor.m_Axis = 1;
    descriptor.m_NumInputs = 3;
    descriptor.m_InputShape = armnn::TensorShape
    (
        { 4, 2 }  // Defined input shape
    );

    const std::vector<armnn::TensorShape> inputShapes
    {
        { 4, 2 }, // Actual input shapes
        { 4, 5 }, // Incorrectly shaped input tensor
        { 4, 2 }
    };

    // Output shape is inferred from the descriptor, so should still be correct despite mismatching input shapes
    std::vector<armnn::TensorShape> outputShapes;
    BOOST_CHECK_NO_THROW(StackInferOutputShapeImpl(descriptor, inputShapes, outputShapes));

    armnn::TensorShape expectedOutputShape
    (
        { 4, 3, 2 }
    );
    BOOST_CHECK(outputShapes.size() == 1);
    BOOST_CHECK(outputShapes[0] == expectedOutputShape);
}

void CreateStackLayerHelper(armnn::Graph& graph,
                            const armnn::StackDescriptor& descriptor,
                            const std::vector<armnn::TensorShape>& inputShapes,
                            const armnn::TensorShape& outputShape)
{
    // Creates the Stack layer
    armnn::Layer* const stackLayer = graph.AddLayer<armnn::StackLayer>(descriptor, "stack");

    // Creates extra layers
    std::vector<armnn::Layer*> inputs;
    for (unsigned int i=0; i<inputShapes.size(); ++i)
    {
        inputs.push_back(graph.AddLayer<armnn::InputLayer>(static_cast<int>(i), "input"));
    }
    armnn::Layer* const output = graph.AddLayer<armnn::OutputLayer>(0, "output");

    // Connects up
    std::vector<armnn::TensorInfo> inputTensorInfos;
    for (unsigned int i=0; i<inputs.size(); ++i)
    {
        inputTensorInfos.push_back(armnn::TensorInfo(inputShapes[i], armnn::DataType::Float32));
    }
    armnn::TensorInfo outputTensorInfo(outputShape, armnn::DataType::Float32);

    for (unsigned int i=0; i<inputs.size(); ++i)
    {
        Connect(inputs[i], stackLayer, inputTensorInfos[i], 0, i);
    }
    Connect(stackLayer, output, outputTensorInfo, 0, 0);
}

void StackValidateTensorShapesFromInputsMatchTest()
{
    armnn::Graph graph;

    armnn::StackDescriptor descriptor;
    descriptor.m_Axis = 0;
    descriptor.m_NumInputs = 3;
    descriptor.m_InputShape = armnn::TensorShape
    (
        { 2, 5 }  // Defined input shape
    );

    const std::vector<armnn::TensorShape> inputShapes
    {
        { 2, 5 }, // Actual input shapes
        { 2, 5 },
        { 2, 5 }
    };

    // Creates the Stack layer
    CreateStackLayerHelper(graph, descriptor, inputShapes, { 3, 2, 5 });

    // Graph::InferTensorInfos calls Layer::ValidateTensorShapesFromInputs
    BOOST_CHECK_NO_THROW(graph.InferTensorInfos());
}

void StackValidateTensorShapesFromInputsNoMatchTest()
{
    armnn::Graph graph;

    armnn::StackDescriptor descriptor;
    descriptor.m_Axis = 0;
    descriptor.m_NumInputs = 3;
    descriptor.m_InputShape = armnn::TensorShape
    (
        { 2, 5 }  // Defined input shape
    );

    const std::vector<armnn::TensorShape> inputShapes
    {
        { 2, 5 }, // Actual input shapes
        { 2, 2 }, // Incorrectly shaped input tensor
        { 2, 5 }
    };

    // Creates the Stack layer
    CreateStackLayerHelper(graph, descriptor, inputShapes, { 3, 2, 5 });

    // Graph::InferTensorInfos calls Layer::ValidateTensorShapesFromInputs
    BOOST_CHECK_THROW(graph.InferTensorInfos(), armnn::LayerValidationException);
}

void Convolution2dInferOutputShapeTest()
{
    armnn::Graph graph;

    armnn::Convolution2dDescriptor descriptor;
    descriptor.m_DilationX = 2;
    descriptor.m_DilationY = 2;
    descriptor.m_PadTop = 1;
    descriptor.m_PadBottom = 1;
    descriptor.m_PadLeft = 1;
    descriptor.m_PadRight = 1;
    descriptor.m_StrideX = 3;
    descriptor.m_StrideY = 3;
    descriptor.m_DataLayout = armnn::DataLayout::NCHW;

    armnn::Convolution2dLayer* const convolution2dLayer =
            graph.AddLayer<armnn::Convolution2dLayer>(descriptor, "convolution2d");

    std::vector<armnn::TensorShape> shapes;
    const std::vector<unsigned int> inputSize = {1, 2, 10, 10};
    armnn::TensorShape inputShape(4, inputSize.data());
    shapes.push_back(inputShape);

    const std::vector<unsigned int> filterSize = { 1, 2, 2, 2};
    armnn::TensorShape filterShape(4, filterSize.data());
    shapes.push_back(filterShape);

    const std::vector<unsigned int> expectedOutputSizes = {1, 1, 4, 4};
    armnn::TensorShape expectedOutputShape(4, expectedOutputSizes.data());

    BOOST_CHECK(expectedOutputShape == convolution2dLayer->InferOutputShapes(shapes).at(0));
}

void TransposeConvolution2dInferOutputShapeTest()
{
    armnn::Graph graph;

    armnn::TransposeConvolution2dDescriptor descriptor;
    descriptor.m_PadTop = 0;
    descriptor.m_PadBottom = 1;
    descriptor.m_PadLeft = 0;
    descriptor.m_PadRight = 1;
    descriptor.m_StrideX = 2;
    descriptor.m_StrideY = 2;
    descriptor.m_DataLayout = armnn::DataLayout::NCHW;

    armnn::TransposeConvolution2dLayer* const transposeConvolution2dLayer =
            graph.AddLayer<armnn::TransposeConvolution2dLayer>(descriptor, "TransposeConvolution2d");

    std::vector<armnn::TensorShape> shapes;
    const std::vector<unsigned int> inputSize = {1, 2, 3, 3};
    armnn::TensorShape inputShape(4, inputSize.data());
    shapes.push_back(inputShape);

    const std::vector<unsigned int> filterSize = { 1, 2, 3, 3};
    armnn::TensorShape filterShape(4, filterSize.data());
    shapes.push_back(filterShape);

    const std::vector<unsigned int> expectedOutputSizes = {1, 1, 6, 6};
    armnn::TensorShape expectedOutputShape(4, expectedOutputSizes.data());

    BOOST_CHECK(expectedOutputShape == transposeConvolution2dLayer->InferOutputShapes(shapes).at(0));
}

void DepthwiseConvolution2dInferOutputShapeTest()
{
    armnn::Graph graph;

    armnn::DepthwiseConvolution2dDescriptor descriptor;
    descriptor.m_DilationX = 3;
    descriptor.m_DilationY = 3;
    descriptor.m_PadTop = 1;
    descriptor.m_PadBottom = 2;
    descriptor.m_PadLeft = 1;
    descriptor.m_PadRight = 2;
    descriptor.m_StrideX = 2;
    descriptor.m_StrideY = 2;
    descriptor.m_DataLayout = armnn::DataLayout::NCHW;

    armnn::DepthwiseConvolution2dLayer* const depthwiseConvolution2dLayer =
            graph.AddLayer<armnn::DepthwiseConvolution2dLayer>(descriptor, "DepthwiseConvolution2d");

    std::vector<armnn::TensorShape> shapes;
    const std::vector<unsigned int> inputSize = {1, 2, 10, 10};
    armnn::TensorShape inputShape(4, inputSize.data());
    shapes.push_back(inputShape);

    const std::vector<unsigned int> filterSize = { 1, 2, 3, 3};
    armnn::TensorShape filterShape(4, filterSize.data());
    shapes.push_back(filterShape);

    const std::vector<unsigned int> expectedOutputSizes = {1, 2, 4, 4};
    armnn::TensorShape expectedOutputShape(4, expectedOutputSizes.data());

    BOOST_CHECK(expectedOutputShape == depthwiseConvolution2dLayer->InferOutputShapes(shapes).at(0));
}

// QuantizedLstm
void QuantizedLstmInferOutputShapeImpl(const std::vector<armnn::TensorShape>& inputShapes,
                                       std::vector<armnn::TensorShape>& outputShapes)
{
    armnn::Graph graph;
    armnn::QuantizedLstmLayer* const quantizedLstmLayer = graph.AddLayer<armnn::QuantizedLstmLayer>("quantizedLstm");
    outputShapes = quantizedLstmLayer->InferOutputShapes(inputShapes);
}

void QuantizedLstmInferOutputShapeTest()
{
    // Input shapes
    const std::vector<unsigned int> inputShape{ 2, 5 };
    const std::vector<unsigned int> previousCellStateInShape{ 2, 10 };
    const std::vector<unsigned int> previousOutputInShape{ 2, 10 };
    armnn::TensorShape inputTensorShape(2, inputShape.data());
    armnn::TensorShape previousCellStateInTensorShape(2, previousCellStateInShape.data());
    armnn::TensorShape previousOutputInTensorShape(2, previousOutputInShape.data());

    std::vector<armnn::TensorShape> inShapes
    {
        inputTensorShape,
        previousCellStateInTensorShape,
        previousOutputInTensorShape
    };

    // Output shapes
    const std::vector<unsigned int> cellStateOutShape{ 2, 10 };
    const std::vector<unsigned int> outputShape{ 2, 10 };
    armnn::TensorShape cellStateOutTensorShape(2, cellStateOutShape.data());
    armnn::TensorShape outputTensorShape(2, outputShape.data());

    std::vector<armnn::TensorShape> expectedOutShapes
    {
        cellStateOutTensorShape,
        outputTensorShape
    };

    std::vector<armnn::TensorShape> actualOutShapes;
    BOOST_CHECK_NO_THROW(QuantizedLstmInferOutputShapeImpl(inShapes, actualOutShapes));

    BOOST_CHECK(actualOutShapes.size() == 2);
    BOOST_CHECK(expectedOutShapes[0] == actualOutShapes[0]);
    BOOST_CHECK(expectedOutShapes[1] == actualOutShapes[1]);
}


/*
 * Copyright (C) 2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef _GAZEBO_MOVING_WINDOW_FILTER_HH_
#define _GAZEBO_MOVING_WINDOW_FILTER_HH_

#include <iostream>
#include <vector>
#include <map>

#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>

#include <gazebo/gazebo_config.h>
#include <gazebo/common/Time.hh>
#include <gazebo/common/CommonTypes.hh>
#include <gazebo/math/Helpers.hh>

namespace gazebo
{
  namespace common
  {
    /// \addtogroup gazebo_common Common
    /// \{

    /// \internal
    /// \brief Private data members for MovingWindowFilter class.
    /// This must be in the header due to templatization.
    template< typename T>
    class MovingWindowFilterPrivate
    {
      // \brief Constructor
      public: MovingWindowFilterPrivate();

      /// \brief For moving window smoothed value
      public: unsigned int valWindowSize;

      /// \brief buffer history of raw values
      public: std::vector<T> valHistory;

      /// \brief iterator pointing to current value in buffer
      public: typename std::vector<T>::iterator valIter;

      /// \brief keep track of running sum
      public: T sum;

      /// \brief keep track of number of elements
      public: unsigned int samples;
    };

    //////////////////////////////////////////////////
    template<typename T>
    MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate()
    {
      /// \TODO FIXME hardcoded initial value for now
      this->valWindowSize = 4;
      this->valHistory.resize(this->valWindowSize);
      this->valIter = this->valHistory.begin();
      this->sum = T();
      this->samples = 0;
    }

    /// \class MovingWindowFilter MovingWindowFilter.hh common/common.hh
    /// \brief Base class for MovingWindowFilter
    template< typename T>
    class MovingWindowFilter
    {
      /// \brief Constructor
      public: MovingWindowFilter();

      /// \brief Destructor
      public: virtual ~MovingWindowFilter();

      /// \brief Update value of filter
      /// \param[in] _val new raw value
      public: void Update(T _val);

      /// \brief Set window size
      /// \param[in] _n new desired window size
      public: void SetWindowSize(unsigned int _n);

      /// \brief Get the window size.
      /// \return The size of the moving window.
      public: unsigned int GetWindowSize() const;

      /// \brief Get whether the window has been filled.
      /// \return True if the window has been filled.
      public: bool GetWindowFilled() const;

      /// \brief Get filtered result
      /// \return latest filtered value
      public: T Get();

      /// \brief Allow subclasses to initialize their own data pointer.
      /// \param[in] _d Reference to data pointer.
      protected: MovingWindowFilter<T>(MovingWindowFilterPrivate<T> &_d);

      /// \brief Data pointer.
      protected: MovingWindowFilterPrivate<T> *dataPtr;
    };

    //////////////////////////////////////////////////
    template<typename T>
    MovingWindowFilter<T>::MovingWindowFilter()
      : dataPtr(new MovingWindowFilterPrivate<T>())
    {
    }

    //////////////////////////////////////////////////
    template<typename T>
    MovingWindowFilter<T>::~MovingWindowFilter()
    {
      this->dataPtr->valHistory.clear();
      delete this->dataPtr;
      this->dataPtr = NULL;
    }

    //////////////////////////////////////////////////
    template<typename T>
    void MovingWindowFilter<T>::Update(T _val)
    {
      // update sum and sample size with incoming _val

      // keep running sum
      this->dataPtr->sum += _val;

      // shift pointer, wrap around if end has been reached.
      ++this->dataPtr->valIter;
      if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
      {
        // reset iterator to beginning of queue
        this->dataPtr->valIter = this->dataPtr->valHistory.begin();
      }

      // increment sample size
      ++this->dataPtr->samples;

      if (this->dataPtr->samples > this->dataPtr->valWindowSize)
      {
        // subtract old value if buffer already filled
        this->dataPtr->sum -= (*this->dataPtr->valIter);
        // put new value into queue
        (*this->dataPtr->valIter) = _val;
        // reduce sample size
        --this->dataPtr->samples;
      }
      else
      {
        // put new value into queue
        (*this->dataPtr->valIter) = _val;
      }
    }

    //////////////////////////////////////////////////
    template<typename T>
    void MovingWindowFilter<T>::SetWindowSize(unsigned int _n)
    {
      this->dataPtr->valWindowSize = _n;
      this->dataPtr->valHistory.clear();
      this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
      this->dataPtr->valIter = this->dataPtr->valHistory.begin();
      this->dataPtr->sum = T();
      this->dataPtr->samples = 0;
    }

    //////////////////////////////////////////////////
    template<typename T>
    unsigned int MovingWindowFilter<T>::GetWindowSize() const
    {
      return this->dataPtr->valWindowSize;
    }

    //////////////////////////////////////////////////
    template<typename T>
    bool MovingWindowFilter<T>::GetWindowFilled() const
    {
      return this->dataPtr->samples == this->dataPtr->valWindowSize;
    }

    //////////////////////////////////////////////////
    template<typename T>
    T MovingWindowFilter<T>::Get()
    {
      return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
    }
    /// \}
  }
}
#endif


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef EIGEN_UTILS_H
#define EIGEN_UTILS_H

#include "compare.hpp"
#include <Eigen/Core>
#include <iomanip>
#include <sstream>
#include <string>
#include <limits>

namespace flexiblesusy {

template <typename Derived>
unsigned closest_index(double mass, const Eigen::ArrayBase<Derived>& v)
{
   unsigned pos;
   typename Derived::PlainObject tmp;
   tmp.setConstant(mass);

   (v - tmp).abs().minCoeff(&pos);

   return pos;
}

/**
 * Divides a by b element wise.  If the quotient is not finite, it is
 * set to zero.
 *
 * @param a numerator
 * @param b denominator
 */
template <typename Scalar, int M, int N>
Eigen::Matrix<Scalar,M,N> div_save(
   const Eigen::Matrix<Scalar,M,N>& a, const Eigen::Matrix<Scalar,M,N>& b)
{
   Eigen::Matrix<Scalar,M,N> result(Eigen::Matrix<Scalar,M,N>::Zero());

   for (int i = 0; i < M; i++) {
      for (int k = 0; k < N; k++) {
         const double quotient = a(i,k) / b(i,k);
         if (std::isfinite(quotient))
            result(i,k) = quotient;
      }
   }

   return result;
}

template <class BinaryOp, class Derived>
Derived binary_map(
   const Eigen::ArrayBase<Derived>& a, const Eigen::ArrayBase<Derived>& b, BinaryOp op)
{
   typename Derived::PlainObject result(a.rows(), b.cols());

   assert(a.rows() == b.rows());
   assert(a.cols() == b.cols());

   for (int i = 0; i < a.rows(); i++)
      for (int k = 0; k < a.cols(); k++)
         result(i,k) = op(a(i,k), b(i,k));

   return result;
}

/**
 * The element of v, which is closest to mass, is moved to the
 * position idx.
 *
 * @param idx new index of the mass eigenvalue
 * @param mass mass to compare against
 * @param v vector of masses
 * @param z corresponding mixing matrix
 */
template <typename DerivedArray, typename DerivedMatrix>
void move_goldstone_to(int idx, double mass, Eigen::ArrayBase<DerivedArray>& v,
                       Eigen::MatrixBase<DerivedMatrix>& z)
{
   int pos = closest_index(mass, v);
   if (pos == idx)
      return;

   const int sign = (idx - pos) < 0 ? -1 : 1;
   int steps = std::abs(idx - pos);

   // now we shuffle the states
   while (steps--) {
      const int new_pos = pos + sign;
      v.row(new_pos).swap(v.row(pos));
      z.row(new_pos).swap(z.row(pos));
      pos = new_pos;
   }
}

/**
 * Copies all elements from src to dst which are not close to the
 * elements in cmp.
 *
 * @param src source vector
 * @param cmp vector with elements to compare against
 * @param dst destination vector
 */
template<class Real, int Nsrc, int Ncmp, int Ndst>
void remove_if_equal(const Eigen::Array<Real,Nsrc,1>& src,
                     const Eigen::Array<Real,Ncmp,1>& cmp,
                     Eigen::Array<Real,Ndst,1>& dst)
{
   static_assert(Nsrc == Ncmp + Ndst,
                 "Error: remove_if_equal: vectors have incompatible length!");

   Eigen::Array<Real,Nsrc,1> non_equal(src);

   for (int i = 0; i < Ncmp; i++) {
      const int idx = closest_index(cmp(i), non_equal);
      non_equal(idx) = std::numeric_limits<double>::infinity();
   }

   std::remove_copy_if(non_equal.data(), non_equal.data() + Nsrc,
                       dst.data(), Is_not_finite<Real>());
}

/**
 * @brief reorders vector v according to ordering in vector v2
 * @param v vector with elementes to be reordered
 * @param v2 vector with reference ordering
 */
template<class Real, int N>
void reorder_vector(
   Eigen::Array<Real,N,1>& v,
   const Eigen::Array<Real,N,1>& v2)
{
   Eigen::PermutationMatrix<N> p;
   p.setIdentity();
   std::sort(p.indices().data(), p.indices().data() + p.indices().size(),
             CompareAbs<Real, N>(v2));

#if EIGEN_VERSION_AT_LEAST(3,1,4)
   v.matrix().transpose() *= p.inverse();
#else
   Eigen::Map<Eigen::Matrix<Real,N,1> >(v.data()).transpose() *= p.inverse();
#endif
}

/**
 * @brief reorders vector v according to ordering of diagonal elements in mass_matrix
 * @param v vector with elementes to be reordered
 * @param matrix matrix with diagonal elements with reference ordering
 */
template<class Derived>
void reorder_vector(
   Eigen::Array<double,Eigen::MatrixBase<Derived>::RowsAtCompileTime,1>& v,
   const Eigen::MatrixBase<Derived>& matrix)
{
   reorder_vector(v, matrix.diagonal().array().eval());
}

template<class Derived>
std::string print_scientific(const Eigen::DenseBase<Derived>& v,
                             unsigned number_of_digits = std::numeric_limits<typename Derived::Scalar>::digits10 + 1)
{
   std::ostringstream sstr;

   for (std::size_t i = 0; i < v.rows(); i++) {
      for (std::size_t k = 0; k < v.cols(); k++) {
         sstr << std::setprecision(number_of_digits)
              << std::scientific << v(i,k) << ' ';
      }
      sstr << '\n';
   }

   return sstr.str();
}

} // namespace flexiblesusy

#endif


#include "PanelPlotHelper.h"
#include "TLine.h"
#include "TArrow.h"
#include "TMathText.h"
#include "TLatex.h"
#include "TList.h"
#include "TStyle.h"
#include "TCanvas.h"
#include "TROOT.h"
#include <boost/format.hpp>
#include <set>
#include <iostream>
#include <cmath>
using namespace NswAsap;

void PanelPlotHelper::pcbPlot (xyMap_t xyvalues, const std::string& module, const std::string& title, maskParser_t& parser) {
  std::map<int,board_layout_t> bmap;
  std::set<int> pcb_numbers;
  for(auto p : xyvalues) {
    std::string id = p.first;
    parser.parseMaskId(id);
    pcb_numbers.insert(parser.ipcb());
  }
  for (int ipcb : pcb_numbers) {
    bmap[ipcb] = makeBoardLayout(ipcb, module);
  }
  TH2* h2 = makeHistoLayout(bmap, title);

  histoMaskResiduals(h2, bmap, xyvalues, parser);

  plotHisto(h2);
}

void PanelPlotHelper::maskPlot (yMap_t yvalues, const std::string& module, const std::string& title, maskParser_t& parser) {
  std::map<int,board_layout_t> bmap;
  std::set<int> pcb_numbers;
  for(auto p : yvalues) {
    std::string id = p.first;
    parser.parseMaskId(id);
    pcb_numbers.insert(parser.ipcb());
  }
  for (int ipcb : pcb_numbers) {
    bmap[ipcb] = makeBoardLayout(ipcb, module);
  }
  TH2* h2 = makeHistoLayout(bmap, title);

  histoMaskResiduals(h2, bmap, yvalues, parser);

  plotHisto(h2);
}

PanelPlotHelper::board_layout_t PanelPlotHelper::makeBoardLayout (int ipcb, std::string module) {
  board_layout_t ret;

  double x0 = 963.7; // halfway through the sole
  double y0 = 2021.7; // pins of PCB5

  if (module=="LM1")
    y0 -= (5-ipcb) * 460.8; // pins of PCB ipcb
  else
    y0 -= (5-ipcb) * 435.2; // pins of PCB ipcb for SM1 and SM2

  double dx; // distance in x between middle of sole and pins of PCB ipb
  if (module=="LM1")
    dx = 860.025-(5-ipcb)*460.8*tan(16.5*M_PI/180.);
  else if (module=="SM1")
    dx = 542.890-(5-ipcb)*435.2*tan(10.5*M_PI/180.);
  else if (module=="SM2")
    dx = 1586.147*0.5-(8-ipcb)*435.2*tan(10.5*M_PI/180.);
  else throw std::runtime_error("Not implemented");

  std::vector<std::tuple<std::string,double,double>> masks; // coordinates of the 4 masks relative to the pin
  if (module=="LM1")
    masks
      = {
        std::make_tuple("MASK2",-19.,0.),
        std::make_tuple("MASK1",-64.,0.),
        std::make_tuple("MASK3",-110.862,158.205),
        std::make_tuple("MASK4",-17.138, -158.205)
      };
  else if (module=="SM1")
    masks
      = {
	std::make_tuple("MASK2",-29.,0.),
	std::make_tuple("MASK1",-64.,0.),
	std::make_tuple("MASK3",-94.069,162.237),
	std::make_tuple("MASK4",-33.231, -162.237)
      };
  else if (module=="SM2")
    masks
      = {
	std::make_tuple("MASK2",-29.,0.),
	std::make_tuple("MASK1",-64.,0.),
	std::make_tuple("MASK3",-94.069,162.237),
	std::make_tuple("MASK4",-33.931, -162.237)
      };

  int imask = 0;
  for (int ori : {1, -1}) {
    std::string side=(ori==1)?"RI":"RE";
    std::string pcbtag=(module=="LM1")?"LE":"SE";
    for (auto p : masks) {
      ++imask;
      std::string maskid;
      double x, y;
      std::tie(maskid,x,y) = p;
      x = x0 + ori*(-dx) + ori*x;
      y = y0 + y;
      std::string tagString = (boost::format("PCB_%s%1d_B_%s_%s")%pcbtag%ipcb%side%maskid).str();
      ret.masks[tagString] = TVector2(x,y);
    }
  }
  {
    std::vector<std::tuple<double,double>> d_pos;
    std::string pcbtag=(module=="LM1")?"LE":"SE";
    if (module=="LM1") {
      //d_pos = { std::make_tuple(0., 0.), std::make_tuple(0., 158.205), std::make_tuple(0., -158.205) };
      // TMP DEBUG Strip window test: valid for LE4 only.
      d_pos = {
	std::make_tuple(   0.   , 0.   ), std::make_tuple(   0.   , 158.205), std::make_tuple(   0.   , -158.205),
	std::make_tuple(-250.   , 0.   ), std::make_tuple(-250.   , 158.205), std::make_tuple(-250.   , -158.205),
	std::make_tuple( 250.   , 0.   ), std::make_tuple( 250.   , 158.205), std::make_tuple( 250.   , -158.205),
	std::make_tuple(-500.   , 0.   ), std::make_tuple(-500.   , 158.205), std::make_tuple(-500.   , -158.205),
	std::make_tuple( 500.   , 0.   ), std::make_tuple( 500.   , 158.205), std::make_tuple( 500.   , -158.205),
	std::make_tuple(-655.   , 0.   ), std::make_tuple(-705.   , 158.205), std::make_tuple(-610.   , -158.205),
	std::make_tuple( 655.   , 0.   ), std::make_tuple( 705.   , 158.205), std::make_tuple( 610.   , -158.205),
      };
    } else if (module=="SM1")
      d_pos = { std::make_tuple(0., 0.), std::make_tuple(0., 162.237), std::make_tuple(0., -162.237) };
    else if (module=="SM2")
      d_pos = { std::make_tuple(0., 0.), std::make_tuple(0., 162.237), std::make_tuple(0., -162.237) };
    int istrip=0;
    for (auto p: d_pos) {
      double dx, dy;
      std::tie(dx,dy) = p;
      ++istrip;
      double x = x0 + dx;
      double y = y0 + dy;
      std::string tagString = (boost::format("PCB_%s%1d_B_CN_STRI%1d")%pcbtag%ipcb%istrip).str();
      ret.masks[tagString] = TVector2(x,y);
    }
  }

  // Corners of PCB very approximate (to guide the eye)
  double corner_dx, corner_dy;
  if (module=="LM1") {
    corner_dx = 0.5*460.8*tan(16.5*M_PI/180.);
    corner_dy = 0.5*460.8;
  } else {
    corner_dx = 0.5*435.2*tan(10.5*M_PI/180.);
    corner_dy = 0.5*435.2;
  }
  ret.corners[0] = TVector2(x0-dx-75.+corner_dx,y0-corner_dy);
  ret.corners[1] = TVector2(x0+dx+75.-corner_dx,y0-corner_dy);
  ret.corners[2] = TVector2(x0+dx+75.+corner_dx,y0+corner_dy);
  ret.corners[3] = TVector2(x0-dx-75.-corner_dx,y0+corner_dy);
  return ret;
}

TH2* PanelPlotHelper::makeHistoLayout (const std::map<int,board_layout_t>& bmap, std::string title)  {
  double xmin(std::numeric_limits<double>::max()), xmax(std::numeric_limits<double>::min());
  double ymin(std::numeric_limits<double>::max()), ymax(std::numeric_limits<double>::min());
  for (const auto& p : bmap) {
    const board_layout_t& l = p.second;
    for (int i=0; i<4; ++i) {
      xmin = std::min(xmin, l.corners[i].X());
      xmax = std::max(xmax, l.corners[i].X());
      ymin = std::min(ymin, l.corners[i].Y());
      ymax = std::max(ymax, l.corners[i].Y());
    }
  }
  double rx = std::min(0.2*(xmax-xmin), 200.);
  double ry = std::min(0.2*(ymax-ymin), 200.);
  xmin -= rx;
  xmax += rx;
  ymin -= ry;
  ymax += ry;

  TH2F* h = new TH2F("h", title.c_str(), 100, xmin, xmax, 100, ymin, ymax);
  h->SetStats(0);
  h->GetXaxis()->SetTitle("X [mm]");
  h->GetYaxis()->SetTitle("Y [mm]");
  h->SetDirectory(0);
  for (const auto& p : bmap) {
    const board_layout_t& l = p.second;
    for (int i=0; i<4; ++i) {
      const TVector2& a = l.corners[i];
      const TVector2& b = l.corners[(i+1)%4];
      TLine* l = new TLine(a.X(), a.Y(), b.X(), b.Y());
      h->GetListOfFunctions()->Add(l);
    }
  }
  return h;
}

void PanelPlotHelper::plotHisto (TH2* h2) {
  double xmin = h2->GetXaxis()->GetXmin();
  double xmax = h2->GetXaxis()->GetXmax();
  double ymin = h2->GetYaxis()->GetXmin();
  double ymax = h2->GetYaxis()->GetXmax();

  double w, h; // canvas size
  double l_1000 = 250; // 1 meter should appear as 400 pixels
  double rm = 0.05*700.;
  double lm = 0.15*700.;
  double tm = 0.10*500.;
  double bm = 0.15*500.;
  double l_xaxis = (xmax-xmin)/1000.*l_1000;
  double l_yaxis = (ymax-ymin)/1000.*l_1000;
  w = l_xaxis+rm+lm;
  h = l_yaxis+tm+bm;

  double scale = 1.;
//   double scale = 700./double(std::min(w,h));
//   std::cout << "scale is " << scale << std::endl;

//   gStyle->SetTitleFontSize(0.05*scale);
//   h2->GetXaxis()->SetTitleSize(gStyle->GetTitleXSize()*scale);
//   h2->GetXaxis()->SetLabelSize(gStyle->GetLabelSize()*scale);
//   h2->GetYaxis()->SetTitleSize(gStyle->GetTitleYSize()*scale);
//   h2->GetYaxis()->SetLabelSize(gStyle->GetLabelSize()*scale);

  TIter next(h2->GetListOfFunctions());
  TObject* obj;
  while ((obj = next())) {
    TLatex* text = dynamic_cast<TLatex*>(obj);
    if (text) {
//       text->SetTextSize(text->GetTextSize()*scale);
//       text->SetTextSize(gStyle->GetTitleYSize());
      text->SetTextSize(0.03);
    }
  }

//   std::cout << w << " " << h << std::endl;
  TCanvas * c1 = new TCanvas("c", "c", w, h);
  if (gROOT->IsBatch()) {
    c1->SetCanvasSize(w,h);
  } else {
    c1->SetWindowSize(w + (w - c1->GetWw()), h + (h - c1->GetWh()));
  }
//   c1->SetRightMargin(rm/w);
//   c1->SetLeftMargin(lm/w);
//   c1->SetTopMargin(tm/h);
//   c1->SetBottomMargin(bm/h);
//   c1->SetFixedAspectRatio();

  gStyle->SetOptTitle(1);

  h2->Draw();
}

void PanelPlotHelper::histoMaskResiduals (TH2* h2, const std::map<int, board_layout_t>& bmap, const yMap_t& yvalues, maskParser_t& parser, double scale) {
  for (const auto& p : yvalues) {
    parser.parseMaskId(p.first);
    double deltay = p.second;
    double deltay_arrow = parser.arrowDirection()*deltay;

    int col = parser.color();

    const board_layout_t& l = bmap.find(parser.ipcb())->second;
    TVector2 vmask = l.masks.find(parser.asapId())->second;
    TArrow* arr = new TArrow(vmask.X(), vmask.Y(), vmask.X(), vmask.Y()+scale*deltay_arrow, 0.01, "|>");
    arr->SetLineColor(col);
    arr->SetFillColor(col);
    h2->GetListOfFunctions()->Add(arr);

    bool do_right_align = parser.isRI();
    if (parser.imask() == 2) do_right_align = !do_right_align;

    double offset = (do_right_align ? -30. : + 30.);
    TLatex* text = new TLatex(vmask.X()+offset, vmask.Y(), (boost::format("%.0f#mum")%(deltay*1000.)).str().c_str());
    text->SetTextFont(42);
    text->SetTextSize(0.02);
    if (do_right_align)
      text->SetTextAlign(32); // right adjusted
    else
      text->SetTextAlign(12); // left adjusted
    text->SetTextColor(col);
    h2->GetListOfFunctions()->Add(text);
  }
}

void PanelPlotHelper::histoMaskResiduals (TH2* h2, const std::map<int, board_layout_t>& bmap, const xyMap_t& xyvalues, maskParser_t& parser, double scale) {
  for (const auto& p : xyvalues) {
    parser.parseMaskId(p.first);
    double deltax = p.second.first;
    double deltay = p.second.second;
    double deltax_arrow = parser.arrowDirection()*deltax;
    double deltay_arrow = parser.arrowDirection()*deltay;

    if( !parser.toScale ){
        deltax_arrow = parser.arrowDirection()*0.02*deltax_arrow/abs(deltax_arrow);
        deltay_arrow = parser.arrowDirection()*0.02*deltay_arrow/abs(deltay_arrow);
    }

    bool do_right_align = parser.isRI();
    if (parser.imask() == 2) do_right_align = !do_right_align;

    double offset = (do_right_align ? -30. : + 30.);

    int col = parser.color();

    const board_layout_t& l = bmap.find(parser.ipcb())->second;
    TVector2 vmask = l.masks.find(parser.asapId())->second;
    TArrow* arr;
//     if (std::isnan(deltax))
//       arr = new TArrow(vmask.X(), vmask.Y(), vmask.X(), vmask.Y()+scale*deltay_arrow, 0.01, "|>");
//     else
//       arr = new TArrow(vmask.X(), vmask.Y(), vmask.X()+scale*deltax_arrow, vmask.Y()+scale*deltay_arrow, 0.01, "|>");
    if( parser.singleArrow ){
        arr = new TArrow(vmask.X(), vmask.Y(), vmask.X()+scale*deltax_arrow, vmask.Y()+scale*deltay_arrow, 0.01, "|>");
//         arr = new TArrow(vmask.X()-offset, vmask.Y(), vmask.X()-offset, vmask.Y()+scale*deltay_arrow, 0.01, "|>");
        arr->SetLineColor(col);
        arr->SetFillColor(col);
        h2->GetListOfFunctions()->Add(arr);
    }
    else{
        arr = new TArrow(vmask.X(), vmask.Y(), vmask.X()+scale*deltax_arrow, vmask.Y(), 0.01, "|>");
        arr->SetLineColor(col);
        arr->SetFillColor(col);
        h2->GetListOfFunctions()->Add(arr);

        arr = new TArrow(vmask.X(), vmask.Y(), vmask.X(), vmask.Y()+scale*deltay_arrow, 0.01, "|>");
        arr->SetLineColor(col);
        arr->SetFillColor(col);
        h2->GetListOfFunctions()->Add(arr);
    }

//     bool do_right_align = parser.isRI();
//     if (parser.imask() == 2) do_right_align = !do_right_align;
//
//     double offset = (do_right_align ? -30. : + 30.);
    TLatex* text;
    if (std::isnan(deltax))
      text = new TLatex(vmask.X()+offset, vmask.Y(), (boost::format("%.0f")%(deltay*1000.)).str().c_str());
    else
      text = new TLatex(vmask.X()+offset, vmask.Y(), (boost::format("( %.0f , %.0f )")%(deltax*1000.)%(deltay*1000.)).str().c_str());
//       text = new TLatex(vmask.X()+offset, vmask.Y(), (boost::format("(%.0f+/-%.0f)#mum")%(deltay*1000.)%(deltax*1000.)).str().c_str());
    text->SetTextFont(42);
    text->SetTextSize(0.02);
    if (do_right_align)
      text->SetTextAlign(32); // right adjusted
    else
      text->SetTextAlign(12); // left adjusted
    text->SetTextColor(col);
    h2->GetListOfFunctions()->Add(text);
  }
}


#ifndef STAN_MATH_PRIM_MAT_FUN_TGAMMA_HPP
#define STAN_MATH_PRIM_MAT_FUN_TGAMMA_HPP

#include <stan/math/prim/mat/vectorize/apply_scalar_unary.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace stan {
namespace math {

/**
 * Structure to wrap tgamma() so that it can be vectorized.
 * @param x Variable.
 * @tparam T Variable type.
 * @return Gamma function applied to x.
 * @throw std::domain_error if x is 0 or a negative integer
 */
struct tgamma_fun {
  template <typename T>
  static inline T fun(const T& x) {
    using boost::math::tgamma;
    return tgamma(x);
  }
};

/**
 * Vectorized version of tgamma().
 * @param x Container.
 * @tparam T Container type.
 * @return Gamma function applied to each value in x.
 * @throw std::domain_error if any value is 0 or a negative integer
 */
template <typename T>
inline typename apply_scalar_unary<tgamma_fun, T>::return_t tgamma(const T& x) {
  return apply_scalar_unary<tgamma_fun, T>::apply(x);
}

}  // namespace math
}  // namespace stan

#endif


//-----------------------------------------------------------------------------
// Copyright (c) 2017-2018 Benjamin Buch
//
// https://github.com/bebuch/disposer
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#ifndef _disposer__tool__config_queue__hpp_INCLUDED_
#define _disposer__tool__config_queue__hpp_INCLUDED_

#include <boost/hana/tuple.hpp>
#include <boost/hana/size.hpp>
#include <boost/hana/minus.hpp>

namespace disposer::detail{

	template < std::size_t Offset, typename ... Configs >
	class config_queue{
	public:
		static constexpr auto offset = hana::size_c< Offset >;

		static constexpr auto size = hana::size_c< sizeof...(Configs) > - offset;

		static constexpr auto is_empty = size() == hana::size_c< 0 >;

		constexpr config_queue(hana::tuple< Configs ... > const& tuple)noexcept
			: tuple_(tuple) {}

		constexpr auto next()const noexcept{
			return config_queue< Offset + 1, Configs ... >{tuple_};
		}

		constexpr decltype(auto) front()const noexcept{
			return tuple_[offset];
		}

	private:
		hana::tuple< Configs ... > const& tuple_;
	};

	template < typename ... Configs >
	config_queue(hana::tuple< Configs ... >)
		-> config_queue< 0, Configs ... >;

}

#endif