/**
 * @file nested_cylinders.cc
 * @brief Compute the convergence of the nested cylinders experiment
 */

#define _USE_MATH_DEFINES
#include <annulus_triag_mesh_builder.h>
#include <build_system_matrix.h>
#include <lf/assemble/dofhandler.h>
#include <lf/io/gmsh_reader.h>
#include <lf/io/vtk_writer.h>
#include <lf/mesh/entity.h>
#include <lf/mesh/hybrid2d/mesh_factory.h>
#include <lf/mesh/utils/utils.h>
#include <lf/quad/quad.h>
#include <mesh_function_velocity.h>
#include <norms.h>
#include <piecewise_const_element_matrix_provider.h>
#include <piecewise_const_element_vector_provider.h>
#include <solution_to_mesh_data_set.h>

#include <algorithm>
#include <boost/program_options.hpp>
#include <cstring>
#include <filesystem>
#include <iomanip>
#include <iostream>
#include <string>

using lf::uscalfe::operator-;

/**
 * @brief Solve the nested cylinders problem with zero potential at the boundary
 * @param mesh A shared pointer to the mesh on which to solve the PDE
 * @param dofh The dofhandler used for the simulation
 * @param r The radius of the inner cylinder
 * @param R The radius of the outer cylinder
 * @param omega1 The angular velocity of the inner cylinder
 * @param omega2 The angular velocity of the outer cylinder
 * @param modified_penalty If true, use the modified penalty term instead of the
 * original one
 * @returns A vector of basis function coefficients for the solution
 */
Eigen::VectorXd solveNestedCylindersZeroBC(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh,
    const lf::assemble::DofHandler &dofh, double r, double R, double omega1,
    double omega2, bool modified_penalty) {
  // The volume forces are equal to zero everywhere
  auto f = [](const Eigen::Vector2d & /*unused*/) {
    return Eigen::Vector2d::Zero();
  };
  // Drive the inner and outer cylinder with omega1 and omega2
  const double eps = 1e-10;
  auto dirichlet_funct = [&](const lf::mesh::Entity &edge) -> Eigen::Vector2d {
    const auto *const geom = edge.Geometry();
    const auto vertices = geom->Global(edge.RefEl().NodeCoords());
    if (vertices.col(0).norm() <= R + eps &&
        vertices.col(0).norm() >= R - eps &&
        vertices.col(1).norm() <= R + eps &&
        vertices.col(1).norm() >= R - eps) {
      return omega2 * R * (vertices.col(1) - vertices.col(0)).normalized();
    }
    if (vertices.col(0).norm() <= r + eps &&
        vertices.col(0).norm() >= r - eps &&
        vertices.col(1).norm() <= r + eps &&
        vertices.col(1).norm() >= r - eps) {
      return omega1 * r * (vertices.col(0) - vertices.col(1)).normalized();
    }
    return Eigen::Vector2d::Zero();
  };

  lf::mesh::utils::CodimMeshDataSet<Eigen::Vector2d> dirichlet(mesh, 1);
  for (const auto *ep : mesh->Entities(1)) {
    dirichlet(*ep) = dirichlet_funct(*ep);
  }

  // Asemble the LSE
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh);
  // Assemble the Matrix
  lf::assemble::COOMatrix<double> A(dofh.NumDofs(), dofh.NumDofs());
  const projects::ipdg_stokes::assemble::PiecewiseConstElementMatrixProvider
      elem_mat_provider(100, boundary, modified_penalty);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elem_mat_provider, A);
  // Assemble the right hand side
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  const projects::ipdg_stokes::assemble::PiecewiseConstElementVectorProvider
      elem_vec_provider(100, f, lf::quad::make_TriaQR_MidpointRule(), boundary,
                        dirichlet);
  lf::assemble::AssembleVectorLocally(0, dofh, elem_vec_provider, rhs);

  // Enforce the no-flow boundary conditions
  auto selector = [&](lf::base::size_type idx) -> std::pair<bool, double> {
    const auto &entity = dofh.Entity(idx);
    if (entity.RefEl() == lf::base::RefElType::kPoint && boundary(entity)) {
      return {true, 0};
    }
    return {false, 0};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A, rhs);

  // Solve the LSE using sparse LU
  Eigen::SparseMatrix<double> As = A.makeSparse();
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(As);
  return solver.solve(rhs);
}

/**
 * @brief Solve the nested cylinders problem with constant potential at the
 * boundary
 * @param mesh A shared pointer to the mesh on which to solve the PDE
 * @param dofh The dofhandler used for the simulation
 * @param r The radius of the inner cylinder
 * @param R The radius of the outer cylinder
 * @param omega1 The angular velocity of the inner cylinder
 * @param omega2 The angular velocity of the outer cylinder
 * @param modified_penalty If true, use the modified penalty term instead of the
 * original one
 * @returns A vector of basis function coefficients for the solution
 */
Eigen::VectorXd solveNestedCylindersNonzeroBC(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh,
    const lf::assemble::DofHandler &dofh, double r, double R, double omega1,
    double omega2, bool modified_penalty) {
  // The volume forces are equal to zero everywhere
  auto f = [](const Eigen::Vector2d & /*unused*/) {
    return Eigen::Vector2d::Zero();
  };
  // Drive the inner and outer cylinder with omega1 and omega2
  const double eps = 1e-10;
  auto dirichlet_funct = [&](const lf::mesh::Entity &edge) -> Eigen::Vector2d {
    const auto *const geom = edge.Geometry();
    const auto vertices = geom->Global(edge.RefEl().NodeCoords());
    if (vertices.col(0).norm() <= R + eps &&
        vertices.col(0).norm() >= R - eps &&
        vertices.col(1).norm() <= R + eps &&
        vertices.col(1).norm() >= R - eps) {
      return omega2 * R * (vertices.col(1) - vertices.col(0)).normalized();
    }
    if (vertices.col(0).norm() <= r + eps &&
        vertices.col(0).norm() >= r - eps &&
        vertices.col(1).norm() <= r + eps &&
        vertices.col(1).norm() >= r - eps) {
      return omega1 * r * (vertices.col(0) - vertices.col(1)).normalized();
    }
    return Eigen::Vector2d::Zero();
  };

  lf::mesh::utils::CodimMeshDataSet<Eigen::Vector2d> dirichlet(mesh, 1);
  for (const auto *ep : mesh->Entities(1)) {
    dirichlet(*ep) = dirichlet_funct(*ep);
  }

  // Asemble the LSE
  const auto boundary = lf::mesh::utils::flagEntitiesOnBoundary(mesh);
  // Assemble the Matrix
  lf::assemble::COOMatrix<double> A(dofh.NumDofs(), dofh.NumDofs());
  const projects::ipdg_stokes::assemble::PiecewiseConstElementMatrixProvider
      elem_mat_provider(100, boundary, modified_penalty);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elem_mat_provider, A);
  // Assemble the right hand side
  Eigen::VectorXd rhs = Eigen::VectorXd::Zero(dofh.NumDofs());
  const projects::ipdg_stokes::assemble::PiecewiseConstElementVectorProvider
      elem_vec_provider(100, f, lf::quad::make_TriaQR_MidpointRule(), boundary,
                        dirichlet);
  lf::assemble::AssembleVectorLocally(0, dofh, elem_vec_provider, rhs);

  // Combine the basis functions on the inside boundary to a single one
  const lf::base::size_type M_outer = 0;
  const lf::base::size_type M_inner = 1;
  // Create a mapping of DOF indices to remove the afterwards unused DOFs
  std::vector<lf::base::size_type> dofmap(dofh.NumDofs());
  lf::base::size_type idx = 2;
  for (lf::base::size_type dof = 0; dof < dofh.NumDofs(); ++dof) {
    const auto &entity = dofh.Entity(dof);
    const auto *const geom = entity.Geometry();
    if (entity.RefEl() == lf::base::RefElType::kPoint && boundary(entity)) {
      if (geom->Global(entity.RefEl().NodeCoords()).norm() > R - eps) {
        dofmap[dof] = M_outer;
      } else {
        dofmap[dof] = M_inner;
      }
    } else {
      dofmap[dof] = idx++;
    }
  }
  // Apply this mapping to the triplets of the matrix
  std::for_each(A.triplets().begin(), A.triplets().end(),
                [&](Eigen::Triplet<double> &trip) {
                  trip = Eigen::Triplet<double>(
                      dofmap[trip.row()], dofmap[trip.col()], trip.value());
                });
  // Apply the mapping to the right hand side vector
  Eigen::VectorXd rhs_mapped = Eigen::VectorXd::Zero(idx);
  for (lf::base::size_type dof = 0; dof < dofh.NumDofs(); ++dof) {
    rhs_mapped[dofmap[dof]] += rhs[dof];
  }

  // Set the potential on the outer boundary to zero
  auto selector = [&](lf::base::size_type idx) -> std::pair<bool, double> {
    return {idx == M_outer, 0};
  };
  lf::assemble::FixFlaggedSolutionComponents(selector, A, rhs);

  // Solve the LSE using sparse LU
  Eigen::SparseMatrix<double> As_mapped = A.makeSparse().block(0, 0, idx, idx);
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(As_mapped);
  const Eigen::VectorXd sol_mapped = solver.solve(rhs_mapped);

  // Apply the inverse mapping to recovr the basis function coefficients for the
  // original basis functions
  Eigen::VectorXd sol = Eigen::VectorXd::Zero(dofh.NumDofs());
  for (lf::base::size_type dof = 0; dof < dofh.NumDofs(); ++dof) {
    sol[dof] = sol_mapped[dofmap[dof]];
  }

  return sol;
}

/**
 * @brief Concatenate objects defining an operator<<(std::ostream&)
 * @param args A variadic pack of objects implementing
 * `operator<<(std::ostream&)`
 * @returns A string with the objects concatenated
 */
template <typename... Args>
static std::string concat(Args &&... args) {
  std::ostringstream ss;
  (ss << ... << args);
  return ss.str();
}

/**
 * @brief outputs the L2 and DG norm errors for the nested cylinders experiment
 *
 * Three different command line arguments can be provided:
 *   - builder
 *   - files
 *   - irregular
 *
 * Providing builder as a command line argument will build the meshes with
 * #AnnulusTriagMeshBuilder. Providing files as a command line argument will use
 * uniform meshes generated by GMSH. Providing irregular as a command line
 * argument will use meshes with a sudden jump in mesh resolution.
 */
int main(int argc, char *argv[]) {
  const double r = 0.25;
  const double R = 1;
  const double omega1 = 0;
  const double omega2 = 1;

  // Parse the command line options
  std::string mesh_selection;
  boost::program_options::options_description desc{"Options"};
  desc.add_options()("help,h", "Help Screen")(
      "type", boost::program_options::value<std::string>(&mesh_selection),
      "Type of mesh to use. Either 'builder', 'files' or 'irregular'");
  boost::program_options::positional_options_description pos_desc;
  pos_desc.add("type", 1);
  boost::program_options::command_line_parser parser{argc, argv};
  parser.options(desc).positional(pos_desc).allow_unregistered();
  boost::program_options::parsed_options po = parser.run();
  boost::program_options::variables_map vm;
  boost::program_options::store(po, vm);
  boost::program_options::notify(vm);
  if (vm.count("help") != 0U) {
    std::cout << desc << std::endl;
  }

  std::vector<std::shared_ptr<lf::mesh::Mesh>> meshes;
  if (mesh_selection == "files") {
    // Read the mesh from the gmsh file
    std::filesystem::path meshpath = __FILE__;
    meshpath = meshpath.parent_path();
    for (int i = 0; i <= 4; ++i) {
      const auto meshfile = meshpath / concat("annulus", std::setw(2),
                                              std::setfill('0'), i, ".msh");
      std::unique_ptr<lf::mesh::MeshFactory> factory =
          std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
      lf::io::GmshReader reader(std::move(factory), meshfile.string());
      meshes.push_back(reader.mesh());
    }
  } else if (mesh_selection == "builder") {
    // Build a sequence of meshes
    for (unsigned i = 0U; i < 8U; ++i) {
      const unsigned nx = 4U << i;
      const double dx = 2 * M_PI * (r + R) / 2 / nx;
      const unsigned ny = std::max(static_cast<unsigned>((R - r) / dx), 1U);

      // Build the mesh
      std::unique_ptr<lf::mesh::MeshFactory> factory =
          std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
      projects::ipdg_stokes::mesh::AnnulusTriagMeshBuilder builder(
          std::move(factory));
      builder.setInnerRadius(r);
      builder.setOuterRadius(R);
      builder.setNumAngularCells(nx);
      builder.setNumRadialCells(ny);
      meshes.push_back(builder.Build());
    }
  } else if (mesh_selection == "irregular") {
    std::filesystem::path meshpath = __FILE__;
    const auto mesh_irregular_path =
        meshpath.parent_path() / "annulus_irregular.msh";
    const auto mesh_irregular_inverted_path =
        meshpath.parent_path() / "annulus_irregular_inverted.msh";
    std::unique_ptr<lf::mesh::MeshFactory> factory_irregular =
        std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    std::unique_ptr<lf::mesh::MeshFactory> factory_irregular_inverted =
        std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    lf::io::GmshReader reader_irregular(std::move(factory_irregular),
                                        mesh_irregular_path.string());
    lf::io::GmshReader reader_irregular_inverted(
        std::move(factory_irregular_inverted),
        mesh_irregular_inverted_path.string());
    meshes.push_back(reader_irregular.mesh());
    meshes.push_back(reader_irregular_inverted.mesh());
  } else {
    std::cout << desc << std::endl;
    exit(1);
  }

  // Compute the analytic solution of the problem
  const double C1 = 2 * (omega1 * r * r - omega2 * R * R) / (r * r - R * R);
  const double C2 = ((omega1 - omega2) * r * r * R * R) / (R * R - r * r);
  auto analytic_velocity = [&](const Eigen::Vector2d &x) -> Eigen::Vector2d {
    const double radius = x.norm();
    Eigen::Vector2d vec;
    vec << x[1], -x[0];
    vec.normalize();
    return -(0.5 * C1 * radius + C2 / radius) * vec;
  };
  auto analytic_gradient = [&](const Eigen::Vector2d &x) -> Eigen::Matrix2d {
    const double r2 = x.squaredNorm();
    Eigen::Matrix2d g;
    g << 2 * C2 * x[0] * x[1] / r2 / r2,
        -C1 / 2 - (C2 * r2 - 2 * C2 * x[1] * x[1]) / r2 / r2,
        C1 / 2 + (C2 * r2 - 2 * C2 * x[0] * x[0]) / r2 / r2,
        -2 * C2 * x[0] * x[1] / r2 / r2;
    return g;
  };

  // Solve the problem on each mesh and compute the error
  for (const auto &mesh : meshes) {
    lf::assemble::UniformFEDofHandler dofh(
        mesh,
        {{lf::base::RefEl::kPoint(), 1}, {lf::base::RefEl::kSegment(), 1}});
    const auto fe_space =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
    const Eigen::VectorXd solution_zero =
        solveNestedCylindersZeroBC(mesh, dofh, r, R, omega1, omega2, false);
    const Eigen::VectorXd solution_nonzero =
        solveNestedCylindersNonzeroBC(mesh, dofh, r, R, omega1, omega2, false);
    const Eigen::VectorXd solution_zero_modified =
        solveNestedCylindersZeroBC(mesh, dofh, r, R, omega1, omega2, true);
    const Eigen::VectorXd solution_nonzero_modified =
        solveNestedCylindersNonzeroBC(mesh, dofh, r, R, omega1, omega2, true);
    // Create mesh functions for the analytic and numerical solutions
    const auto velocity_exact =
        lf::mesh::utils::MeshFunctionGlobal(analytic_velocity);
    const auto grad_exact =
        lf::mesh::utils::MeshFunctionGlobal(analytic_gradient);
    const auto velocity_zero =
        projects::ipdg_stokes::post_processing::MeshFunctionVelocity<double,
                                                                     double>(
            fe_space, solution_zero);
    const auto velocity_nonzero =
        projects::ipdg_stokes::post_processing::MeshFunctionVelocity<double,
                                                                     double>(
            fe_space, solution_nonzero);
    const auto velocity_zero_modified =
        projects::ipdg_stokes::post_processing::MeshFunctionVelocity<double,
                                                                     double>(
            fe_space, solution_zero_modified);
    const auto velocity_nonzero_modified =
        projects::ipdg_stokes::post_processing::MeshFunctionVelocity<double,
                                                                     double>(
            fe_space, solution_nonzero_modified);
    // Store the solution
    lf::io::VtkWriter writer_zero(
        mesh, concat("result_zero_", dofh.NumDofs(), ".vtk"));
    lf::io::VtkWriter writer_nonzero(
        mesh, concat("result_nonzero_", dofh.NumDofs(), ".vtk"));
    writer_zero.WriteCellData("velocity", velocity_zero);
    writer_zero.WriteCellData("velocity_modified", velocity_zero_modified);
    writer_nonzero.WriteCellData("velocity", velocity_nonzero);
    writer_nonzero.WriteCellData("velocity_modified",
                                 velocity_nonzero_modified);
    writer_zero.WriteCellData(
        "analytic", lf::mesh::utils::MeshFunctionGlobal(analytic_velocity));
    writer_nonzero.WriteCellData(
        "analytic", lf::mesh::utils::MeshFunctionGlobal(analytic_velocity));
    // Compute the difference between the numerical and the analytical solution
    auto diff_velocity_zero = velocity_zero - velocity_exact;
    auto diff_velocity_zero_modified = velocity_zero_modified - velocity_exact;
    auto diff_velocity_nonzero = velocity_nonzero - velocity_exact;
    auto diff_velocity_nonzero_modified =
        velocity_nonzero_modified - velocity_exact;
    auto diff_gradient_zero = -grad_exact;
    auto diff_gradient_zero_modified = -grad_exact;
    auto diff_gradient_nonzero = -grad_exact;
    auto diff_gradient_nonzero_modified = -grad_exact;
    const auto qr_provider = [](const lf::mesh::Entity &e) {
      return lf::quad::make_QuadRule(e.RefEl(), 0);
    };
    const double L2_zero = projects::ipdg_stokes::post_processing::L2norm(
        mesh, diff_velocity_zero, qr_provider);
    const double L2_nonzero = projects::ipdg_stokes::post_processing::L2norm(
        mesh, diff_velocity_nonzero, qr_provider);
    ;
    const double DG_zero = projects::ipdg_stokes::post_processing::DGnorm(
        mesh, diff_velocity_zero, diff_gradient_zero, qr_provider);
    const double DG_nonzero = projects::ipdg_stokes::post_processing::DGnorm(
        mesh, diff_velocity_nonzero, diff_gradient_nonzero, qr_provider);
    const double L2_zero_modified =
        projects::ipdg_stokes::post_processing::L2norm(
            mesh, diff_velocity_zero_modified, qr_provider);
    const double L2_nonzero_modified =
        projects::ipdg_stokes::post_processing::L2norm(
            mesh, diff_velocity_nonzero_modified, qr_provider);
    const double DG_zero_modified =
        projects::ipdg_stokes::post_processing::DGnorm(
            mesh, diff_velocity_zero_modified, diff_gradient_zero_modified,
            qr_provider);
    const double DG_nonzero_modified =
        projects::ipdg_stokes::post_processing::DGnorm(
            mesh, diff_velocity_nonzero_modified,
            diff_gradient_nonzero_modified, qr_provider);
    std::cout << mesh->NumEntities(2) << ' ' << L2_zero << ' ' << DG_zero << ' '
              << L2_nonzero << ' ' << DG_nonzero << ' ' << L2_zero_modified
              << ' ' << DG_zero_modified << ' ' << L2_nonzero_modified << ' '
              << DG_nonzero_modified << std::endl;
  }

  return 0;
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

/**
 * @file odeint.cpp ODE numerical integration example.
 */

#include <boost/numeric/odeint.hpp>

#include <matplot/matplot.h>

#include "smooth/bundle.hpp"
#include "smooth/compat/odeint.hpp"
#include "smooth/so3.hpp"
#include "smooth/tn.hpp"

#include "plot_tools.hpp"

using matplot::plot;
using std::views::transform;

/**
 * @brief Numerically solve the following ODE on \f$ \mathbb{SO}(3) \times \mathbb{R}^3 \f$:
 *
 * \f[
 * \mathrm{d}^r X_t = v(t)  \\
 * \mathrm{d}^r v_t = -k_p * (X(t) \ominus X_{des}(t)) - k_d * v(t)
 * \f]
 */
int main(int argc, char const * argv[])
{
  using state_t = smooth::Bundle<smooth::SO3d, Eigen::Vector3d>;
  using deriv_t = typename state_t::Tangent;

  std::srand(5);

  // equilibrium point
  const smooth::SO3d Xdes = smooth::SO3d::Identity();

  // "control" proportional and derivative gains
  constexpr double kp = 1;
  constexpr double kd = 1;

  auto ode = [&](const state_t & state, deriv_t & deriv, double t) {
    deriv.template head<3>() = state.part<1>();
    deriv.template tail<3>() = -kp * (state.part<0>() - Xdes) - kd * state.part<1>();
  };

  state_t state;
  state.part<0>() = smooth::SO3d(Eigen::Quaterniond(0, 0.8, 0, 0.1));
  state.part<1>() = Eigen::Vector3d(0, 0, 2);

  std::vector<double> tvec;
  std::vector<state_t> gvec;

  auto stepper = boost::numeric::odeint::
    runge_kutta4<state_t, double, deriv_t, double, boost::numeric::odeint::vector_space_algebra>();

  boost::numeric::odeint::integrate_const(
    stepper, ode, state, 0., 10., 0.01, [&tvec, &gvec](const state_t & s, double t) {
      tvec.push_back(t);
      gvec.push_back(s);
    });

  matplot::figure();
  matplot::hold(matplot::on);
  // plot a sphere
  auto phi = matplot::linspace(0, 2 * M_PI, 200);
  for (double h = -0.9; h < 0.95; h += 0.2) {
    auto xsph = r2v(phi | transform([&](double p) { return std::sqrt(1. - h * h) * std::cos(p); }));
    auto ysph = r2v(phi | transform([&](double p) { return std::sqrt(1. - h * h) * std::sin(p); }));
    auto zsph = r2v(phi | transform([&](double p) { return h; }));
    matplot::plot3(xsph, ysph, zsph)->line_width(0.25).color("gray");
    matplot::plot3(ysph, zsph, xsph)->line_width(0.25).color("gray");
    matplot::plot3(zsph, xsph, ysph)->line_width(0.25).color("gray");
  }
  // plot the trajectory
  auto xyz =
    gvec | transform([](auto s) { return s.template part<0>() * Eigen::Vector3d::UnitZ(); });
  matplot::plot3(r2v(xyz | transform([](auto s) { return s.x(); })),
    r2v(xyz | transform([](auto s) { return s.y(); })),
    r2v(xyz | transform([](auto s) { return s.z(); })))
    ->line_width(4)
    .color("blue");
  matplot::title("Attitude");

  matplot::figure();
  matplot::hold(matplot::on);
  plot(tvec, r2v(gvec | transform([](auto s) { return s.template part<1>()[0]; })), "r")
    ->line_width(2);
  plot(tvec, r2v(gvec | transform([](auto s) { return s.template part<1>()[1]; })), "g")
    ->line_width(2);
  plot(tvec, r2v(gvec | transform([](auto s) { return s.template part<1>()[2]; })), "b")
    ->line_width(2);
  matplot::title("Velocity");

  matplot::show();

  return 0;
}


#include <array>
#include <iostream>
#include <numeric>
#include <string_view>

#include <boost/range/irange.hpp>

#include "input.hpp"

// compile-time parsing of string_view
struct ParseStats {
  int num_cols,
      num_rows;
};

constexpr auto NEWLINE = '\n';

constexpr auto parse_stats(std::string_view input) {
  auto stats = ParseStats{};

  auto& num_cols = stats.num_cols;
  auto& num_rows = stats.num_rows;

  for(const auto c : input) {
    num_cols += (c != NEWLINE);
    num_rows += (c == NEWLINE);
  }

  ++num_rows;
  num_cols /= num_rows;

  return stats;
}

constexpr auto DEFAULT_INPUT = puzzle_input;
constexpr auto DEFAULT_STEPS = 100;
constexpr auto PARSE_STATS = parse_stats(DEFAULT_INPUT);

/*
 * This will create a "ring" buffer around the actual grid, allowing us to
 * bypass the need to treat first/last rows/columns and all four corners of the
 * actual grid as special cases.
 */
constexpr auto GRIDLEN = PARSE_STATS.num_cols + 2;
constexpr auto GRIDWIDTH = PARSE_STATS.num_rows + 2;

constexpr auto GRIDSIZE = (GRIDLEN * GRIDWIDTH);

using Grid = std::array<char, GRIDSIZE>;

const auto my_index = [] (auto row, auto col) {
  return (row * GRIDLEN + col);
};

auto parse(std::string_view input) {

  auto grid = Grid{};

  for(const auto row : boost::irange(1, GRIDWIDTH-1)) {

    for(const auto col : boost::irange(1, GRIDLEN-1)) {

      if(input.front() == '#') {
        grid[my_index(row, col)] = 1;
      }

      input.remove_prefix(1);
    }
    // skip newline
    input.remove_prefix(1);
  }

  return grid;
}

auto num_lights_on(Grid grid, const unsigned steps) {

  /*
   * Each cell in the sums grid represents the sum of turned-on lights from
   * grid[i-1] to grid[i+1]. The sum of three of these cells minus the current
   * cell represents the sum of all turned-on lights in the neighborhood.
   *
   * Because we are only looking at the current cell in any iteration, we can
   * overwrite the grid in-place.
   */
  auto sums = Grid{};

  const auto turn_on_corners = [&grid, OTHERLEN = (2*GRIDLEN)] {
    // top-left corner
    grid[GRIDLEN+1] =
    // top-right corner
    grid[OTHERLEN-2] =
    // bottom-left corner
    grid[GRIDSIZE-OTHERLEN+1] =
    // bottom-right corner
    grid[GRIDSIZE-GRIDLEN-2] = 1;
  };

  turn_on_corners();

  for(const auto step : boost::irange(steps)) {

    for(const auto i : boost::irange(GRIDLEN, GRIDSIZE-GRIDLEN)) {
      sums[i] = grid[i-1]
              + grid[i]
              + grid[i+1];
    }

    for(const auto row: boost::irange(1, (GRIDWIDTH - 1))) {

      for(const auto col : boost::irange(1, (GRIDLEN - 1))) {

        const auto pos = my_index(row, col);

        auto& cell = grid[pos];

        const auto sum = sums[pos]
                       - cell
                       + sums[pos+GRIDLEN]
                       + sums[pos-GRIDLEN];

        cell = (sum == 3) or (sum == 2 and cell == 1);

      }
    }
    turn_on_corners();
  }

  return std::accumulate(grid.begin(), grid.end(), 0);
}

auto solution(std::string_view input, const unsigned steps) {

  const auto& grid = parse(input);

  const auto result = num_lights_on(grid, steps);

  return result;
}

int main() {

  std::cout << solution(DEFAULT_INPUT, DEFAULT_STEPS) << std::endl;

}


#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Geometry>

#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/stitching.hpp>
#include <opencv2/video.hpp>
#include <opencv2/optflow.hpp>

#include <opencv2/cudaoptflow.hpp>
#include "opencv2/cudaarithm.hpp"

#include <cstdio>
#include <iostream>

#include <sphericalsfm/so3.h>
#include <sphericalsfm/plane_estimator.h>
#include <sphericalsfm/preemptive_ransac.h>

#include "stereo_panorama_tools.h"

using namespace sphericalsfm;

namespace stereopanotools {

    static cv::Ptr<cv::cuda::BroxOpticalFlow> optflow = cv::cuda::BroxOpticalFlow::create(0.197f, 50.0f, 0.8f, 10, 150, 10);

    static const double depth = 10; // depth of plane
    static const double synth_radius = 0.5; // radius of circle of synthetic views
    static const double synth_focal_factor = 1.2; // synthetic focal = factor * input focal

    void _compute_flow( cv::Mat &prev, cv::Mat &next, cv::Mat &out, bool upsample = false )
    {
        cv::cuda::GpuMat d_frame0(prev);
        cv::cuda::GpuMat d_frame1(next);

        cv::cuda::GpuMat d_flow(d_frame0.size(), CV_32FC2), d_flowxy;
        cv::cuda::GpuMat d_frame0f;
        cv::cuda::GpuMat d_frame1f;

        d_frame0.convertTo(d_frame0f, CV_32F, 1.0 / 255.0);
        d_frame1.convertTo(d_frame1f, CV_32F, 1.0 / 255.0);

        optflow->calc(d_frame0f, d_frame1f, d_flow);

        d_flow.download(out);

        if ( upsample )
        {
            cv::Mat out2;
            cv::resize(out, out2, cv::Size(0,0), 2, 2, cv::INTER_LINEAR);
            out2 *= 2;
            out = out2;
        }
    }

    Eigen::Vector3d project(const Eigen::Vector3d &vec, const Eigen::Vector3d &up)
    {
      return vec - up * (vec.dot(up));
    }

    double signed_angle_between( const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &up )
    {
        return atan2(a.cross(b).dot(up),a.dot(b));
    }

    bool get_synthetic_column_maps(
                                    double focal, double centerx, double centery,
                                    double theta, double phi,
                                    const Keyframe &left, const Keyframe &right,
                                    cv::Mat &xL, cv::Mat &xR )
    {
        Eigen::Vector3d synth_t(0,0,-synth_radius);
        Eigen::Matrix3d synth_R = so3exp(Eigen::Vector3d(0,-theta,0));
        Eigen::Vector3d synth_center = -synth_R.transpose() * synth_t;
        double synth_focal = focal * synth_focal_factor;

        Eigen::Vector3d left_center = -left.R.transpose() * left.t;
        Eigen::Vector3d right_center = -right.R.transpose() * right.t;

        for ( int y = 0; y < xL.rows; y++ )
        {
          double col = tan(phi);

          Eigen::Vector3d synth_x(col,(y-centery)/synth_focal,1);
          Eigen::Vector3d synth_X = synth_x * depth;
          Eigen::Vector3d world_X = synth_R.transpose() * (synth_X - synth_t);

          // find projection in left and right images
          Eigen::Vector3d XL = left.R * world_X + left.t;
          Eigen::Vector3d XR = right.R * world_X + right.t;

          if ( XL(2) <= 0 ) return false;
          if ( XR(2) <= 0 ) return false;

          Eigen::Vector2d XL_proj(focal*XL(0)/XL(2)+centerx,focal*XL(1)/XL(2)+centery);
          Eigen::Vector2d XR_proj(focal*XR(0)/XR(2)+centerx,focal*XR(1)/XR(2)+centery);

          xL.at<cv::Vec2f>(y,0) = cv::Vec2f(XL_proj(0),XL_proj(1));
          xR.at<cv::Vec2f>(y,0) = cv::Vec2f(XR_proj(0),XR_proj(1));
        }

        return true;
    }

    bool synthesize_column_linear( double focal, double centerx, double centery,
                                   double theta, double phi, double alpha,
                                   Keyframe &left, Keyframe &right,
                                   cv::Mat &left_image_float, cv::Mat &right_image_float,
                                   cv::Mat synth_column )
    {
        const int height = synth_column.rows;

        cv::Mat xL(height,1,CV_32FC2);
        cv::Mat xR(height,1,CV_32FC2);

        bool success = get_synthetic_column_maps( focal, centerx, centery, theta, phi, left, right, xL, xR );
        if ( !success ) return false;

        // remap left and right views
        cv::Mat I_L;
        cv::Mat I_R;
        cv::remap(left_image_float,I_L,xL,cv::noArray(),cv::INTER_LINEAR);
        cv::remap(right_image_float,I_R,xR,cv::noArray(),cv::INTER_LINEAR);

        cv::Mat result;
        cv::addWeighted(I_L,(1.0-alpha),I_R,alpha,0.,result);
        result.convertTo(synth_column, CV_8UC3);

        return true;
    }

    bool synthesize_column_flowbased( double focal, double centerx, double centery,
                          double theta, double phi, double alpha,
                          Keyframe &left, Keyframe &right,
                          cv::Mat &left_image_float, cv::Mat &right_image_float,
                          cv::Mat &forward_flow, cv::Mat &backward_flow,
                          cv::Mat &synth_column )
    {
        const int width = 1;
        const int height = synth_column.rows;

        cv::Mat xL(height,width,CV_32FC2);
        cv::Mat xR(height,width,CV_32FC2);

        bool success = get_synthetic_column_maps( focal, centerx, centery, theta, phi, left, right, xL, xR);
        if ( !success ) return false;

        /// compute plane-induced displacement
        cv::Mat v_LR;
        cv::Mat v_RL;
        cv::subtract( xR, xL, v_LR );
        cv::subtract( xL, xR, v_RL );

        // look up flows
        cv::Mat F_LR;
        cv::Mat F_RL;
        cv::remap(forward_flow, F_LR, xL, cv::noArray(), cv::INTER_LINEAR);
        cv::remap(backward_flow, F_RL, xR, cv::noArray(), cv::INTER_LINEAR);

        // compute corrected flows
        cv::Mat Fs_LR;
        cv::Mat Fs_RL;
        cv::subtract( v_LR, F_LR, Fs_LR );
        cv::subtract( v_RL, F_RL, Fs_RL );

        // compute maps for interpolation
        cv::Mat xs_L;
        cv::Mat xs_R;

        cv::addWeighted( xL, 1.0, Fs_LR, alpha, 0.0, xs_L );
        cv::addWeighted( xR, 1.0, Fs_RL, 1.0-alpha, 0.0, xs_R );

        // remap left and right views
        cv::Mat I_L;
        cv::Mat I_R;
        cv::remap(left_image_float,I_L,xs_L,cv::noArray(),cv::INTER_LINEAR);
        cv::remap(right_image_float,I_R,xs_R,cv::noArray(),cv::INTER_LINEAR);

        // alpha blend
        cv::Mat result;
        cv::addWeighted(I_L,(1.0-alpha),I_R,alpha,0.,result);
        result.convertTo(synth_column, CV_8UC3);

        return true;
    }

    void load_keyframes( const std::string & posespath, std::vector<Keyframe> &keyframes )
    {
        FILE *posesf = fopen(posespath.c_str(),"r");
        while ( true )
        {
            int index;
            double t[3];
            double r[3];

            int nread = fscanf(posesf,"%d %lf %lf %lf %lf %lf %lf\n",
                               &index,
                               t+0,t+1,t+2,
                               r+0,r+1,r+2);
            if ( nread != 7 ) break;

            Keyframe kf;
            kf.index = index;
            kf.t << t[0],t[1],t[2];
            kf.r << r[0],r[1],r[2];
            kf.R = so3exp(kf.r);

            keyframes.push_back(kf);
        }
        fclose(posesf);
    }

    void decompose_keyframe_rotations( std::vector<Keyframe> &keyframes )
    {
        for ( int i = 0; i < keyframes.size(); i++ )
        {
            Keyframe &kf = keyframes[i];

            Eigen::Vector3d up(0,1,0);
            Eigen::Vector3d Rup = kf.R*up;
            std::cout << "Rup: " << Rup.transpose() << "\n";
            double angle = acos(up.dot(Rup));
            std::cout << "angle: " << angle*180/M_PI << "\n";

            if ( fabs(angle) > 0 )
            {
                Eigen::Vector3d v = up.cross(Rup);
                v /= v.norm();
                kf.Rxz = so3exp(v*angle);
            }
            else
            {
                kf.Rxz = Eigen::Matrix3d::Identity();
            }
            kf.Ry = kf.Rxz.transpose()*kf.R;

            Eigen::Vector3d ry = so3ln(kf.Ry);
            kf.theta = -ry(1);
        }
    }

    double compute_theta( const Eigen::Vector3d &c, const Eigen::Vector3d &up )
    {
      Eigen::Vector3d cproj = c - up * c.dot(up);
      Eigen::Vector3d x = Eigen::Vector3d(1,0,0);
      Eigen::Vector3d xproj = x - up * (x.dot(up));
      return atan2(xproj.cross(cproj).dot(up),xproj.dot(cproj))+M_PI;
    }

    void compute_thetas( std::vector<Keyframe> &keyframes )
    {
        Eigen::Vector3d up = Eigen::Vector3d(0,1,0);
        for ( int i = 0; i < keyframes.size(); i++ )
        {
            Eigen::Vector3d c = -keyframes[i].R.transpose() * keyframes[i].t;
            keyframes[i].theta = compute_theta( c, up );
        }
    }

    Eigen::Matrix3d get_rotation( const Eigen::Vector3d &from, const Eigen::Vector3d &to )
    {
        Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
        double angle = acos(from.dot(to));
        if ( fabs(angle) > 0 )
        {
            Eigen::Vector3d v = from.cross(to);
            v /= v.norm();
            R = so3exp(v*angle);
        }
        return R;
    }

    void estimate_plane( std::vector<Keyframe> &keyframes )
    {
        RayPairList centers(keyframes.size());
        for ( int i = 0; i < keyframes.size(); i++ )
        {
            Keyframe &kf = keyframes[i];

            // get camera center
            Eigen::Vector3d c = -kf.R.transpose() * kf.t;
            Ray ray;
            ray.head(3) = c;
            centers[i] = std::make_pair( ray, ray );
        }

        std::vector<PlaneEstimator*> estimators(200);
        for ( int i = 0; i < estimators.size(); i++ ) estimators[i] = new PlaneEstimator;

        PreemptiveRANSAC<RayPairList, PlaneEstimator> plane_ransac;
        plane_ransac.inlier_threshold = 0.01;

        std::vector<bool> inliers;
        PlaneEstimator *best_estimator = NULL;
        int ninliers = plane_ransac.compute( centers.begin(), centers.end(), estimators, &best_estimator, inliers );
        std::cout << ninliers << "/" << centers.size() << " inliers\n";
        std::cout << best_estimator->normal << "\n";
        Eigen::Vector3d up = best_estimator->normal;
        if ( up(1) < 0 ) up = -up;

        // calculate rotation to correct up vector
        Eigen::Matrix3d correction = get_rotation( up, Eigen::Vector3d(0,1,0) );
        std::cout << "correction rotation:\n" << correction << "\n";
        std::cout << "corrected up vector:\n" << correction * up << "\n";

        // c = -R.' * t
        // newup = correction * up
        // newc = correction * c = - correction * R.' * t = - (R * correction.') .' * t

        // rotate all cameras to correct up vector
        for ( int i = 0; i < keyframes.size(); i++ )
        {
            Keyframe &kf = keyframes[i];

            kf.R = kf.R * correction.transpose();
            kf.r = so3ln(kf.R);
        }

        // check if we should flip upside-down
        int nflip = 0;
        for ( int i = 0; i < keyframes.size(); i++ )
        {
            Keyframe &kf = keyframes[i];
            if ( kf.R(1,1) < 0 ) nflip++;
        }

        if ( nflip > keyframes.size() / 2 )
        {
            std::cout << "FLIP\n";
            Eigen::Matrix3d Rflip = Eigen::Matrix3d::Identity();
            Rflip(1,1) = -1;
            Rflip(2,2) = -1;
            for ( int i = 0; i < keyframes.size(); i++ )
            {
                Keyframe &kf = keyframes[i];
                kf.R = kf.R * Rflip;
                kf.r = so3ln(kf.R);
            }
        }
    }

    cv::Mat convert_to_spherical(const Intrinsics &intrinsics, const cv::Mat &cylindrical )
    {
        const int height = cylindrical.cols/2;
        double vertfov = 2. * atan2(cylindrical.rows,2.*intrinsics.focal);

        // do vertical remap first
        // make lookup table for vertical remap
        std::vector<double> lut(height);

        double minphi = -M_PI/2;
        double maxphi = M_PI/2;
        double phistep = (maxphi-minphi)/(height-1);

        for ( int phinum = 0; phinum < height; phinum++ )
        {
            double phi = minphi + phinum*phistep;
            double H = tan(phi);
            lut[phinum] = intrinsics.focal*H+intrinsics.centery;
        }

        // now remap columns of cylindrical panorama
        cv::Mat spherical(height,cylindrical.cols,CV_8UC3);

        for ( int y = 0; y < height; y++ )
        {
            if ( std::isinf(lut[y]) || std::isnan(lut[y]) ) continue;
            if ( lut[y] < 0 || lut[y] >= cylindrical.rows ) continue;
            cv::Mat patch;
            cv::getRectSubPix(cylindrical,cv::Size(cylindrical.cols,1),cv::Point2f(cylindrical.cols/2.f,lut[y]),patch);
            patch.copyTo(spherical.row(y));
        }

        // resize horizontally if necessary
        cv::Mat spherical_resized;
        if ( spherical.cols != 2*height )
        {
            cv::resize( spherical, spherical_resized, cv::Size(2*height,height) );
        } else {
            spherical_resized = spherical;
        }

        return spherical_resized;
    }

    void make_stereo_panoramas( const Intrinsics &intrinsics, const std::string &videopath, const std::string &outputpath,
        const int panowidth, const bool is_loop )
    {
        const int start_theta = 0;
        const int end_theta = panowidth;

        std::string posespath = outputpath + "/poses.txt";

        std::vector<Keyframe> keyframes;

        std::cout << "loading keyframes from " << posespath << "...\n";
        load_keyframes( posespath, keyframes );
        std::cout << "loaded " << keyframes.size() << " keyframes\n";

        std::cout << "estimating plane...\n";
        Eigen::Vector3d up(0,1,0);
        estimate_plane( keyframes );

        std::cout << "decomposing rotations...\n";
        decompose_keyframe_rotations( keyframes );

        std::cout << "computing thetas...\n";
        compute_thetas( keyframes );

        std::cout << "keyframe poses: \n";
        for ( int i = 0; i < keyframes.size(); i++ ) std::cout << i << "\t" << keyframes[i].t.transpose() << "\t" << so3ln(keyframes[i].R).transpose() << "\n";

        std::cout << "thetas before: \n";
        for ( int i = 0; i < keyframes.size(); i++ ) std::cout << i << "\t" << keyframes[i].theta*180/M_PI << "\n";

        // re-order keyframes if necessary to make thetas increase
        std::cout << "re-ordering...\n";
        int npos = 0;
        int nneg = 0;
        for ( int i = 0; i < 10; i++ )
        {
            if ( keyframes[1].theta < keyframes[0].theta ) nneg++;
            else if ( keyframes[0].theta < keyframes[1].theta ) npos++;
        }
        bool reverse = ( nneg > npos );

        // remove end frames that overlap with beginning frames
        if ( is_loop )
        {
            if ( reverse )
            {
                while ( keyframes.back().theta < keyframes[0].theta )
                {
                    keyframes.pop_back();
                }
            } else {
                while ( keyframes.back().theta > keyframes[0].theta )
                {
                    keyframes.pop_back();
                }
            }
        }

        std::cout << "thetas after: \n";
        for ( int i = 0; i < keyframes.size(); i++ ) std::cout << i << "\t" << keyframes[i].theta*180/M_PI << "\n";

        std::cout << "loading images...\n";
        cv::VideoCapture cap(videopath);
        int video_index = -1;
        for ( int i = 0; i < keyframes.size(); i++ )
        {
            while ( video_index < keyframes[i].index )
            {
                if ( !cap.read(keyframes[i].image) )
                {
                    std::cout << "could not read all keyframe images from " << videopath << "\n";
                    exit(1);
                }
                video_index++;
            }
        }

        const int width = keyframes[0].image.cols;
        const int height = keyframes[0].image.rows;

        const int nphi = 9;
        const double min_phi = (-(nphi-1)/2.)*M_PI/180.;
        const double max_phi = ((nphi-1)/2.)*M_PI/180.;
        std::vector<double> phirange(nphi);
        if ( nphi == 1 )
        {
          phirange[0] = 0;
        } else {
          for ( int i = 0; i < nphi; i++ ) phirange[i] = min_phi + i*(max_phi-min_phi)/(nphi-1);
        }

        const double min_theta = -M_PI;
        const double max_theta = M_PI;
        const int ntheta = panowidth;
        std::vector<double> thetarange(ntheta);
        const double theta_step = (max_theta-min_theta)/(ntheta-1);
        for ( int i = 0; i < ntheta; i++ ) thetarange[i] = min_theta + i*theta_step;

        std::cout << "interpolating frames...\n";

        int last_leftnum = -1;
        cv::Mat forward_flow;
        cv::Mat backward_flow;
        cv::Mat left_image;
        cv::Mat right_image;
        cv::Mat left_image_gray;
        cv::Mat right_image_gray;
        cv::Mat left_image_float;
        cv::Mat right_image_float;

        std::vector<cv::Mat> panoramas(nphi);
        for ( int phinum = 0; phinum < nphi; phinum++ )
        {
            panoramas[phinum] = cv::Mat(height,ntheta,CV_8UC3,cv::Scalar(0,0,0,0));
        }

        // iterate through each keyframe pair
        for ( int kfnum = 0; kfnum < keyframes.size(); kfnum++ )
        {
          if ( kfnum % 10 == 0 ) std::cout << kfnum+1 << " / " << keyframes.size() << "\n";
          if ( !is_loop && kfnum == keyframes.size()-1 ) break;

          int leftnum = kfnum;
          int rightnum = (kfnum+1)%keyframes.size();

          Keyframe &left = keyframes[leftnum];
          Keyframe &right = keyframes[rightnum];

          // load images
          left_image = left.image;
          cv::cvtColor( left_image, left_image_gray, cv::COLOR_BGR2GRAY );
          left_image.convertTo(left_image_float,CV_32FC3);
          right_image = right.image;
          right_image.convertTo(right_image_float,CV_32FC3);
          cv::cvtColor( right_image, right_image_gray, cv::COLOR_BGR2GRAY );

          // compute flow
          _compute_flow( left_image_gray, right_image_gray, forward_flow );
          _compute_flow( right_image_gray, left_image_gray, backward_flow );

          // get left and right camera centers
          Eigen::Vector3d C_L = -left.R.transpose() * left.t;
          Eigen::Vector3d C_R = -right.R.transpose() * right.t;

          bool found_one_theta = false;

          // find theta / phi combinations which fall between these keyframes
          for ( int thetanum = start_theta; thetanum < end_theta; thetanum++ )
          {
            double theta = thetarange[thetanum];
            Eigen::Vector3d synth_t(0,0,-synth_radius);
            Eigen::Vector3d synth_r(0,-theta,0);
            Eigen::Matrix3d synth_R = so3exp(synth_r);

            // get synthetic camera center
            Eigen::Vector3d C_D = -synth_R.transpose() * synth_t;

            // project left and right camera centers into synth camera
            Eigen::Vector3d r_L = C_L - C_D;
            Eigen::Vector3d r_R = C_R - C_D;

            // project rays to circle
            Eigen::Vector3d rs_L = project(r_L,up);
            Eigen::Vector3d rs_R = project(r_R,up);

            for ( int phinum = 0; phinum < phirange.size(); phinum++ )
            {
              double phi = phirange[phinum];

              // get synthetic ray in synth camera coordinate frame
              Eigen::Vector3d r_D(tan(phi),0,1);
              // get synthetic ray in world coordinate frame
              r_D = synth_R.transpose() * (r_D - synth_t);

              // project synthetic ray to circle
              Eigen::Vector3d rs_D = project(r_D,up);

              double angle_LD = signed_angle_between(rs_L,rs_D,up);
              double angle_RD = signed_angle_between(rs_R,rs_D,up);
              double angle_LR = signed_angle_between(rs_L,rs_R,up);

              // check that cameras lie on either side of synth ray
              if ( ! ( angle_LD * angle_RD < 0 ) ) continue;

              // check that cameras are within front hemisphere
              const double angle_thresh = M_PI/2;
              if ( fabs(angle_LD) >= angle_thresh ) continue;
              if ( fabs(angle_RD) >= angle_thresh ) continue;

              // calculate alpha
              double alpha = fabs(angle_LD)/fabs(angle_LR);
              //std::cout << alpha << "\n";

              // synthesize column
              cv::Mat synth_column(height,1,CV_8UC3);

              bool success = synthesize_column_flowbased( intrinsics.focal, intrinsics.centerx, intrinsics.centery, theta, phi, alpha, left, right, left_image_float, right_image_float, forward_flow, backward_flow, synth_column );
              if ( !success ) continue;
              found_one_theta = true;
              std::cout << theta << " " << phi << "\n";

              // store column in output panorama
              int colout = thetanum;
              int shift = round(phi/theta_step);
              colout = (colout+shift)%ntheta;
              if ( colout < 0 ) colout += ntheta;

              synth_column.copyTo(panoramas[phinum].col(colout));
            }
          }

        }

        std::vector<cv::Mat> spherical_panos(panoramas.size());
        for ( int i = 0; i < panoramas.size(); i++ )
        {
            spherical_panos[i] = convert_to_spherical(intrinsics, panoramas[i]);
        }
        for ( int phinum = 0; phinum < nphi; phinum++ )
        {
            cv::imwrite(outputpath + "/cylindrical" + std::to_string(phinum) + ".png", panoramas[phinum]);
            cv::imwrite(outputpath + "/spherical" + std::to_string(phinum) + ".png", spherical_panos[phinum]);
        }
        for ( int phinum = 0; phinum < nphi/2; phinum++ )
        {
            cv::Mat overunder;
            cv::vconcat(spherical_panos[nphi-phinum-1],spherical_panos[phinum],overunder);
            cv::imwrite(outputpath + "/overunder" + std::to_string(nphi-phinum-1) + std::to_string(phinum) + ".png", overunder);
            cv::imwrite(outputpath + "/overunder" + std::to_string(nphi-phinum-1) + std::to_string(phinum) + ".jpg", overunder);
        }
    }
}


/* ----------------------------------------------------------------------------
 * Copyright 2021, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#pragma once

#include "panther_types.hpp"
#include <Eigen/Dense>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Convex_hull_traits_3.h>
#include <CGAL/convex_hull_3.h>
#include <CGAL/Triangulation_3.h>
#include <decomp_geometry/polyhedron.h>
#include <decomp_geometry/polyhedron.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Convex_hull_traits_3<K> Traits;
typedef Traits::Polyhedron_3 CGAL_Polyhedron_3;
typedef K::Segment_3 Segment_3;
typedef K::Plane_3 Plane_3;
// define point creator
typedef K::Point_3 Point_3;
typedef K::Vector_3 Vector_3;
typedef CGAL::Creator_uniform_3<double, Point_3> PointCreator;

typedef std::vector<CGAL_Polyhedron_3> ConvexHullsOfCurve;
typedef std::vector<ConvexHullsOfCurve> ConvexHullsOfCurves;

ConvexHullsOfCurves_Std vectorGCALPol2vectorStdEigen(ConvexHullsOfCurves& convexHulls);

vec_E<Polyhedron<3>> vectorGCALPol2vectorJPSPol(ConvexHullsOfCurves& convex_hulls_of_curves);

CGAL_Polyhedron_3 convexHullOfPoints(const std::vector<Point_3>& points);

mt::Edges vectorGCALPol2edges(const ConvexHullsOfCurves& convexHulls);


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*!
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2004 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

/*  This example shows how to set up a Term Structure and then price a simple
    swap.
*/

#include <ql/qldefines.hpp>
#ifdef BOOST_MSVC
#  include <ql/auto_link.hpp>
#endif
#include <ql/termstructures/yield/piecewiseyieldcurve.hpp>
#include <ql/termstructures/yield/ratehelpers.hpp>
#include <ql/pricingengines/swap/discountingswapengine.hpp>
#include <ql/indexes/ibor/euribor.hpp>
#include <ql/time/imm.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/time/daycounters/thirty360.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#include <boost/timer.hpp>
#include <iostream>
#include <iomanip>

using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

int main(int, char* []) {

    try {

        boost::timer timer;
        std::cout << std::endl;

        /*********************
         ***  MARKET DATA  ***
         *********************/

        Calendar calendar = TARGET();
        Date settlementDate(22, September, 2004);
        // must be a business day
        settlementDate = calendar.adjust(settlementDate);

        Integer fixingDays = 2;
        Date todaysDate = calendar.advance(settlementDate, -fixingDays, Days);
        // nothing to do with Date::todaysDate
        Settings::instance().evaluationDate() = todaysDate;

        todaysDate = Settings::instance().evaluationDate();
        std::cout << "Today: " << todaysDate.weekday()
                  << ", " << todaysDate << std::endl;

        std::cout << "Settlement date: " << settlementDate.weekday()
                  << ", " << settlementDate << std::endl;

        // deposits
        Rate d1wQuote=0.0382;
        Rate d1mQuote=0.0372;
        Rate d3mQuote=0.0363;
        Rate d6mQuote=0.0353;
        Rate d9mQuote=0.0348;
        Rate d1yQuote=0.0345;
        // FRAs
        Rate fra3x6Quote=0.037125;
        Rate fra6x9Quote=0.037125;
        Rate fra6x12Quote=0.037125;
        // futures
        Real fut1Quote=96.2875;
        Real fut2Quote=96.7875;
        Real fut3Quote=96.9875;
        Real fut4Quote=96.6875;
        Real fut5Quote=96.4875;
        Real fut6Quote=96.3875;
        Real fut7Quote=96.2875;
        Real fut8Quote=96.0875;
        // swaps
        Rate s2yQuote=0.037125;
        Rate s3yQuote=0.0398;
        Rate s5yQuote=0.0443;
        Rate s10yQuote=0.05165;
        Rate s15yQuote=0.055175;

        /********************
         ***    QUOTES    ***
         ********************/

        // SimpleQuote stores a value which can be manually changed;
        // other Quote subclasses could read the value from a database
        // or some kind of data feed.

        // deposits
        boost::shared_ptr<Quote> d1wRate(new SimpleQuote(d1wQuote));
        boost::shared_ptr<Quote> d1mRate(new SimpleQuote(d1mQuote));
        boost::shared_ptr<Quote> d3mRate(new SimpleQuote(d3mQuote));
        boost::shared_ptr<Quote> d6mRate(new SimpleQuote(d6mQuote));
        boost::shared_ptr<Quote> d9mRate(new SimpleQuote(d9mQuote));
        boost::shared_ptr<Quote> d1yRate(new SimpleQuote(d1yQuote));
        // FRAs
        boost::shared_ptr<Quote> fra3x6Rate(new SimpleQuote(fra3x6Quote));
        boost::shared_ptr<Quote> fra6x9Rate(new SimpleQuote(fra6x9Quote));
        boost::shared_ptr<Quote> fra6x12Rate(new SimpleQuote(fra6x12Quote));
        // futures
        boost::shared_ptr<Quote> fut1Price(new SimpleQuote(fut1Quote));
        boost::shared_ptr<Quote> fut2Price(new SimpleQuote(fut2Quote));
        boost::shared_ptr<Quote> fut3Price(new SimpleQuote(fut3Quote));
        boost::shared_ptr<Quote> fut4Price(new SimpleQuote(fut4Quote));
        boost::shared_ptr<Quote> fut5Price(new SimpleQuote(fut5Quote));
        boost::shared_ptr<Quote> fut6Price(new SimpleQuote(fut6Quote));
        boost::shared_ptr<Quote> fut7Price(new SimpleQuote(fut7Quote));
        boost::shared_ptr<Quote> fut8Price(new SimpleQuote(fut8Quote));
        // swaps
        boost::shared_ptr<Quote> s2yRate(new SimpleQuote(s2yQuote));
        boost::shared_ptr<Quote> s3yRate(new SimpleQuote(s3yQuote));
        boost::shared_ptr<Quote> s5yRate(new SimpleQuote(s5yQuote));
        boost::shared_ptr<Quote> s10yRate(new SimpleQuote(s10yQuote));
        boost::shared_ptr<Quote> s15yRate(new SimpleQuote(s15yQuote));

        /*********************
         ***  RATE HELPERS ***
         *********************/

        // RateHelpers are built from the above quotes together with
        // other instrument dependant infos.  Quotes are passed in
        // relinkable handles which could be relinked to some other
        // data source later.

        // deposits
        DayCounter depositDayCounter = Actual360();

        boost::shared_ptr<RateHelper> d1w(new DepositRateHelper(
            Handle<Quote>(d1wRate),
            1*Weeks, fixingDays,
            calendar, ModifiedFollowing,
            true, depositDayCounter));
        boost::shared_ptr<RateHelper> d1m(new DepositRateHelper(
            Handle<Quote>(d1mRate),
            1*Months, fixingDays,
            calendar, ModifiedFollowing,
            true, depositDayCounter));
        boost::shared_ptr<RateHelper> d3m(new DepositRateHelper(
            Handle<Quote>(d3mRate),
            3*Months, fixingDays,
            calendar, ModifiedFollowing,
            true, depositDayCounter));
        boost::shared_ptr<RateHelper> d6m(new DepositRateHelper(
            Handle<Quote>(d6mRate),
            6*Months, fixingDays,
            calendar, ModifiedFollowing,
            true, depositDayCounter));
        boost::shared_ptr<RateHelper> d9m(new DepositRateHelper(
            Handle<Quote>(d9mRate),
            9*Months, fixingDays,
            calendar, ModifiedFollowing,
            true, depositDayCounter));
        boost::shared_ptr<RateHelper> d1y(new DepositRateHelper(
            Handle<Quote>(d1yRate),
            1*Years, fixingDays,
            calendar, ModifiedFollowing,
            true, depositDayCounter));

        // setup FRAs
        boost::shared_ptr<RateHelper> fra3x6(new FraRateHelper(
            Handle<Quote>(fra3x6Rate),
            3, 6, fixingDays, calendar, ModifiedFollowing,
            true, depositDayCounter));
        boost::shared_ptr<RateHelper> fra6x9(new FraRateHelper(
            Handle<Quote>(fra6x9Rate),
            6, 9, fixingDays, calendar, ModifiedFollowing,
            true, depositDayCounter));
        boost::shared_ptr<RateHelper> fra6x12(new FraRateHelper(
            Handle<Quote>(fra6x12Rate),
            6, 12, fixingDays, calendar, ModifiedFollowing,
            true, depositDayCounter));

        // setup futures
        // Rate convexityAdjustment = 0.0;
        Integer futMonths = 3;
        Date imm = IMM::nextDate(settlementDate);
        boost::shared_ptr<RateHelper> fut1(new FuturesRateHelper(
            Handle<Quote>(fut1Price),
            imm,
            futMonths, calendar, ModifiedFollowing,
            true, depositDayCounter));
        imm = IMM::nextDate(imm+1);
        boost::shared_ptr<RateHelper> fut2(new FuturesRateHelper(
            Handle<Quote>(fut2Price),
            imm,
            futMonths, calendar, ModifiedFollowing,
            true, depositDayCounter));
        imm = IMM::nextDate(imm+1);
        boost::shared_ptr<RateHelper> fut3(new FuturesRateHelper(
            Handle<Quote>(fut3Price),
            imm,
            futMonths, calendar, ModifiedFollowing,
            true, depositDayCounter));
        imm = IMM::nextDate(imm+1);
        boost::shared_ptr<RateHelper> fut4(new FuturesRateHelper(
            Handle<Quote>(fut4Price),
            imm,
            futMonths, calendar, ModifiedFollowing,
            true, depositDayCounter));
        imm = IMM::nextDate(imm+1);
        boost::shared_ptr<RateHelper> fut5(new FuturesRateHelper(
            Handle<Quote>(fut5Price),
            imm,
            futMonths, calendar, ModifiedFollowing,
            true, depositDayCounter));
        imm = IMM::nextDate(imm+1);
        boost::shared_ptr<RateHelper> fut6(new FuturesRateHelper(
            Handle<Quote>(fut6Price),
            imm,
            futMonths, calendar, ModifiedFollowing,
            true, depositDayCounter));
        imm = IMM::nextDate(imm+1);
        boost::shared_ptr<RateHelper> fut7(new FuturesRateHelper(
            Handle<Quote>(fut7Price),
            imm,
            futMonths, calendar, ModifiedFollowing,
            true, depositDayCounter));
        imm = IMM::nextDate(imm+1);
        boost::shared_ptr<RateHelper> fut8(new FuturesRateHelper(
            Handle<Quote>(fut8Price),
            imm,
            futMonths, calendar, ModifiedFollowing,
            true, depositDayCounter));

        // setup swaps
        Frequency swFixedLegFrequency = Annual;
        BusinessDayConvention swFixedLegConvention = Unadjusted;
        DayCounter swFixedLegDayCounter = Thirty360(Thirty360::European);
        boost::shared_ptr<IborIndex> swFloatingLegIndex(new Euribor6M);

        boost::shared_ptr<RateHelper> s2y(new SwapRateHelper(
            Handle<Quote>(s2yRate), 2*Years,
            calendar, swFixedLegFrequency,
            swFixedLegConvention, swFixedLegDayCounter,
            swFloatingLegIndex));
        boost::shared_ptr<RateHelper> s3y(new SwapRateHelper(
            Handle<Quote>(s3yRate), 3*Years,
            calendar, swFixedLegFrequency,
            swFixedLegConvention, swFixedLegDayCounter,
            swFloatingLegIndex));
        boost::shared_ptr<RateHelper> s5y(new SwapRateHelper(
            Handle<Quote>(s5yRate), 5*Years,
            calendar, swFixedLegFrequency,
            swFixedLegConvention, swFixedLegDayCounter,
            swFloatingLegIndex));
        boost::shared_ptr<RateHelper> s10y(new SwapRateHelper(
            Handle<Quote>(s10yRate), 10*Years,
            calendar, swFixedLegFrequency,
            swFixedLegConvention, swFixedLegDayCounter,
            swFloatingLegIndex));
        boost::shared_ptr<RateHelper> s15y(new SwapRateHelper(
            Handle<Quote>(s15yRate), 15*Years,
            calendar, swFixedLegFrequency,
            swFixedLegConvention, swFixedLegDayCounter,
            swFloatingLegIndex));

        /*********************
         **  CURVE BUILDING **
         *********************/

        // Any DayCounter would be fine.
        // ActualActual::ISDA ensures that 30 years is 30.0
        DayCounter termStructureDayCounter =
            ActualActual(ActualActual::ISDA);

        double tolerance = 1.0e-15;

        // A depo-swap curve
        std::vector<boost::shared_ptr<RateHelper> > depoSwapInstruments;
        depoSwapInstruments.push_back(d1w);
        depoSwapInstruments.push_back(d1m);
        depoSwapInstruments.push_back(d3m);
        depoSwapInstruments.push_back(d6m);
        depoSwapInstruments.push_back(d9m);
        depoSwapInstruments.push_back(d1y);
        depoSwapInstruments.push_back(s2y);
        depoSwapInstruments.push_back(s3y);
        depoSwapInstruments.push_back(s5y);
        depoSwapInstruments.push_back(s10y);
        depoSwapInstruments.push_back(s15y);
        boost::shared_ptr<YieldTermStructure> depoSwapTermStructure(
            new PiecewiseYieldCurve<Discount,LogLinear>(
                                          settlementDate, depoSwapInstruments,
                                          termStructureDayCounter,
                                          tolerance));

        // A depo-futures-swap curve
        std::vector<boost::shared_ptr<RateHelper> > depoFutSwapInstruments;
        depoFutSwapInstruments.push_back(d1w);
        depoFutSwapInstruments.push_back(d1m);
        depoFutSwapInstruments.push_back(fut1);
        depoFutSwapInstruments.push_back(fut2);
        depoFutSwapInstruments.push_back(fut3);
        depoFutSwapInstruments.push_back(fut4);
        depoFutSwapInstruments.push_back(fut5);
        depoFutSwapInstruments.push_back(fut6);
        depoFutSwapInstruments.push_back(fut7);
        depoFutSwapInstruments.push_back(fut8);
        depoFutSwapInstruments.push_back(s3y);
        depoFutSwapInstruments.push_back(s5y);
        depoFutSwapInstruments.push_back(s10y);
        depoFutSwapInstruments.push_back(s15y);
        boost::shared_ptr<YieldTermStructure> depoFutSwapTermStructure(
            new PiecewiseYieldCurve<Discount,LogLinear>(
                                       settlementDate, depoFutSwapInstruments,
                                       termStructureDayCounter,
                                       tolerance));

        // A depo-FRA-swap curve
        std::vector<boost::shared_ptr<RateHelper> > depoFRASwapInstruments;
        depoFRASwapInstruments.push_back(d1w);
        depoFRASwapInstruments.push_back(d1m);
        depoFRASwapInstruments.push_back(d3m);
        depoFRASwapInstruments.push_back(fra3x6);
        depoFRASwapInstruments.push_back(fra6x9);
        depoFRASwapInstruments.push_back(fra6x12);
        depoFRASwapInstruments.push_back(s2y);
        depoFRASwapInstruments.push_back(s3y);
        depoFRASwapInstruments.push_back(s5y);
        depoFRASwapInstruments.push_back(s10y);
        depoFRASwapInstruments.push_back(s15y);
        boost::shared_ptr<YieldTermStructure> depoFRASwapTermStructure(
            new PiecewiseYieldCurve<Discount,LogLinear>(
                                       settlementDate, depoFRASwapInstruments,
                                       termStructureDayCounter,
                                       tolerance));

        // Term structures that will be used for pricing:
        // the one used for discounting cash flows
        RelinkableHandle<YieldTermStructure> discountingTermStructure;
        // the one used for forward rate forecasting
        RelinkableHandle<YieldTermStructure> forecastingTermStructure;

        /*********************
        * SWAPS TO BE PRICED *
        **********************/

        // constant nominal 1,000,000 Euro
        Real nominal = 1000000.0;
        // fixed leg
        Frequency fixedLegFrequency = Annual;
        BusinessDayConvention fixedLegConvention = Unadjusted;
        BusinessDayConvention floatingLegConvention = ModifiedFollowing;
        DayCounter fixedLegDayCounter = Thirty360(Thirty360::European);
        Rate fixedRate = 0.04;
        DayCounter floatingLegDayCounter = Actual360();

        // floating leg
        Frequency floatingLegFrequency = Semiannual;
        boost::shared_ptr<IborIndex> euriborIndex(
                                     new Euribor6M(forecastingTermStructure));
        Spread spread = 0.0;

        Integer lenghtInYears = 5;
        VanillaSwap::Type swapType = VanillaSwap::Payer;

        Date maturity = settlementDate + lenghtInYears*Years;
        Schedule fixedSchedule(settlementDate, maturity,
                               Period(fixedLegFrequency),
                               calendar, fixedLegConvention,
                               fixedLegConvention,
                               DateGeneration::Forward, false);
        Schedule floatSchedule(settlementDate, maturity,
                               Period(floatingLegFrequency),
                               calendar, floatingLegConvention,
                               floatingLegConvention,
                               DateGeneration::Forward, false);
        VanillaSwap spot5YearSwap(swapType, nominal,
            fixedSchedule, fixedRate, fixedLegDayCounter,
            floatSchedule, euriborIndex, spread,
            floatingLegDayCounter);

        Date fwdStart = calendar.advance(settlementDate, 1, Years);
        Date fwdMaturity = fwdStart + lenghtInYears*Years;
        Schedule fwdFixedSchedule(fwdStart, fwdMaturity,
                                  Period(fixedLegFrequency),
                                  calendar, fixedLegConvention,
                                  fixedLegConvention,
                                  DateGeneration::Forward, false);
        Schedule fwdFloatSchedule(fwdStart, fwdMaturity,
                                  Period(floatingLegFrequency),
                                  calendar, floatingLegConvention,
                                  floatingLegConvention,
                                  DateGeneration::Forward, false);
        VanillaSwap oneYearForward5YearSwap(swapType, nominal,
            fwdFixedSchedule, fixedRate, fixedLegDayCounter,
            fwdFloatSchedule, euriborIndex, spread,
            floatingLegDayCounter);

        /***************
        * SWAP PRICING *
        ****************/

        // utilities for reporting
        std::vector<std::string> headers(4);
        headers[0] = "term structure";
        headers[1] = "net present value";
        headers[2] = "fair spread";
        headers[3] = "fair fixed rate";
        std::string separator = " | ";
        Size width = headers[0].size() + separator.size()
                   + headers[1].size() + separator.size()
                   + headers[2].size() + separator.size()
                   + headers[3].size() + separator.size() - 1;
        std::string rule(width, '-'), dblrule(width, '=');
        std::string tab(8, ' ');

        // calculations
        std::cout << dblrule << std::endl;
        std::cout <<  "5-year market swap-rate = "
                  << std::setprecision(2) << io::rate(s5yRate->value())
                  << std::endl;
        std::cout << dblrule << std::endl;

        std::cout << tab << "5-years swap paying "
                  << io::rate(fixedRate) << std::endl;
        std::cout << headers[0] << separator
                  << headers[1] << separator
                  << headers[2] << separator
                  << headers[3] << separator << std::endl;
        std::cout << rule << std::endl;

        Real NPV;
        Rate fairRate;
        Spread fairSpread;

        boost::shared_ptr<PricingEngine> swapEngine(
                         new DiscountingSwapEngine(discountingTermStructure));

        spot5YearSwap.setPricingEngine(swapEngine);
        oneYearForward5YearSwap.setPricingEngine(swapEngine);

        // Of course, you're not forced to really use different curves
        forecastingTermStructure.linkTo(depoSwapTermStructure);
        discountingTermStructure.linkTo(depoSwapTermStructure);

        NPV = spot5YearSwap.NPV();
        fairSpread = spot5YearSwap.fairSpread();
        fairRate = spot5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        // let's check that the 5 years swap has been correctly re-priced
        QL_REQUIRE(std::fabs(fairRate-s5yQuote)<1e-8,
                   "5-years swap mispriced by "
                   << io::rate(std::fabs(fairRate-s5yQuote)));

        forecastingTermStructure.linkTo(depoFutSwapTermStructure);
        discountingTermStructure.linkTo(depoFutSwapTermStructure);

        NPV = spot5YearSwap.NPV();
        fairSpread = spot5YearSwap.fairSpread();
        fairRate = spot5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-fut-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        QL_REQUIRE(std::fabs(fairRate-s5yQuote)<1e-8,
                   "5-years swap mispriced!");

        forecastingTermStructure.linkTo(depoFRASwapTermStructure);
        discountingTermStructure.linkTo(depoFRASwapTermStructure);

        NPV = spot5YearSwap.NPV();
        fairSpread = spot5YearSwap.fairSpread();
        fairRate = spot5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-FRA-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        QL_REQUIRE(std::fabs(fairRate-s5yQuote)<1e-8,
                   "5-years swap mispriced!");

        std::cout << rule << std::endl;

        // now let's price the 1Y forward 5Y swap

        std::cout << tab << "5-years, 1-year forward swap paying "
                  << io::rate(fixedRate) << std::endl;
        std::cout << headers[0] << separator
                  << headers[1] << separator
                  << headers[2] << separator
                  << headers[3] << separator << std::endl;
        std::cout << rule << std::endl;

        forecastingTermStructure.linkTo(depoSwapTermStructure);
        discountingTermStructure.linkTo(depoSwapTermStructure);

        NPV = oneYearForward5YearSwap.NPV();
        fairSpread = oneYearForward5YearSwap.fairSpread();
        fairRate = oneYearForward5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        forecastingTermStructure.linkTo(depoFutSwapTermStructure);
        discountingTermStructure.linkTo(depoFutSwapTermStructure);

        NPV = oneYearForward5YearSwap.NPV();
        fairSpread = oneYearForward5YearSwap.fairSpread();
        fairRate = oneYearForward5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-fut-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        forecastingTermStructure.linkTo(depoFRASwapTermStructure);
        discountingTermStructure.linkTo(depoFRASwapTermStructure);

        NPV = oneYearForward5YearSwap.NPV();
        fairSpread = oneYearForward5YearSwap.fairSpread();
        fairRate = oneYearForward5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-FRA-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        // now let's say that the 5-years swap rate goes up to 4.60%.
        // A smarter market element--say, connected to a data source-- would
        // notice the change itself. Since we're using SimpleQuotes,
        // we'll have to change the value manually--which forces us to
        // downcast the handle and use the SimpleQuote
        // interface. In any case, the point here is that a change in the
        // value contained in the Quote triggers a new bootstrapping
        // of the curve and a repricing of the swap.

        boost::shared_ptr<SimpleQuote> fiveYearsRate =
            boost::dynamic_pointer_cast<SimpleQuote>(s5yRate);
        fiveYearsRate->setValue(0.0460);

        std::cout << dblrule << std::endl;
        std::cout <<  "5-year market swap-rate = "
                  << io::rate(s5yRate->value()) << std::endl;
        std::cout << dblrule << std::endl;

        std::cout << tab << "5-years swap paying "
                  << io::rate(fixedRate) << std::endl;
        std::cout << headers[0] << separator
                  << headers[1] << separator
                  << headers[2] << separator
                  << headers[3] << separator << std::endl;
        std::cout << rule << std::endl;

        // now get the updated results
        forecastingTermStructure.linkTo(depoSwapTermStructure);
        discountingTermStructure.linkTo(depoSwapTermStructure);

        NPV = spot5YearSwap.NPV();
        fairSpread = spot5YearSwap.fairSpread();
        fairRate = spot5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        QL_REQUIRE(std::fabs(fairRate-s5yRate->value())<1e-8,
                   "5-years swap mispriced!");

        forecastingTermStructure.linkTo(depoFutSwapTermStructure);
        discountingTermStructure.linkTo(depoFutSwapTermStructure);

        NPV = spot5YearSwap.NPV();
        fairSpread = spot5YearSwap.fairSpread();
        fairRate = spot5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-fut-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        QL_REQUIRE(std::fabs(fairRate-s5yRate->value())<1e-8,
                   "5-years swap mispriced!");

        forecastingTermStructure.linkTo(depoFRASwapTermStructure);
        discountingTermStructure.linkTo(depoFRASwapTermStructure);

        NPV = spot5YearSwap.NPV();
        fairSpread = spot5YearSwap.fairSpread();
        fairRate = spot5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-FRA-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        QL_REQUIRE(std::fabs(fairRate-s5yRate->value())<1e-8,
                   "5-years swap mispriced!");

        std::cout << rule << std::endl;

        // the 1Y forward 5Y swap changes as well

        std::cout << tab << "5-years, 1-year forward swap paying "
                  << io::rate(fixedRate) << std::endl;
        std::cout << headers[0] << separator
                  << headers[1] << separator
                  << headers[2] << separator
                  << headers[3] << separator << std::endl;
        std::cout << rule << std::endl;

        forecastingTermStructure.linkTo(depoSwapTermStructure);
        discountingTermStructure.linkTo(depoSwapTermStructure);

        NPV = oneYearForward5YearSwap.NPV();
        fairSpread = oneYearForward5YearSwap.fairSpread();
        fairRate = oneYearForward5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        forecastingTermStructure.linkTo(depoFutSwapTermStructure);
        discountingTermStructure.linkTo(depoFutSwapTermStructure);

        NPV = oneYearForward5YearSwap.NPV();
        fairSpread = oneYearForward5YearSwap.fairSpread();
        fairRate = oneYearForward5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-fut-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        forecastingTermStructure.linkTo(depoFRASwapTermStructure);
        discountingTermStructure.linkTo(depoFRASwapTermStructure);

        NPV = oneYearForward5YearSwap.NPV();
        fairSpread = oneYearForward5YearSwap.fairSpread();
        fairRate = oneYearForward5YearSwap.fairRate();

        std::cout << std::setw(headers[0].size())
                  << "depo-FRA-swap" << separator;
        std::cout << std::setw(headers[1].size())
                  << std::fixed << std::setprecision(2) << NPV << separator;
        std::cout << std::setw(headers[2].size())
                  << io::rate(fairSpread) << separator;
        std::cout << std::setw(headers[3].size())
                  << io::rate(fairRate) << separator;
        std::cout << std::endl;

        double seconds = timer.elapsed();
        Integer hours = int(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = int(seconds/60);
        seconds -= minutes * 60;
        std::cout << " \nRun completed in ";
        if (hours > 0)
            std::cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            std::cout << minutes << " m ";
        std::cout << std::fixed << std::setprecision(0)
                  << seconds << " s\n" << std::endl;

        return 0;

    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    } catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/constexpr/Numeric.h"

#include <iostream>

using namespace Scine::Molassembler;

BOOST_AUTO_TEST_CASE(NumericAverageStdDev, *boost::unit_test::label("Temple")) {
  const std::vector<double> values {29, 30, 31, 32, 33};

  BOOST_CHECK(Temple::average(values) == 31);
  BOOST_CHECK(
    std::fabs(Temple::stddev(values) - std::sqrt(2))
    < 1e-10
  );
}


#include "vicon_odom/filter.h"
#include <Eigen/LU>  // For matrix inverse

namespace vicon_odom {

void KalmanFilter::initialize(const State_t &state,
                              const ProcessCov_t &initial_cov,
                              const ProcessCov_t &process_noise,
                              const MeasurementCov_t &meas_noise) {
  x = state;
  P = initial_cov;
  Q = process_noise;
  R = meas_noise;
}

void KalmanFilter::processUpdate(double dt) {
  ProcessCov_t A = ProcessCov_t::Identity();
  A.topRightCorner<3, 3>() = Eigen::Vector3d(dt, dt, dt).asDiagonal();

  x = A * x;
  P = A * P * A.transpose() + Q;
}

void KalmanFilter::measurementUpdate(const Measurement_t &meas, double dt) {
  Eigen::Matrix<double, n_meas, n_states> H;
  H.setZero();
  H(0, 0) = 1;
  H(1, 1) = 1;
  H(2, 2) = 1;

  const Eigen::Matrix<double, n_states, n_meas> K =
      P * H.transpose() * (H * P * H.transpose() + R).inverse();
  const Measurement_t inno = meas - H * x;
  x += K * inno;
  P = (ProcessCov_t::Identity() - K * H) * P;
}

}  // namespace vicon_odom


#include "cpca.hpp"

#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <Eigen/SVD>
#include <chrono>
#include <cmath>
#include <iostream>
#include <utility>

CPCA::CPCA(Eigen::Index const nComponents, bool const standardize)
    : nComponents_(nComponents), standardize_(standardize) {
  initialize();
}

void CPCA::initialize() { components_.resize(0, 0); }

Eigen::MatrixXf
CPCA::fitTransform(Eigen::MatrixXf const &fg, Eigen::MatrixXf const &bg,
                   bool const autoAlphaSelection, float const alpha,
                   float const eta, float const convergenceRatio,
                   unsigned int maxIter, bool const keepReports) {
  fit(fg, bg, autoAlphaSelection, alpha, eta, convergenceRatio, maxIter,
      keepReports);
  return transform(fg_);
}

void CPCA::fit(Eigen::MatrixXf const &fg, Eigen::MatrixXf const &bg,
               bool const autoAlphaSelection, float const alpha,
               float const eta, float const convergenceRatio,
               unsigned int maxIter, bool const keepReports) {
  if (autoAlphaSelection) {
    fitWithBestAlpha(fg, bg, alpha, eta, convergenceRatio, maxIter,
                     keepReports);
  } else {
    fitWithManualAlpha(fg, bg, alpha);
  }
}

void CPCA::fitWithManualAlpha(Eigen::MatrixXf const &fg,
                              Eigen::MatrixXf const &bg, float const alpha) {
  fg_ = fg;
  bg_ = bg;

  Eigen::Index fgSize = fg_.size();
  Eigen::Index bgSize = bg_.size();

  if (fgSize == 0 && bgSize == 0) {
    std::cerr << "Both target and background matrices are empty." << std::endl;
  } else if (fgSize == 0) {
    // the result will be the same with when alpha is +inf
    fg_ = Eigen::MatrixXf::Zero(1, bg_.cols());
  } else if (bgSize == 0) {
    // the result will be the same with ordinary PCA
    bg_ = Eigen::MatrixXf::Zero(1, fg_.cols());
  }

  Eigen::Index nFeaturesFg = fg_.cols();
  Eigen::Index nFeaturesBg = bg_.cols();

  if (nFeaturesFg != nFeaturesBg) {
    std::cerr << "# of features of foregraound and background must be the same."
              << std::endl;
  }

  fg_ = fg_.rowwise() - fg_.colwise().mean();
  bg_ = bg_.rowwise() - bg_.colwise().mean();

  if (standardize_) {
    Eigen::RowVectorXf fgStd = fg_.array().square().colwise().mean().sqrt();
    Eigen::RowVectorXf bgStd = bg_.array().square().colwise().mean().sqrt();

    fg_ = fg_.array().rowwise() / fgStd.array();
    bg_ = bg_.array().rowwise() / bgStd.array();

    // NaN to 0.0f
    fg_ = fg_.unaryExpr([](float v) { return std::isfinite(v) ? v : 0.0f; });
    bg_ = bg_.unaryExpr([](float v) { return std::isfinite(v) ? v : 0.0f; });
  }

  fgCov_ = (fg_.adjoint() * fg_) /
           std::fmax(float(fg_.rows() - 1), std::numeric_limits<float>::min());
  bgCov_ = (bg_.adjoint() * bg_) /
           std::fmax(float(bg_.rows() - 1), std::numeric_limits<float>::min());

  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es(fgCov_ - alpha * bgCov_);
  components_ = es.eigenvectors().rightCols(nComponents_).rowwise().reverse();
  eigenvalues_ = es.eigenvalues().real().tail(nComponents_).reverse();
  loadings_ = components_.array().rowwise() * eigenvalues_.array().abs().sqrt();
}

void CPCA::fitWithBestAlpha(Eigen::MatrixXf const &fg,
                            Eigen::MatrixXf const &bg, float const initAlpha,
                            float const eta, float const convergenceRatio,
                            unsigned int const maxIter,
                            bool const keepReports) {
  bestAlpha(fg, bg, initAlpha, eta, convergenceRatio, maxIter, keepReports);
  // updateComponents(bestAlpha_);
  fitWithManualAlpha(fg, bg, bestAlpha_);
}

void CPCA::updateComponents(float const alpha) {
  if (components_.cols() == 0) {
    std::cerr << "Run fit() at least once before updateComponents()"
              << std::endl;
  }
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es(fgCov_ - alpha * bgCov_);
  components_ = es.eigenvectors().rightCols(nComponents_).rowwise().reverse();

  eigenvalues_ = es.eigenvalues().real().tail(nComponents_).reverse();
  loadings_ = components_.array().rowwise() * eigenvalues_.array().abs().sqrt();
}

Eigen::MatrixXf CPCA::transform(Eigen::MatrixXf const &X) {
  if (components_.cols() == 0) {
    std::cerr << "Run fit() before transform()" << std::endl;
  }
  return X * components_;
}

float CPCA::bestAlpha(Eigen::MatrixXf const &fg, Eigen::MatrixXf const &bg,
                      float const initAlpha, float const eta,
                      float const convergenceRatio, unsigned int const maxIter,
                      bool const keepReports) {
  reports_.clear();
  float alpha = initAlpha;
  fit(fg, bg, alpha);

  // method 1. discard minor eigenvectors to avoid singular
  // Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> esQ(bgCov_);
  // float ratioToKeep = 0.999f;
  // Eigen::RowVectorXf eigenvalues = esQ.eigenvalues().real().reverse();
  // float targetTotalEigenVal = eigenvalues.sum() * ratioToKeep;
  // Eigen::Index nEigenVectorsToKeep = 0;
  // while (targetTotalEigenVal > 0) {
  //     targetTotalEigenVal -= eigenvalues(nEigenVectorsToKeep);
  //     nEigenVectorsToKeep++;
  // }
  // float ratioToKeep = 0.9f;
  // Eigen::Index nEigenVectorsToKeep = Eigen::Index(bg.cols() * ratioToKeep);
  //
  // std::cout << fg.cols() << " " << nEigenVectorsToKeep << std::endl;
  //
  // Eigen::MatrixXf Q =
  //     esQ.eigenvectors().rightCols(nEigenVectorsToKeep).rowwise().reverse();
  // Eigen::MatrixXf fgCovQ = Q.adjoint() * fgCov_ * Q;
  // Eigen::MatrixXf bgCovQ = Q.adjoint() * bgCov_ * Q;
  //
  // Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> esU(fgCovQ - alpha *
  // bgCovQ); Eigen::MatrixXf U =
  //     esU.eigenvectors().rightCols(nComponents_).rowwise().reverse();
  //
  // if (keepReports) {
  //   reports_.push_back(alpha);
  // }
  //
  // for (unsigned int i = 0; i < maxIter; ++i) {
  //   float fgTr = (U.adjoint() * fgCovQ * U).trace();
  //   float bgTr = (U.adjoint() * bgCovQ * U).trace();
  //   bgTr = std::fmax(bgTr, std::numeric_limits<float>::min());
  //   alpha = fgTr / bgTr;
  //
  //   // update U
  //   Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> esU(fgCovQ - alpha *
  //   bgCovQ); U =
  //   esU.eigenvectors().rightCols(nComponents_).rowwise().reverse();
  //
  //   if (keepReports) {
  //     reports_.push_back(alpha);
  //   }
  // }
  // bestAlpha_ = alpha;

  // method 2: add small constant to diag of bgCov_ to avoid singular
  bgCov_ += Eigen::MatrixXf::Identity(bgCov_.rows(), bgCov_.cols()) * eta;

  if (keepReports) {
    reports_.push_back(alpha);
  }

  for (unsigned int i = 0; i < maxIter; ++i) {
    float fgTr = (components_.adjoint() * fgCov_ * components_).trace();
    float bgTr = (components_.adjoint() * bgCov_ * components_).trace();
    bgTr = std::fmax(bgTr, std::numeric_limits<float>::min());

    float prevAlpha = alpha;
    alpha = fgTr / bgTr;
    updateComponents(alpha);
    if (keepReports) {
      reports_.push_back(alpha);
    }

    if (std::abs(prevAlpha - alpha) / alpha < convergenceRatio)
      break;
  }
  bestAlpha_ = alpha;

  return bestAlpha_;
}

std::vector<float> CPCA::logspace(float const start, float const end,
                                  unsigned int const num, float const base) {
  float realStart = std::pow(base, start);
  float realBase = std::pow(
      base, (end - start) /
                std::fmax(float(num - 1), std::numeric_limits<float>::min()));

  std::vector<float> result;
  result.reserve(num);
  std::generate_n(
      std::back_inserter(result), num, [=]() mutable throw()->float {
        float val = realStart;
        realStart *= realBase;
        return val;
      });
  return result;
}

// TODO: implement semi-automatic selection of alpha of the original cpca
// (but not necessary for ccPCA)

std::vector<float> CPCA::findSpectalAlphas(unsigned int const nAlphasToReturn,
                                           unsigned int const nAlphas,
                                           float const maxLogAlpha) {
  std::vector<float> alphas;
  Eigen::MatrixXf affinityMat = createAffinityMatrix(fg_, nAlphas, maxLogAlpha);

  // TODO: implement rest of here

  return alphas;
}

Eigen::MatrixXf CPCA::createAffinityMatrix(Eigen::MatrixXf const &X,
                                           unsigned int const nAlphas,
                                           float const maxLogAlpha) {
  std::vector<float> alphas;
  alphas.reserve(nAlphas + 1);
  alphas.push_back(0.0f);

  auto logspaceAlphas = logspace(-1.0f, maxLogAlpha, nAlphas);
  alphas.insert(alphas.end(), logspaceAlphas.begin(), logspaceAlphas.end());

  auto k = alphas.size();
  Eigen::MatrixXf affinityMat =
      0.5 * Eigen::MatrixXf::Identity(Eigen::Index(k), Eigen::Index(k));

  std::vector<Eigen::MatrixXf> subspaces;
  subspaces.reserve(k);
  for (auto const &alpha : alphas) {
    updateComponents(alpha);
    Eigen::MatrixXf proj = transform(X);
    Eigen::HouseholderQR<Eigen::MatrixXf> qr(proj);
    Eigen::MatrixXf Q = qr.householderQ();
    subspaces.push_back(qr.householderQ());
  }

  for (size_t i = 0; i < k; ++i) {
    for (size_t j = i + 1; j < k; ++j) {
      Eigen::BDCSVD<Eigen::MatrixXf> svd(subspaces[i] * subspaces[j],
                                         Eigen::ComputeThinU |
                                             Eigen::ComputeThinV);
      Eigen::VectorXf s = svd.singularValues();
      affinityMat(Eigen::Index(i), Eigen::Index(j)) = s(0) * s(1);
    }
  }

  affinityMat = affinityMat + affinityMat.transpose();
  // NaN to 0.0f
  affinityMat = affinityMat.unaryExpr(
      [](float v) { return std::isfinite(v) ? v : 0.0f; });

  return affinityMat;
}


//
// SPDX-License-Identifier: MIT
// Copyright (c) 2016-2020 Michael Purcaro, Henry Pratt, Jill Moore, Zhiping Weng
//

#include <vector>
#include <string>
#include <unordered_map>

#include <armadillo>
#include <boost/filesystem.hpp>

#include "utils.hpp"
#include "lambda.hpp"
#include "region.hpp"
#include "rDHS.hpp"
#include "binarysignal.hpp"
#include "correlation.hpp"

namespace SCREEN {

  namespace bfs = boost::filesystem;
  namespace a = arma;

  /**
     compute the pairwise correlation of the regions for the given chromosome
     @returns: armadillo matrix containing pairwise correlation coefficients for the regions
   */
  a::Mat<float> runCorrelation(std::vector<ScoredRegionSet> &regions, const std::string &chr) {

    // create emptry matrix
    if (0 == regions.size()) return a::Mat<float>(0, 0);
    a::Mat<float> values(regions.size(), regions[0].regions_.regions_[chr].size());

    // populate with region scores
    for (auto i = 0; i < regions.size(); ++i) {
#pragma omp parallel for
      for (auto j = 0; j < regions[i].regions_.regions_[chr].size(); ++j) {
	values.at(i, j) = regions[i].regions_.regions_[chr][j].score;
      }
    }

    // return pairwise correlation
    return a::cor(values);

  }

  a::Mat<float> runCorrelation(BinarySignal &b, const std::vector<boost::filesystem::path> &signalfiles,
			       const std::string &chr, RegionSet &regions) {

    // create empty matrix
    if (0 == signalfiles.size()) return a::Mat<float>(0, 0);
    a::Mat<float> values(signalfiles.size(), regions.regions_.regions_[chr].size());

    // populate with region scores
    for (auto i = 0; i < signalfiles.size(); ++i) {
      a::Col<float> v = b.readSignal<RegionSet>(signalfiles[i], chr, regions);
#pragma omp parallel for
      for (auto j = 0; j < regions.regions_.regions_[chr].size(); ++j) {
	values.at(i, j) = v.at(j);
      }
    }

    // return pairwise correlation
    return a::cor(values);

  }

  /**
     write a computed pairwise correlation in JSON 2D array format to an output file
     @param corr: matrix containing the correlation coefficients
     @param output_path: output path to which to write the matrix
   */
  template <typename T>
  void writeCorrelation(const a::Mat<T> &corr, const bfs::path &output_path) {
    std::ofstream f(output_path.string());
    long i, j;
    f << '[';
    for (i = 0; i < corr.n_rows - 1; ++i) {
      for (j = 0; j < corr.n_cols - 1; ++j) {
	f << corr.at(i, j) << ',';
      }
      f << corr.at(i, j) << "],[";
    }
    for (j = 0; j < corr.n_cols - 1; ++j) {
      f << corr.at(i, j) << ',';
    }
    f << corr.at(i, j) << ']';
  }

  template void writeCorrelation<float>(const a::Mat<float> &corr, const bfs::path &output_path);
  template void writeCorrelation<double>(const a::Mat<double> &corr, const bfs::path &output_path);

} // SCREEN