#ifndef SHASTA_APPROXIMATE_COLORING_HPP
#define SHASTA_APPROXIMATE_COLORING_HPP

/*******************************************************************************

Approximate coloring of a Boost undirected graph using the Dsatur algorithm.

See this Wikipedia article for greedy coloring in general:
https://en.wikipedia.org/wiki/Greedy_coloring

This code uses the Brelaz (1979) Dsatur
algorithm described there in this section
https://en.wikipedia.org/wiki/Greedy_coloring#Adaptive

The main Wikipedia article for this algorithm is here:
https://en.wikipedia.org/wiki/DSatur

The Dsatur algorithm was ingtroduced in this paper:
Brelaz, Daniel (April 1979),
"New methods to color the vertices of a graph",
Communications of the ACM, 22 (4): 251–256, doi:10.1145/359094.359101

Many alternative approximate coloring methods are also available, for example
https://www.gerad.ca/~alainh/RLFPaper.pdf

For a partial coloring of a graph, the Dsatur algorithm defines
the saturation degree of a vertex as the number of distinct colors
to which the vertex is adjacent (only counting, of course,
vertices that have already been colored).
With this definition, the Dsatur algorithm is simply described
as follows:

- Starting with the uncolored graph, iterate over vertices in the order
  defined below.
- At each iteration, assign to each vertex the lowest possible color.
  This is the lowest color that the vertex is not adjacent to.
- At each iteration choose the vertex with maximum saturation degree
  (maximum number of adjacent colors). In case of ties, break
  the ties by selecting the vertex with maximum degree in the uncolored
  subgraph - that is, the vertex with the greatest number of uncolored
  adjacent vertices.

The DSatur algorithm is an approximate coloring algorithm, but it
is exact for 2-colorable graphs.

This function applies the above algorithm to a Boost undirected graph.
It assumes that the graph is defined as a
boost::adjacency_list<
    OutEdgeList,
    VertexList,
    Directed,
    VertexProperties, EdgeProperties, GraphProperties,
    EdgeList>
with:

- VertexList = boost::vecS (so vertex descriptors are integers starting at zero).
- Directed = boost::undirectedS.

It returns the computed coloring in its second argument, indexed
by vertex_descriptor.

*******************************************************************************/

// Shasta.
#include "SHASTA_ASSERT.hpp"

// Boost libraries.
#include <boost/container/flat_set.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/iteration_macros.hpp>

// Standard library.
#include "algorithm.hpp"
#include "cstdint.hpp"
#include <map>
#include <numeric>
#include "vector.hpp"

namespace shasta {
    template<class Graph> void approximateColoring(
        const Graph&,
        vector<uint64_t>&
    );
}

template<class Graph> void shasta::approximateColoring(
    const Graph& graph,
    vector<uint64_t>& color
)
{
    // Get the number of vertices and initialize the colors.
    const uint64_t n = boost::num_vertices(graph);
    color.resize(n);
    const uint64_t noColor = std::numeric_limits<uint64_t>::max();
    fill(color.begin(), color.end(), noColor);

    // Vector to contain the number of colored vertices adjacent to
    // each uncolored vertex. This is used to break ties in the Dsatur algorithm.
    // We only maintain this for uncolored vertices.
    // Once a vertex gets colored we stop updating this as it will never be needed.
    vector<uint64_t> adjacentColoredCount(n, 0);

    // The colors adjacent to each uncolored vertex. The number of colors adjacent
    // to each vertex is small, so we use boost::container::flat_set
    // instead of std::set.
    // We only maintain this for uncolored vertices.
    // Once a vertex gets colored we stop updating this as it will never be needed.
    using boost::container::flat_set;
    vector< flat_set<uint64_t> > adjacentColors(n);

    // A multimap that stores uncolored vertices
    // keyed by saturation degree (that is, the number of colors adjacent to
    // each vertex), in decreasing order.
    // This is used to select, at each iteration, the next vertex to be colored.
    using MultiMap = std::multimap<uint64_t, uint64_t, std::greater<uint64_t> >;
    MultiMap verticesBySaturationDegree;
    for(uint64_t v=0; v<n; v++) {
        verticesBySaturationDegree.insert(make_pair(0, v));
    }

    // Main iteration loop. At each iteration we color one vertex.
    for(uint64_t iteration=0; iteration<n; iteration++) {

        // To choose the vertex to color at this iteration, loop over
        // all uncolored vertices with maximum saturation degree.
        SHASTA_ASSERT(not verticesBySaturationDegree.empty());
        const uint64_t highestSaturationDegree = verticesBySaturationDegree.begin()->first;
        MultiMap::iterator begin, end;
        tie(begin, end) = verticesBySaturationDegree.equal_range(highestSaturationDegree);
        SHASTA_ASSERT(begin != end);
        uint64_t v0 = begin->second;
        uint64_t adjacentColoredCount0 = adjacentColoredCount[v0];
        MultiMap::iterator it0 = begin;
        for(MultiMap::iterator it=begin; it!=end; ++it) {
            const uint64_t v1 = it->second;
            if(adjacentColoredCount[v1] > adjacentColoredCount0) {
                v0 = v1;
                adjacentColoredCount0 = adjacentColoredCount[v1];
                it0 = it;
            }
        }

        // Remove v0 from verticesBySaturationDegree.
        verticesBySaturationDegree.erase(it0);

        // This iteration is coloring LocalVertexId v0.
        // We choose the lowest color that is not present in adjacentColors[v0].
        const auto& adjacentColors0 = adjacentColors[v0];
        uint64_t color0;
        for(uint64_t color=0; ; color++) {
            if(adjacentColors0.find(color) == adjacentColors0.end()) {
                color0 = color;
                break;
            }
        }

        // Color v0 with the color chosen in this way.
        color[v0] = color0;

        // Increment adjacentColoredCount for adjacent uncolored vertices.
        BGL_FORALL_ADJ_T(v0, v1, graph, Graph) {
            if(color[v1] == noColor) {
                adjacentColoredCount[v1]++;
            }
        }

        // Update adjacentColors for adjacent uncolored vertices, and
        // update verticesBySaturationDegree accordingly.
        BGL_FORALL_ADJ_T(v0, v1, graph, Graph) {
            if(color[v1] == noColor) {
                auto& adjacentColors1 = adjacentColors[v1];
                if(adjacentColors1.find(color0) == adjacentColors1.end()) {
                    adjacentColors1.insert(color0);

                    // The saturation degree of this vertex has increased.
                    MultiMap::iterator begin, end;
                    tie(begin, end) = verticesBySaturationDegree.equal_range(adjacentColors1.size()-1);
                    SHASTA_ASSERT(begin != end);
                    bool done = false;
                    for(MultiMap::iterator it=begin; it!=end; ++it) {
                        if(it->second == v1) {
                            verticesBySaturationDegree.erase(it);
                            verticesBySaturationDegree.insert(make_pair(adjacentColors1.size(), v1));
                            done = true;
                            break;
                        }
                    }
                    SHASTA_ASSERT(done);
                }
            }
        }
    }

    // Verify that we colored all the vertices.
    SHASTA_ASSERT(find(color.begin(), color.end(), noColor) == color.end());

}

#endif


#include <iostream>
#include <vector>
#include <string>

#include <Eigen/Core>

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

void getMeanCurvature(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
                      Eigen::VectorXd &VCs) {
  // Get the laplacian
  Eigen::SparseMatrix<double> L,M,Minv;
  //   from the cotangent matrix
  igl::cotmatrix(V,F, L);
  //   and the mass matrix
  igl::massmatrix(V,F, igl::MASSMATRIX_TYPE_VORONOI, M);
  igl::invert_diag(M, Minv);

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

  // Smooth this out a bit.
  //igl::per_vertex_attribute_smoothing(VC,F, VCs);
  VCs = VC;
  Eigen::VectorXd VC_before = VCs;
  VCs = VCs.cwiseMin(0.1);

  // Normalize all the values.
  VCs /= VCs.maxCoeff();
  VCs = VCs.cwiseSqrt();
  Eigen::VectorXd ones = Eigen::VectorXd::Constant(VCs.rows(), 1, 1);
  VCs = ones - VCs;
}

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

  Eigen::MatrixXd V;
  Eigen::MatrixXi F;

  printf("Reading in mesh...\n");
  // Read in the input file.
  igl::readOFF(argv[1], V, F);
  // Need the adjacency list
  std::vector<std::vector<int> > VV;
  igl::adjacency_list(F, VV);
  // Also caluculate the curvature values.
  printf("Calucalting curvature...\n");
  std::vector<std::vector<double> > curveWeight;
  Eigen::VectorXd mean_curve;

  Eigen::MatrixXd PD1,PD2;
  Eigen::VectorXd PV1,PV2;
  igl::principal_curvature(V,F,PD1,PD2,PV1,PV2, 5);
  //mean_curve = 0.5 * (PV1 + PV2);
  mean_curve = PV1.cwiseProduct(PV1) + PV2.cwiseProduct(PV2);
  mean_curve = mean_curve.cwiseMin(0.1);
  mean_curve.cwiseSqrt();
  std::cout << mean_curve << std::endl;
  //getMeanCurvature(V,F, mean_curve);

  igl::viewer::Viewer viewer;
  viewer.data.set_mesh(V, F);
  Eigen::MatrixXd this_col;
  igl::jet(mean_curve, true, this_col);
  viewer.data.set_colors(this_col);
  viewer.core.show_lines = false;
  viewer.launch();
}


//
// Created by dchansen on 6/16/18.
//
#include <numeric>
#include <boost/math/constants/constants.hpp>
#include "correct_frequency_shift.h"
#include "hoNDFFT.h"
#include "hoNDArray_elemwise.h"
namespace Gadgetron{
    namespace FatWater {

        using namespace std::complex_literals;
        static constexpr float PI = boost::math::constants::pi<float>();

        void correct_frequency_shift(hoNDArray<std::complex<float>> &species_images, const Parameters &parameters){

            uint16_t X = species_images.get_size(0);
            uint16_t Y = species_images.get_size(1);
            uint16_t Z = species_images.get_size(2);
            uint16_t CHA = species_images.get_size(3);
            uint16_t N = species_images.get_size(4);
            uint16_t S = species_images.get_size(5);
            uint16_t LOC = species_images.get_size(6);

            std::vector<size_t> sub_dimension = {X,Y,Z,CHA,N,1,1};
            auto data_ptr = species_images.get_data_ptr();

            hoNDFFT<float>::instance()->fft(&species_images,0);

            for (int kspecies = 0; kspecies < parameters.species.size(); kspecies++){

                auto& species = parameters.species[kspecies];

                auto mean_frequency_offset = std::accumulate(
                        species.amplitude_frequency_pairs.begin(), species.amplitude_frequency_pairs.end(),0.0f,
                                              [](auto v, auto pair){ return v+pair.first.real()*pair.second;}
                                              ) /
                              std::accumulate(
                        species.amplitude_frequency_pairs.begin(),species.amplitude_frequency_pairs.end(), 0.0f,
                                            [](auto v, auto pair) { return v + pair.first.real();});
                mean_frequency_offset *= parameters.field_strength_T*parameters.gyromagnetic_ratio_Mhz;

                if (std::abs(mean_frequency_offset) < 1.0 ) continue;

                hoNDArray<std::complex<float>> phase_ramp(X);

                for (int i = 0; i < phase_ramp.get_number_of_elements(); i++){
                    phase_ramp[i] = std::exp(2if*PI*parameters.sample_time_us*1e-6f*float(i)*mean_frequency_offset);
                }

                //Kristoffer will have a field day with this.
                for (int kL = 0; kL < LOC; kL++)
                    for (int kN = 0; kN < N; kN++)
                        for (int kCHA = 0; kCHA < CHA; kCHA++)
                            for (int kZ = 0; kZ < Z; kZ++)
                                for (int kY = 0; kY < Y; kY++)
                                    for (int kX = 0; kX < Y; kX++)
                                        species_images(kX,kY,kZ,kCHA,kN,kspecies,kL) *= phase_ramp[kX];

            }
//
            hoNDFFT<float>::instance()->ifft(&species_images,0);

        }
    }
}


//#include "test_cases.h"
//#include <iostream>
//#include "Mesh.h"
//#include "vector_var.h"
//#include <stdio.h>      /* printf */
//#include <iostream>
//#include <math.h>
//#include "Boundary_Conditions.h"
//#include "Solution.h"
//#include "Solver.h"
//#include "quad_bcs.h"
//#include "external_forces.h"
//#include "global_variables.h"
//#include <algorithm>
//#include <string>
//#include <sstream>
//#include <boost/algorithm/string/replace.hpp>
//#include <boost/filesystem.hpp>
//#include <cstdio>
//#include <ctime>
//
//using namespace std;
//using namespace boost::filesystem;
//
//test_cases::test_cases()
//{
//    //ctor
//}
//
//test_cases::~test_cases()
//{
//    //dtor
//}
//
//void test_cases::west_to_east_poiseuille_flow(){
//
//    double X,Y,dx,dy,dt; // dt is streaming time step
//    double kine_viscosity,tau;
//    double reynolds, umax;
//    double simulation_length;
//    double delta_t; // time stepping step
//    quad_bcs_plus bcs;
//    double cs;
//    double pressure_grad;
//    global_variables globals;
//    std::string output_file;
//    double average_pressure;
//    double pre_condition_gamma;
//    std::clock_t start;
//    double duration;
//
//    pre_condition_gamma = 1;
//    //average_pressure =1*3*pre_condition_gamma;
//    average_pressure =1*3;
//
//    //vector_var pressure_gradient(-0.01,0,0), origin(1.1,0,0), origin_loc(0,0,0) ;
//    vector_var pressure_gradient(0,0,0), origin(average_pressure,0,0), origin_loc(0,0,0) ;
//
//    /// Parameters unique to test case
//
//    X= 16;
//    Y= 4.4;
//    dx= 0.4; // grid spacing
//    dy = 0.4;  // grid spacing
//    dt = 0.2;  // streaming time step -> dictates mach number -> grid spacing /2
//
//
//    /// Error :: let dt = l_dx = l_dy i.e. lattice spacing
//
//    simulation_length = 5000;
//    //kine_viscosity = U * X/ reynolds;
//    kine_viscosity = 0.0833333;
//
//    delta_t = 0.3;  // time marching step
//    cs = 1/sqrt(3);
//
//    reynolds = 33.7094;
//    umax = 0.17557;
//    // tau = 0.5 + Umax* Length / reynolds/dt  --- assumes c = 1.
//
//    tau = 0.5 + umax*X /reynolds /dt;
//    tau = 0.5 + umax*X /reynolds /dt *pre_condition_gamma;
//    //tau = 1/pre_condition_gammma*(tau-0.5) + 0.5;
//
//
//
//    pressure_grad =0;
//
//    //tau =0.75
//    output_file = create_output_directory(globals.tolerance,dx,delta_t);
//
//
//    // set boundary conditions for this test case
//    //bcs.w_rho = 1.1*3*pre_condition_gammma;
//    bcs.w_rho = 1.1*3*pre_condition_gamma;
//    bcs.w_u = 0;
//    bcs.w_v = 0;
//    bcs.w_w = 0;
//    bcs.w_type_vel = globals.neumann;
//    bcs.w_type_rho = globals.dirichlet;
//
//    bcs.s_rho = 0;
//    bcs.s_u = 0;
//    bcs.s_v = 0;
//    bcs.s_w = 0;
//    bcs.s_type_vel= globals.dirichlet;
//    bcs.s_type_rho = globals.neumann;
//
//    //bcs.e_rho = 1*3*pre_condition_gammma;
//    bcs.e_rho = 1*3*pre_condition_gamma;
//    bcs.e_u = 0;
//    bcs.e_v = 0;
//    bcs.e_w = 0;
//    bcs.e_type_vel= globals.neumann;
//    bcs.e_type_rho = globals.dirichlet;
//
//    bcs.n_rho = 0;
//    bcs.n_u = 0;
//    bcs.n_v = 0;
//    bcs.n_w = 0;
//    bcs.n_type_vel= globals.dirichlet;
//    bcs.n_type_rho = globals.neumann;
//
//
//
//
//
//    /// Methods to run the test case
//    //vector_var_tests();
//
//    // create Mesh
//    Mesh mesh(X,Y,dx,dy);
//
//    // create boundary conditions
//    Boundary_Conditions bc(mesh.get_num_x(), mesh.get_num_y());
//    bc.assign_boundary_conditions(mesh.get_num_x(), mesh.get_num_y(),bcs);
//
//    // assign external force terms
//    external_forces source_term(mesh.get_total_nodes());
//    source_term.set_uniform_force(pressure_grad);
//
//    //create solution
//    Solution soln(mesh.get_total_nodes());
//    soln.assign_pressure_gradient(pressure_gradient,origin_loc,origin,mesh);
//    soln.set_average_rho(average_pressure);
//    // Solvec
//
//    Solver solve;
//
//    solve.Mesh_Solver(dt,tau,mesh,soln,bc,simulation_length, delta_t,dx ,output_file,source_term,
//                              pre_condition_gamma);
//
//    soln.post_process(pre_condition_gamma);
//    soln.output(output_file);
//    tau = 1;
//
//}
//
//
//
//void test_cases::lid_driven_cavity_N(){
//
//    double X,Y,dx,dy,dt; // dt is streaming time step
//    double reynolds,kine_viscosity,tau;
//    double U;
//
//
//    quad_bcs bcs;
//    double cs,delta_t;
//
//    /// Parameters unique to test case
//    reynolds = 1000;
//    X= 100;
//    Y=100;
//    dx=1; // grid spacing
//    dy = 1;  // grid spacing
//    dt = 0.05;  // streaming time step let dt =dx = dy i.e. lattice spacing
//    U = 1;
//    simulation_length = 200;
//    kine_viscosity = U * X/ reynolds;
//    //kine_viscosity = 20;
//    delta_t = 0.1;
//    cs = 1/sqrt(3);
//    tau = kine_viscosity + 0.5* pow(cs,2) *dt;
//    //tau =0.75;
//
//
//    // set boundary conditions for this test case
//    bcs.w_rho = 1;
//    bcs.w_u = 0;
//    bcs.w_v = 0;
//    bcs.w_w = 0;
//
//    bcs.s_rho = 1;
//    bcs.s_u = 0;
//    bcs.s_v = 0;
//    bcs.s_w = 0;
//
//    bcs.e_rho = 1;
//    bcs.e_u = 0;
//    bcs.e_v = 0;
//    bcs.e_w = 0;
//
//    bcs.n_rho = 1;
//    bcs.n_u = 1;
//    bcs.n_v = 0;
//    bcs.n_w = 0;
//
//    /// Methods to run the test case
//    //vector_var_tests();
//
//    // create Mesh
//    Mesh mesh(X,Y,dx,dy);
//
//    // create boundary conditions
//    Boundary_Conditions bc(mesh.get_num_x(), mesh.get_num_y());
//    bc.assign_boundary_conditions(mesh.get_num_x(), mesh.get_num_y(),bcs);
//
//
//    //create solution
//    Solution soln(mesh.get_total_nodes());
//
//    // Solve
//
//    Solver solve;
//    //solve.Mesh_Solver(dt,tau,mesh,soln,bc,simulation_length, delta_t, dx,tolerance);
//
//    tau = 1;
//
//}
//void test_cases::west_to_east_couette_flow(){
//
//    double X,Y,dx,dy,dt; // dt is streaming time step
//    double kine_viscosity,tau;
//    double U;
//    double simulation_length;
//    double delta_t; // time stepping step
//    quad_bcs bcs;
//    double cs;
//    double pre_conditioned_gamma;
//    pre_conditioned_gamma = 1.0;
//    std::string output_file;
//
//    /// Parameters unique to test case
//
//    X= 0.3;
//    Y=1;
//    dx=0.1; // grid spacing
//    dy = 0.1;  // grid spacing
//    dt = 0.05;  // streaming time step -> dictates mach number -> grid spacing /2
//    /// Error :: let dt =dx = dy i.e. lattice spacing
//    U = 2;
//    simulation_length = 2500;
//    //kine_viscosity = U * X/ reynolds;
//    kine_viscosity = 0.6;
//
//    delta_t = 0.1;  // time marching step
//    cs = 1/sqrt(3);
//    tau = kine_viscosity + 0.5* pow(cs,2) *dt;
//
//
//    //output_file = "/home/brendan/Dropbox/PhD/Test Cases/Couette Flow/";
//
//    output_file = "C:/Users/brendan/Dropbox/PhD/Test Cases/Couette Flow/";
//    //tau =0.75;
//
//    // set boundary conditions for this test case
//    bcs.w_rho = 1;
//    bcs.w_u = 1;
//    bcs.w_v = 0;
//    bcs.w_w = 0;
//    bcs.w_type = 3;
//
//    bcs.s_rho = 1;
//    bcs.s_u = 0;
//    bcs.s_v = 0;
//    bcs.s_w = 0;
//    bcs.s_type = 1;
//
//    bcs.e_rho = 0;
//    bcs.e_u = 0;
//    bcs.e_v = 0;
//    bcs.e_w = 0;
//    bcs.e_type =2;
//
//    bcs.n_rho = 1;
//    bcs.n_u = U;
//    bcs.n_v = 0;
//    bcs.n_w = 0;
//    bcs.n_type = 1;
//
//
//
//    /// Methods to run the test case
//    //vector_var_tests();
//
//    // create Mesh
//    Mesh mesh(X,Y,dx,dy);
//
//    // create boundary conditions
//    Boundary_Conditions bc(mesh.get_num_x(), mesh.get_num_y());
//    bc.assign_boundary_conditions(mesh.get_num_x(), mesh.get_num_y(),bcs);
//
//    // assign external force terms
//    external_forces source_term(mesh.get_total_nodes());
//    source_term.set_uniform_force(0.0);
//
//
//    //create solution
//    Solution soln(mesh.get_total_nodes());
//    soln.set_average_rho(1.0);
//
//    // Solve
//
//    Solver solve;
//    solve.Mesh_Solver(dt,tau,mesh,soln,bc,simulation_length, delta_t,dx ,output_file, source_term,pre_conditioned_gamma);
//    soln.output(output_file);
//    tau = 1;
//
//}
//
//void test_cases::east_to_west_couette_flow(){
//
//    double X,Y,dx,dy,dt; // dt is streaming time step
//    double kine_viscosity,tau;
//    double U;
//    double simulation_length;
//    double delta_t; // time stepping step
//    quad_bcs bcs;
//    double cs;
//
//    double pre_conditioned_gamma;
//    pre_conditioned_gamma = 1.0;
//
//    std::string output_file;
//     output_file = "/home/brendan/Dropbox/PhD/Test Cases/Couette Flow/";
//    /// Parameters unique to test case
//
//    X= 0.3;
//    Y=1;
//    dx=0.1; // grid spacing
//    dy = 0.1;  // grid spacing
//    dt = 0.05;  // streaming time step -> dictates mach number -> grid spacing /2
//    /// Error :: let dt =dx = dy i.e. lattice spacing
//    U = -2;
//    simulation_length = 2500;
//    //kine_viscosity = U * X/ reynolds;
//    kine_viscosity = 0.6;
//
//    delta_t = 0.1;  // time marching step
//    cs = 1/sqrt(3);
//    tau = kine_viscosity + 0.5* pow(cs,2) *dt;
//    //tau =0.75;
//
//
//    output_file = "/home/brendan/Dropbox/PhD/Test Cases/Couette Flow/";
//
//    // set boundary conditions for this test case
//    bcs.w_rho = 0;
//    bcs.w_u = 0;
//    bcs.w_v = 0;
//    bcs.w_w = 0;
//    bcs.w_type = 2;
//
//    bcs.s_rho = 1;
//    bcs.s_u = 0;
//    bcs.s_v = 0;
//    bcs.s_w = 0;
//    bcs.s_type = 1;
//
//    bcs.e_rho = 1;
//    bcs.e_u = 0;
//    bcs.e_v = 0;
//    bcs.e_w = 0;
//    bcs.e_type =3;
//
//    bcs.n_rho = 1;
//    bcs.n_u = U;
//    bcs.n_v = 0;
//    bcs.n_w = 0;
//    bcs.n_type = 1;
//
//
//
//    /// Methods to run the test case
//    //vector_var_tests();
//
//    // create Mesh
//    Mesh mesh(X,Y,dx,dy);
//
//    // create boundary conditions
//    Boundary_Conditions bc(mesh.get_num_x(), mesh.get_num_y());
//    bc.assign_boundary_conditions(mesh.get_num_x(), mesh.get_num_y(),bcs);
//
//    // assign external force terms
//    external_forces source_term(mesh.get_total_nodes());
//    source_term.set_uniform_force(0.0);
//
//    //create solution
//    Solution soln(mesh.get_total_nodes());
//
//    // Solve
//
//
//    Solver solve;
//    solve.Mesh_Solver(dt,tau,mesh,soln,bc,simulation_length, delta_t,dx, output_file,source_term,pre_conditioned_gamma);
//    soln.output(output_file);
//
//    tau = 1;
//
//}
//void test_cases::north_to_south_couette_flow(){
//
//    double X,Y,dx,dy,dt; // dt is streaming time step
//    double kine_viscosity,tau;
//    double U;
//    double simulation_length;
//    double delta_t; // time stepping step
//    quad_bcs bcs;
//    double cs;
//    std::string output_file;
//
//    double pre_conditioned_gamma;
//    pre_conditioned_gamma = 1.0;
//     output_file = "/home/brendan/Dropbox/PhD/Test Cases/Couette Flow/";
//
//    /// Parameters unique to test case
//
//    X= 1;
//    Y=0.3;
//    dx=0.1; // grid spacing
//    dy = 0.1;  // grid spacing
//    dt = 0.05;  // streaming time step -> dictates mach number -> grid spacing /2
//    /// Error :: let dt =dx = dy i.e. lattice spacing
//    U = -2;
//    simulation_length = 2500;
//    //kine_viscosity = U * X/ reynolds;
//    kine_viscosity = 0.6;
//
//    delta_t = 0.1;  // time marching step
//    cs = 1/sqrt(3);
//    tau = kine_viscosity + 0.5* pow(cs,2) *dt;
//    //tau =0.75;
//
//
//
//    // set boundary conditions for this test case
//    bcs.w_rho = 1;
//    bcs.w_u = 0;
//    bcs.w_v = 0;
//    bcs.w_w = 0;
//    bcs.w_type = 1;
//
//    bcs.s_rho = 0;
//    bcs.s_u = 0;
//    bcs.s_v = 0;
//    bcs.s_w = 0;
//    bcs.s_type = 2;
//
//    bcs.e_rho = 1;
//    bcs.e_u = 0;
//    bcs.e_v = U;
//    bcs.e_w = 0;
//    bcs.e_type =1;
//
//    bcs.n_rho = 1;
//    bcs.n_u = 0;
//    bcs.n_v = 0;
//    bcs.n_w = 0;
//    bcs.n_type = 3;
//
//
//
//    /// Methods to run the test case
//    //vector_var_tests();
//
//    // create Mesh
//    Mesh mesh(X,Y,dx,dy);
//
//    // create boundary conditions
//    Boundary_Conditions bc(mesh.get_num_x(), mesh.get_num_y());
//    bc.assign_boundary_conditions(mesh.get_num_x(), mesh.get_num_y(),bcs);
//
//    // assign external force terms
//    external_forces source_term(mesh.get_total_nodes());
//    source_term.set_uniform_force(0.0);
//
//    //create solution
//    Solution soln(mesh.get_total_nodes());
//
//    // Solve
//
//    Solver solve;
//    solve.Mesh_Solver(dt,tau,mesh,soln,bc,simulation_length, delta_t,dx,output_file,source_term,pre_conditioned_gamma);
//
//    tau = 1;
//
//}
//
//void test_cases::south_to_north_couette_flow(){
//
//    double X,Y,dx,dy,dt; // dt is streaming time step
//    double kine_viscosity,tau;
//    double U;
//    double simulation_length;
//    double delta_t; // time stepping step
//    quad_bcs bcs;
//    double cs;
//    std::string output_file;
//
//    double pre_conditioned_gamma;
//    pre_conditioned_gamma = 1.0;
//     output_file = "/home/brendan/Dropbox/PhD/Test Cases/Couette Flow/";
//
//    /// Parameters unique to test case
//
//    X= 1;
//    Y=0.3;
//    dx=0.1; // grid spacing
//    dy = 0.1;  // grid spacing
//    dt = 0.05;  // streaming time step -> dictates mach number -> grid spacing /2
//    /// Error :: let dt =dx = dy i.e. lattice spacing
//    U = 2;
//    simulation_length = 2500;
//    //kine_viscosity = U * X/ reynolds;
//    kine_viscosity = 0.6;
//
//    delta_t = 0.1;  // time marching step
//    cs = 1/sqrt(3);
//    tau = kine_viscosity + 0.5* pow(cs,2) *dt;
//    //tau =0.75;
//
//    // set boundary conditions for this test case
//    bcs.w_rho = 1;
//    bcs.w_u = 0;
//    bcs.w_v = 0;
//    bcs.w_w = 0;
//    bcs.w_type = 1;
//
//    bcs.s_rho = 1;
//    bcs.s_u = 0;
//    bcs.s_v = 0;
//    bcs.s_w = 0;
//    bcs.s_type = 3;
//
//    bcs.e_rho = 1;
//    bcs.e_u = 0;
//    bcs.e_v = U;
//    bcs.e_w = 0;
//    bcs.e_type =1;
//
//    bcs.n_rho = 0;
//    bcs.n_u = 0;
//    bcs.n_v = 0;
//    bcs.n_w = 0;
//    bcs.n_type = 2;
//
//
//
//    /// Methods to run the test case
//    //vector_var_tests();
//
//    // create Mesh
//    Mesh mesh(X,Y,dx,dy);
//
//    // create boundary conditions
//    Boundary_Conditions bc(mesh.get_num_x(), mesh.get_num_y());
//    bc.assign_boundary_conditions(mesh.get_num_x(), mesh.get_num_y(),bcs);
//
//    // assign external force terms
//    external_forces source_term(mesh.get_total_nodes());
//    source_term.set_uniform_force(0.0);
//
//    //create solution
//    Solution soln(mesh.get_total_nodes());
//
//    // Solve
//
//    Solver solve;
//    solve.Mesh_Solver(dt,tau,mesh,soln,bc,simulation_length, delta_t,dx, output_file, source_term,pre_conditioned_gamma);
//
//    tau = 1;
//
//}
//
//std::string test_cases::create_output_directory(double tol, double dx, double dt){
//
//    std::string output_file;
//    std::string folder;
//    std::ostringstream s;
//    //output_file = "C:/Users/brendan/Dropbox/PhD/Test Cases/Poiseuille Flow/";
//
//    output_file = "/home/brendan/Dropbox/PhD/Test Cases/Poiseuille Flow/";
//    s << "tol " << tol << " x " << dx
//     << " t " << dt;
//     folder = s.str();
//    //folder.replace(folder.begin(),folder.end(), ".",  "_");
//    boost::replace_all(folder, "." , "_");
//    output_file = output_file + folder;
//
//    boost::filesystem::path dir(output_file);
//    boost::filesystem::create_directories(dir);
//
//    return output_file;
//
//}
//
//void test_cases::vector_var_tests(){
//     vector_var a,b,c,d,e,f;
//
//    a.x = 3;
//    a.y = 4;
//    a.z = 0;
//
//    b.x = 2;
//    b.y = 2;
//    b.z = 2;
//
//    c.x = 4;
//    c.y = -3;
//    c.z = 0;
//
//    d.x= 0;
//    d.y = 0;
//    d.z = 0;
//
//    f.x = 5;
//    f.y = 5;
//    f.z = 0;
//
//
//
//
//    double mag , dp, ang;
//    mag = a.Magnitude();
//
//    cout <<  "Magnitude of a:" << mag << endl ;
//    dp = a.Dot_Product(b);
//    cout << "Dot product of a and b: " << dp << endl ;
//    ang = a.Angle_Between_Vectors(c);
//    ang = ang *360/2/M_PI;
//    cout << "angle between a and c: " << ang << endl ;
//    e.Get_Gradient(10,20,d,b);
//    cout << "gradient vector between d and b =" << e.x << "," << e.y << "," << e.z << endl ;
//
//
//
//}


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

//// Body panels creation
// Compute panel collocation point, surface, vertices, normal, longitudinal, transverse and perpendicular unit vectors
// from data contained into sGrid
//
// I/O:
// - sGrid: temporary dynamic array containing body panel vertices
// - bPan: body panels (structure)

#include <iostream>
#include <Eigen/Dense>
#include "create_panel.h"

#define NDIM 3

using namespace std;
using namespace Eigen;

void create_panel(MatrixX3d &sGrid, Network &bPan) {

    // Temporary variables
    double norm = 0;
    int panIdx;
    int c1, c2, c3, c4;
    Vector3d v1(NDIM), v2(NDIM);

    //// Begin
    cout << "Creating panels... " << flush;

    // Resizing network matrices
    bPan.CG.resize(bPan.nP, NDIM);
    bPan.v0.resize(bPan.nP, NDIM);
    bPan.v1.resize(bPan.nP, NDIM);
    bPan.v2.resize(bPan.nP, NDIM);
    bPan.v3.resize(bPan.nP, NDIM);
    bPan.S.resize(bPan.nP, 1);
    bPan.n.resize(bPan.nP, NDIM);
    bPan.l.resize(bPan.nP, NDIM);
    bPan.t.resize(bPan.nP, NDIM);
    bPan.p.resize(bPan.nP, NDIM);

    // Compute corner points, collocation points and vectors
    for (int i = 0; i < bPan.nC_; ++i) {
        for (int k = 0; k < bPan.nS_; ++k) {

            panIdx = i + k * (bPan.nC_);
            c1 = i + k * bPan.nC;
            c2 = i + 1 + k * bPan.nC;
            c3 = i + 1 + (k+1) *bPan.nC;
            c4 = i + (k+1) * bPan.nC;

            bPan.v0(panIdx,0) = sGrid(c1,0); // pts[i][k]
            bPan.v1(panIdx,0) = sGrid(c2,0); // pts[i+1][k]
            bPan.v2(panIdx,0) = sGrid(c3,0); // pts[i+1][k+1]
            bPan.v3(panIdx,0) = sGrid(c4,0); // pts[i][k+1]
            bPan.v0(panIdx,1) = sGrid(c1,1);
            bPan.v1(panIdx,1) = sGrid(c2,1);
            bPan.v2(panIdx,1) = sGrid(c3,1);
            bPan.v3(panIdx,1) = sGrid(c4,1);
            bPan.v0(panIdx,2) = sGrid(c1,2);
            bPan.v1(panIdx,2) = sGrid(c2,2);
            bPan.v2(panIdx,2) = sGrid(c3,2);
            bPan.v3(panIdx,2) = sGrid(c4,2);

            bPan.CG(panIdx,0) = (bPan.v0(panIdx,0) + bPan.v1(panIdx,0) + bPan.v2(panIdx,0) + bPan.v3(panIdx,0)) / 4;
            bPan.CG(panIdx,1) = (bPan.v0(panIdx,1) + bPan.v1(panIdx,1) + bPan.v2(panIdx,1) + bPan.v3(panIdx,1)) / 4;
            bPan.CG(panIdx,2) = (bPan.v0(panIdx,2) + bPan.v1(panIdx,2) + bPan.v2(panIdx,2) + bPan.v3(panIdx,2)) / 4;

            bPan.l(panIdx,0) = (bPan.v0(panIdx,0) + bPan.v3(panIdx,0) - bPan.v1(panIdx,0) - bPan.v2(panIdx,0)) / 4;
            bPan.l(panIdx,1) = (bPan.v0(panIdx,1) + bPan.v3(panIdx,1) - bPan.v1(panIdx,1) - bPan.v2(panIdx,1)) / 4;
            bPan.l(panIdx,2) = (bPan.v0(panIdx,2) + bPan.v3(panIdx,2) - bPan.v1(panIdx,2) - bPan.v2(panIdx,2)) / 4;
            bPan.l.row(panIdx) /= bPan.l.row(panIdx).norm();

            bPan.t(panIdx,0) = (bPan.v2(panIdx,0) + bPan.v3(panIdx,0) - bPan.v0(panIdx,0) - bPan.v1(panIdx,0)) / 4;
            bPan.t(panIdx,1) = (bPan.v2(panIdx,1) + bPan.v3(panIdx,1) - bPan.v0(panIdx,1) - bPan.v1(panIdx,1)) / 4;
            bPan.t(panIdx,2) = (bPan.v2(panIdx,2) + bPan.v3(panIdx,2) - bPan.v0(panIdx,2) - bPan.v1(panIdx,2)) / 4;
            bPan.t.row(panIdx) /= bPan.t.row(panIdx).norm();
        }
    }
    // Compute panel normals and surfaces
    for (int i = 0; i < bPan.nC_; ++i) {
        for (int k = 0; k < bPan.nS_; ++k) {
            panIdx = i + k * (bPan.nC_);

            v1(0) = bPan.v2(panIdx,0) - bPan.v0(panIdx,0);
            v2(0) = bPan.v1(panIdx,0) - bPan.v3(panIdx,0);
            v1(1) = bPan.v2(panIdx,1) - bPan.v0(panIdx,1);
            v2(1) = bPan.v1(panIdx,1) - bPan.v3(panIdx,1);
            v1(2) = bPan.v2(panIdx,2) - bPan.v0(panIdx,2);
            v2(2) = bPan.v1(panIdx,2) - bPan.v3(panIdx,2);

            bPan.n.row(panIdx) = v1.cross(v2);
            norm = bPan.n.row(panIdx).norm();

            bPan.S(panIdx) = norm/2;
            bPan.n.row(panIdx) /= norm;
        }
    }
    // Compute panel perpendicular vector
    for (int i = 0; i < bPan.nC_; ++i) {
        for (int k = 0; k < bPan.nS_; ++k) {
            panIdx = i + k * (bPan.nC_);
            bPan.p.row(panIdx) = bPan.n.row(panIdx).cross(bPan.l.row(panIdx));
        }
    }

    //// Control display
    cout << "Done!" << endl;
    #ifdef VERBOSE
        cout << "Collocation points: " << bPan.CG.rows() << 'X' << bPan.CG.cols() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.CG(i,0) << ' ' << bPan.CG(i,1) << ' ' << bPan.CG(i,2) << endl;
        cout << "Panel surfaces: " << bPan.S.rows() << 'X' << bPan.S.cols() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.S(i) << endl;
        cout << "Unit normals: " << bPan.n.rows() << 'X' << bPan.n.cols() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.n(i,0) << ' ' << bPan.n(i,1) << ' ' << bPan.n(i,2) << endl;
        cout << "Unit longitudinal vectors: " << bPan.l.rows() << 'X' << bPan.l.cols() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.l(i,0) << ' ' << bPan.l(i,1) << ' ' << bPan.l(i,2) << endl;
        cout << "Unit transverse vectors: " << bPan.t.rows() << 'X' << bPan.t.cols() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.t(i,0) << ' ' << bPan.t(i,1) << ' ' << bPan.t(i,2) << endl;
        cout << "Unit perpendicular vectors: " << bPan.p.rows() << 'X' << bPan.p.cols() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.p(i,0) << ' ' << bPan.p(i,1) << ' ' << bPan.p(i,2) << endl;
    #endif
    cout << endl;
}


#pragma once

#include <Eigen/Dense>
#include <iostream>
#include <vector>

namespace tools::linalg {

struct SolverData {
  double relative_residual = 0;
  double initial_algebraic_error = 0;
  double algebraic_error = 0;
  size_t iterations = 0;
  bool converged = false;
};

enum StoppingCriterium { Relative, Algebraic };

// Loosely based off Eigen/ConjugateGradient.h.
template <typename MatType, typename PrecondType>
std::pair<Eigen::VectorXd, SolverData> PCG(
    const MatType &A, const Eigen::VectorXd &b, const PrecondType &M,
    const Eigen::VectorXd &x0, int imax, double tol,
    enum StoppingCriterium stopping = StoppingCriterium::Relative);

template <typename MatType, typename PrecondType>
class Lanczos {
 public:
  Lanczos(const MatType &A, const PrecondType &P, size_t max_iterations = 200,
          double tol = 0.0001, double tol_bisec = 0.000001)
      : Lanczos(A, P, Eigen::VectorXd::Random(A.cols()), max_iterations, tol,
                tol_bisec) {}
  Lanczos(const MatType &A, const PrecondType &P,
          const Eigen::VectorXd &initial_guess, size_t max_iterations = 200,
          double tol = 0.0001, double tol_bisec = 0.000001);

  double max() const { return lmax_; }
  double min() const { return lmin_; }
  double cond() const { return lmax_ / lmin_; }
  float time() const { return time_; };

  size_t iterations() const { return iterations_; }
  bool converged() const { return converged_; }

  // overload the << operator
  friend std::ostream &operator<<(std::ostream &os, const Lanczos &lanczos) {
    if (lanczos.converged())
      os << "converged\t";
    else
      os << "NOT converged\t";

    os << "its=" << lanczos.iterations() << "\tlmax=" << lanczos.max()
       << "\tlmin=" << lanczos.min() << "\tkappa=" << lanczos.cond()
       << "\ttime=" << lanczos.time() << " s";
    return os;
  }

 private:
  Eigen::VectorXd alpha_, beta_;
  double lmax_, lmin_;

  size_t iterations_;
  bool converged_;
  float time_;

  void bisec(size_t k, double &ymax, double &zmin, double tol_bisec);
  double pol(int k, double x);
};

};  // namespace tools::linalg

#include "linalg.ipp"


/*
Author: Rohan Chetan Thanki
Date created: 16-Oct-2021
*/

/*
This contains the implementation of the Hedging_Portfolio class
*/

#include "Hedging_Portfolio.hpp"
#include <string>
#include <boost/math/distributions.hpp>

using namespace std;

/******************************* Constructors and Destructors ***************************************/

// Default constructor
Hedging_Portfolio::Hedging_Portfolio() : Option() { }

// Parametrised constructor
Hedging_Portfolio::Hedging_Portfolio(const double& K1, const double& S1, const double& r1, const double& T1, const double& vol1, const char& optionType) : Option(K1, S1, r1, T1, vol1, optionType)
{
	if (!(optionType == 'c' || optionType == 'C' || optionType == 'p' || optionType == 'P'))
		throw(20);
}

// Destructor
Hedging_Portfolio::~Hedging_Portfolio(void) { }

/******************************* Getters ***************************************/

double Hedging_Portfolio::getDelta(void) const { return delta; }
double Hedging_Portfolio::getOptionPrice(void) const { return optionPrice; }
double Hedging_Portfolio::getB(void) const { return B; }
double Hedging_Portfolio::getHedgingError(void) const { return HE; }

string Hedging_Portfolio::getDate(void) const { return date; }
string Hedging_Portfolio::getExpDate(void) const { return expDate; }
double Hedging_Portfolio::getpnlNaked(void) const { return pnlNaked; }
double Hedging_Portfolio::getpnlHedged(void) const { return pnlHedged; }

/******************************* Setters ***************************************/

void Hedging_Portfolio::setDelta(const double& delta1) { delta = delta1; }
void Hedging_Portfolio::setOptionPrice(const double& optionPrice1) { optionPrice = optionPrice1; }
void Hedging_Portfolio::setB(const double& B1) { B = B1; }
void Hedging_Portfolio::setHedgingError(const double& HE1) { HE = HE1; }

void Hedging_Portfolio::setDate(const string& date1) { date = date1; }
void Hedging_Portfolio::setExpDate(const string& expDate1) { expDate = expDate1; }
void Hedging_Portfolio::setpnlNaked(const double& pnlNaked1) { pnlNaked = pnlNaked1; }
void Hedging_Portfolio::setpnlHedged(const double& pnlHedged1) { pnlHedged = pnlHedged1; }

/******************************* Other Functions ***************************************/

// Get CDF of Standard Normal
inline double N(const double& x)
{
	boost::math::normal_distribution<> stdNormal(0.0, 1.0);
	return(cdf(stdNormal, x));
}

double Hedging_Portfolio::computeDelta(void) const
{
	char flag = getFlag();
	double K = getStrikePrice();
	double S = getSpotPrice();
	double r = getRiskFreeRate();
	double T = getTimeToMaturity();
	double sigma = getVolatility();
	double d1 = (log(S / K) + ((r + (pow(sigma, 2) / 2)) * T)) / (sigma * sqrt(T));
	if (flag == 'c' || flag == 'C')
		return (N(d1));
	else if (flag == 'p' || flag == 'P')
		return (N(d1));
	else
		throw(10);
}

// Compute option price using Black Scholes formula
double Hedging_Portfolio::computeBlackScholesOptionPrice(const double& sigma) const
{
	char flag = getFlag();
	double K = getStrikePrice();
	double S = getSpotPrice();
	double r = getRiskFreeRate();
	double T = getTimeToMaturity();

	Option opt1(K, S, r, T, sigma, flag);

	double d1 = (log(S / K) + ((r + (pow(sigma, 2) / 2)) * T)) / (sigma * sqrt(T));
	double d2 = d1 - (sigma * sqrt(T));

	if (flag == 'c' || flag == 'C')
		return (S * N(d1)) - (K * exp(-r * T) * N(d2));
	else if (flag == 'p' || flag == 'P')
		return (K * exp(-r * T) * N(-d2)) - (S * N(-d1));
	else
		throw(10);
}

//double getImpliedVol(const double&, const double&, const double&,
double Hedging_Portfolio:: computeImpliedVol(double& volMax) const
{
	double flag = getFlag();
	double volMin = 0;
	double impVol;
	double TMat = getTimeToMaturity();
	double epsilon = 0.001;
	double modelPrice;
	string temp;

	while (true)
	{
		impVol = 0.5 * (volMin + volMax);
		modelPrice = computeBlackScholesOptionPrice(impVol);

		if (modelPrice > optionPrice + epsilon)
			volMax = impVol;
		else if (modelPrice < optionPrice - epsilon)
			volMin = impVol;
		else
			return(impVol);
	}
}


#include <nori/integrator.h>
#include <nori/scene.h>
#include <nori/ray.h>
#include <filesystem/resolver.h>
#include <sh/spherical_harmonics.h>
#include <sh/default_image.h>
#include <Eigen/Core>
#include <fstream>
#include <random>
#include <stb_image.h>

NORI_NAMESPACE_BEGIN

namespace ProjEnv
{
    std::vector<std::unique_ptr<float[]>>
    LoadCubemapImages(const std::string &cubemapDir, int &width, int &height,
                      int &channel)
    {
        std::vector<std::string> cubemapNames{"negx.jpg", "posx.jpg", "posy.jpg",
                                              "negy.jpg", "posz.jpg", "negz.jpg"};
        std::vector<std::unique_ptr<float[]>> images(6);
        for (int i = 0; i < 6; i++)
        {
            std::string filename = cubemapDir + "/" + cubemapNames[i];
            int w, h, c;
            float *image = stbi_loadf(filename.c_str(), &w, &h, &c, 3);
            if (!image)
            {
                std::cout << "Failed to load image: " << filename << std::endl;
                exit(-1);
            }
            if (i == 0)
            {
                width = w;
                height = h;
                channel = c;
            }
            else if (w != width || h != height || c != channel)
            {
                std::cout << "Dismatch resolution for 6 images in cubemap" << std::endl;
                exit(-1);
            }
            images[i] = std::unique_ptr<float[]>(image);
            int index = (0 * 128 + 0) * channel;
            // std::cout << images[i][index + 0] << "\t" << images[i][index + 1] << "\t"
            //           << images[i][index + 2] << std::endl;
        }
        return images;
    }

    const Eigen::Vector3f cubemapFaceDirections[6][3] = {
        {{0, 0, 1}, {0, -1, 0}, {-1, 0, 0}},  // negx
        {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}},   // posx
        {{1, 0, 0}, {0, 0, -1}, {0, -1, 0}},  // negy
        {{1, 0, 0}, {0, 0, 1}, {0, 1, 0}},    // posy
        {{-1, 0, 0}, {0, -1, 0}, {0, 0, -1}}, // negz
        {{1, 0, 0}, {0, -1, 0}, {0, 0, 1}},   // posz
    };

    float CalcPreArea(const float &x, const float &y)
    {
        return std::atan2(x * y, std::sqrt(x * x + y * y + 1.0));
    }

    float CalcArea(const float &u_, const float &v_, const int &width,
                   const int &height)
    {
        // transform from [0..res - 1] to [- (1 - 1 / res) .. (1 - 1 / res)]
        // ( 0.5 is for texel center addressing)
        float u = (2.0 * (u_ + 0.5) / width) - 1.0;
        float v = (2.0 * (v_ + 0.5) / height) - 1.0;

        // shift from a demi texel, mean 1.0 / size  with u and v in [-1..1]
        float invResolutionW = 1.0 / width;
        float invResolutionH = 1.0 / height;

        // u and v are the -1..1 texture coordinate on the current face.
        // get projected area for this texel
        float x0 = u - invResolutionW;
        float y0 = v - invResolutionH;
        float x1 = u + invResolutionW;
        float y1 = v + invResolutionH;
        float angle = CalcPreArea(x0, y0) - CalcPreArea(x0, y1) -
                      CalcPreArea(x1, y0) + CalcPreArea(x1, y1);

        return angle;
    }

    // template <typename T> T ProjectSH() {}

    template <size_t SHOrder>
    std::vector<Eigen::Array3f> PrecomputeCubemapSH(const std::vector<std::unique_ptr<float[]>> &images,
                                                    const int &width, const int &height,
                                                    const int &channel)
    {
        std::vector<Eigen::Vector3f> cubemapDirs;
        cubemapDirs.reserve(6 * width * height);
        for (int i = 0; i < 6; i++)
        {
            Eigen::Vector3f faceDirX = cubemapFaceDirections[i][0];
            Eigen::Vector3f faceDirY = cubemapFaceDirections[i][1];
            Eigen::Vector3f faceDirZ = cubemapFaceDirections[i][2];
            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    float u = 2 * ((x + 0.5) / width) - 1;
                    float v = 2 * ((y + 0.5) / height) - 1;
                    Eigen::Vector3f dir = (faceDirX * u + faceDirY * v + faceDirZ).normalized();
                    cubemapDirs.push_back(dir);
                }
            }
        }
        constexpr int SHNum = (SHOrder + 1) * (SHOrder + 1);
        std::vector<Eigen::Array3f> SHCoeffiecents(SHNum);
        for (int i = 0; i < SHNum; i++)
            SHCoeffiecents[i] = Eigen::Array3f(0);
        float sumWeight = 0;
        for (int i = 0; i < 6; i++)
        {
            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    // TODO: here you need to compute light sh of each face of cubemap of each pixel
                    // TODO: 此处你需要计算每个像素下cubemap某个面的球谐系数
                    Eigen::Vector3f dir = cubemapDirs[i * width * height + y * width + x];
                    int index = (y * width + x) * channel;
                    Eigen::Array3f Le(images[i][index + 0], images[i][index + 1],
                                      images[i][index + 2]);
                    auto wOmega = CalcArea(x, y, width, height);

                    for (auto l = 0; l <= SHOrder; ++l)
                    {
                        for (auto m = -l; m <= l; ++m)
                        {
                            auto k = sh::GetIndex(l, m);
                            auto basicFunc = sh::EvalSH(l, m, dir.cast<double>().normalized());

                            SHCoeffiecents[k] += wOmega * Le * basicFunc;
                        }
                    }
                }
            }
        }
        return SHCoeffiecents;
    }
}

class PRTIntegrator : public Integrator
{
public:
    static constexpr int SHOrder = 2;
    static constexpr int SHCoeffLength = (SHOrder + 1) * (SHOrder + 1);

    enum class Type
    {
        Unshadowed = 0,
        Shadowed = 1,
        Interreflection = 2
    };

    PRTIntegrator(const PropertyList &props)
    {
        /* No parameters this time */
        m_SampleCount = props.getInteger("PRTSampleCount", 100);
        m_CubemapPath = props.getString("cubemap");
        auto type = props.getString("type", "unshadowed");
        if (type == "unshadowed")
        {
            m_Type = Type::Unshadowed;
        }
        else if (type == "shadowed")
        {
            m_Type = Type::Shadowed;
        }
        else if (type == "interreflection")
        {
            m_Type = Type::Interreflection;
            m_Bounce = props.getInteger("bounce", 1);
        }
        else
        {
            throw NoriException("Unsupported type: %s.", type);
        }
    }

    virtual void preprocess(const Scene *scene) override
    {

        // Here only compute one mesh
        const auto mesh = scene->getMeshes()[0];
        // Projection environment
        auto cubePath = getFileResolver()->resolve(m_CubemapPath);
        auto lightPath = cubePath / "light.txt";
        auto transPath = cubePath / "transport.txt";
        std::ofstream lightFout(lightPath.str());
        std::ofstream fout(transPath.str());
        int width, height, channel;
        std::vector<std::unique_ptr<float[]>> images =
            ProjEnv::LoadCubemapImages(cubePath.str(), width, height, channel);
        auto envCoeffs = ProjEnv::PrecomputeCubemapSH<SHOrder>(images, width, height, channel);
        m_LightCoeffs.resize(3, SHCoeffLength);
        for (int i = 0; i < envCoeffs.size(); i++)
        {
            lightFout << (envCoeffs)[i].x() << " " << (envCoeffs)[i].y() << " " << (envCoeffs)[i].z() << std::endl;
            m_LightCoeffs.col(i) = (envCoeffs)[i];
        }
        std::cout << "Computed light sh coeffs from: " << cubePath.str() << " to: " << lightPath.str() << std::endl;
        // Projection transport
        m_TransportSHCoeffs.resize(SHCoeffLength, mesh->getVertexCount());
        fout << mesh->getVertexCount() << std::endl;
        for (int i = 0; i < mesh->getVertexCount(); i++)
        {
            const Point3f &v = mesh->getVertexPositions().col(i);
            const Normal3f &n = mesh->getVertexNormals().col(i);
            auto shFunc = [&](double phi, double theta) -> double
            {
                Eigen::Array3d d = sh::ToVector(phi, theta);
                const auto wi = Vector3f(d.x(), d.y(), d.z());
                const auto H = wi.dot(n);
                if (m_Type == Type::Unshadowed)
                {
                    // TODO: here you need to calculate unshadowed transport term of a given direction
                    // TODO: 此处你需要计算给定方向下的unshadowed传输项球谐函数值
                    return std::max(0.f, H);
                }
                else
                {
                    // TODO: here you need to calculate shadowed transport term of a given direction
                    // TODO: 此处你需要计算给定方向下的shadowed传输项球谐函数值
                    Ray3f ray(v, wi.normalized());
                    if (H > 0 && !scene->rayIntersect(ray))
                        return H;
                    return 0;
                }
            };
            auto shCoeff = sh::ProjectFunction(SHOrder, shFunc, m_SampleCount);
            for (int j = 0; j < shCoeff->size(); j++)
            {
                m_TransportSHCoeffs.col(i).coeffRef(j) = (*shCoeff)[j];
            }
        }
        if (m_Type == Type::Interreflection)
        {
            // TODO: leave for bonus
        }

        // Save in face format
        for (int f = 0; f < mesh->getTriangleCount(); f++)
        {
            const MatrixXu &F = mesh->getIndices();
            uint32_t idx0 = F(0, f), idx1 = F(1, f), idx2 = F(2, f);
            for (int j = 0; j < SHCoeffLength; j++)
            {
                fout << m_TransportSHCoeffs.col(idx0).coeff(j) << " ";
            }
            fout << std::endl;
            for (int j = 0; j < SHCoeffLength; j++)
            {
                fout << m_TransportSHCoeffs.col(idx1).coeff(j) << " ";
            }
            fout << std::endl;
            for (int j = 0; j < SHCoeffLength; j++)
            {
                fout << m_TransportSHCoeffs.col(idx2).coeff(j) << " ";
            }
            fout << std::endl;
        }
        std::cout << "Computed SH coeffs"
                  << " to: " << transPath.str() << std::endl;
    }

    Color3f Li(const Scene *scene, Sampler *sampler, const Ray3f &ray) const
    {
        Intersection its;
        if (!scene->rayIntersect(ray, its))
            return Color3f(0.0f);

        const Eigen::Matrix<Vector3f::Scalar, SHCoeffLength, 1> sh0 = m_TransportSHCoeffs.col(its.tri_index.x()),
                                                                sh1 = m_TransportSHCoeffs.col(its.tri_index.y()),
                                                                sh2 = m_TransportSHCoeffs.col(its.tri_index.z());
        const Eigen::Matrix<Vector3f::Scalar, SHCoeffLength, 1> rL = m_LightCoeffs.row(0), gL = m_LightCoeffs.row(1), bL = m_LightCoeffs.row(2);

        Color3f c0 = Color3f(rL.dot(sh0), gL.dot(sh0), bL.dot(sh0)),
                c1 = Color3f(rL.dot(sh1), gL.dot(sh1), bL.dot(sh1)),
                c2 = Color3f(rL.dot(sh2), gL.dot(sh2), bL.dot(sh2));

        const Vector3f &bary = its.bary;
        Color3f c = bary.x() * c0 + bary.y() * c1 + bary.z() * c2;
        // TODO: you need to delete the following four line codes after finishing your calculation to SH,
        //       we use it to visualize the normals of model for debug.
        // TODO: 在完成了球谐系数计算后，你需要删除下列四行，这四行代码的作用是用来可视化模型法线
        // if (c.isZero())
        // {
        //     auto n_ = its.shFrame.n.cwiseAbs();
        //     return Color3f(n_.x(), n_.y(), n_.z());
        // }
        return c;
    }

    std::string toString() const
    {
        return "PRTIntegrator[]";
    }

private:
    Type m_Type;
    int m_Bounce = 1;
    int m_SampleCount = 100;
    std::string m_CubemapPath;
    Eigen::MatrixXf m_TransportSHCoeffs;
    Eigen::MatrixXf m_LightCoeffs;
};

NORI_REGISTER_CLASS(PRTIntegrator, "prt");
NORI_NAMESPACE_END


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

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a Great Circle segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class spherical_side_formula
{

public :
    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,

                // Select at least a double...
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                typename select_most_precise
                                    <
                                        typename coordinate_type<P1>::type,
                                        typename coordinate_type<P2>::type
                                    >::type,
                                typename coordinate_type<P>::type
                            >::type,
                        double
                    >::type,
                CalculationType
            >::type coordinate_type;

        // Convenient shortcuts
        typedef coordinate_type ct;
        ct const lambda1 = get_as_radian<0>(p1);
        ct const delta1 = get_as_radian<1>(p1);
        ct const lambda2 = get_as_radian<0>(p2);
        ct const delta2 = get_as_radian<1>(p2);
        ct const lambda = get_as_radian<0>(p);
        ct const delta = get_as_radian<1>(p);

        // Create temporary points (vectors) on unit a sphere
        ct const cos_delta1 = cos(delta1);
        ct const c1x = cos_delta1 * cos(lambda1);
        ct const c1y = cos_delta1 * sin(lambda1);
        ct const c1z = sin(delta1);

        ct const cos_delta2 = cos(delta2);
        ct const c2x = cos_delta2 * cos(lambda2);
        ct const c2y = cos_delta2 * sin(lambda2);
        ct const c2z = sin(delta2);

        // (Third point is converted directly)
        ct const cos_delta = cos(delta);

        // Apply the "Spherical Side Formula" as presented on my blog
        ct const dist
            = (c1y * c2z - c1z * c2y) * cos_delta * cos(lambda)
            + (c1z * c2x - c1x * c2z) * cos_delta * sin(lambda)
            + (c1x * c2y - c1y * c2x) * sin(delta);

        ct zero = ct();
        return dist > zero ? 1
            : dist < zero ? -1
            : 0;
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

/*template <typename CalculationType>
struct default_strategy<spherical_polar_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};*/

template <typename CalculationType>
struct default_strategy<spherical_equatorial_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};

template <typename CalculationType>
struct default_strategy<geographic_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};

}
#endif

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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.
// ----------------------------------------------------------------------------

#include "PointCloud.h"

#include <Eigen/Eigenvalues>
#include <Core/Utility/Console.h>
#include <Core/Geometry/KDTreeFlann.h>

namespace three{

namespace {

double sqr(double x) { return x * x; }

Eigen::Vector3d FastEigen3x3(const Eigen::Matrix3d &A)
{
	// Based on:
	// https://en.wikipedia.org/wiki/Eigenvalue_algorithm#3.C3.973_matrices
	double p1 = sqr(A(0, 1)) + sqr(A(0, 2)) + sqr(A(1, 2));
	Eigen::Vector3d eigenvalues;
	if (p1 == 0.0) {
		eigenvalues(2) = std::min(A(0, 0), std::min(A(1, 1), A(2, 2)));
		eigenvalues(0) = std::max(A(0, 0), std::max(A(1, 1), A(2, 2)));
		eigenvalues(1) = A.trace() - eigenvalues(0) - eigenvalues(2);
	} else {
		double q = A.trace() / 3.0;
		double p2 = sqr((A(0, 0) - q)) + sqr(A(1, 1) - q) + sqr(A(2, 2) - q) +
				2 * p1;
		double p = sqrt(p2 / 6.0);
		Eigen::Matrix3d B = (1.0 / p) * (A - q * Eigen::Matrix3d::Identity());
		double r = B.determinant() / 2.0;
		double phi;
		if (r <= -1) {
			phi = M_PI / 3.0;
		} else if (r >= 1) {
			phi = 0.0;
		} else {
			phi = std::acos(r) / 3.0;
		}
		eigenvalues(0) = q + 2.0 * p * std::cos(phi);
		eigenvalues(2) = q + 2.0 * p * std::cos(phi + 2.0 * M_PI / 3.0);
		eigenvalues(1) = q * 3.0 - eigenvalues(0) - eigenvalues(2);
	}

	Eigen::Vector3d eigenvector =
			(A - Eigen::Matrix3d::Identity() * eigenvalues(0)) *
			(A.col(0) - Eigen::Vector3d(eigenvalues(1), 0.0, 0.0));
	double len = eigenvector.norm();
	if (len == 0.0) {
		return Eigen::Vector3d::Zero();
	} else {
		return eigenvector.normalized();
	}
}

Eigen::Vector3d ComputeNormal(const PointCloud &cloud,
		const std::vector<int> &indices)
{
	if (indices.size() == 0) {
		return Eigen::Vector3d::Zero();
	}
	Eigen::Matrix3d covariance;
	Eigen::Matrix<double, 9, 1> cumulants;
	cumulants.setZero();
	for (size_t i = 0; i < indices.size(); i++) {
		const Eigen::Vector3d &point = cloud.points_[indices[i]];
		cumulants(0) += point(0);
		cumulants(1) += point(1);
		cumulants(2) += point(2);
		cumulants(3) += point(0) * point(0);
		cumulants(4) += point(0) * point(1);
		cumulants(5) += point(0) * point(2);
		cumulants(6) += point(1) * point(1);
		cumulants(7) += point(1) * point(2);
		cumulants(8) += point(2) * point(2);
	}
	cumulants /= (double)indices.size();
	covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
	covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
	covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
	covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
	covariance(1, 0) = covariance(0, 1);
	covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
	covariance(2, 0) = covariance(0, 2);
	covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
	covariance(2, 1) = covariance(1, 2);

	return FastEigen3x3(covariance);
	//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
	//solver.compute(covariance, Eigen::ComputeEigenvectors);
	//return solver.eigenvectors().col(0);
}

}	// unnamed namespace

bool EstimateNormals(PointCloud &cloud,
		const KDTreeSearchParam &search_param/* = KDTreeSearchParamKNN()*/)
{
	bool has_normal = cloud.HasNormals();
	if (cloud.HasNormals() == false) {
		cloud.normals_.resize(cloud.points_.size());
	}
	KDTreeFlann kdtree;
	kdtree.SetGeometry(cloud);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)cloud.points_.size(); i++) {
		std::vector<int> indices;
		std::vector<double> distance2;
		Eigen::Vector3d normal;
		if (kdtree.Search(cloud.points_[i], search_param, indices,
				distance2) >= 3) {
			normal = ComputeNormal(cloud, indices);
			if (normal.norm() == 0.0) {
				if (has_normal) {
					normal = cloud.normals_[i];
				} else {
					normal = Eigen::Vector3d(0.0, 0.0, 1.0);
				}
			}
			if (has_normal && normal.dot(cloud.normals_[i]) < 0.0) {
				normal *= -1.0;
			}
			cloud.normals_[i] = normal;
		} else {
			cloud.normals_[i] = Eigen::Vector3d(0.0, 0.0, 1.0);
		}
	}

	return true;
}

bool OrientNormalsToAlignWithDirection(PointCloud &cloud,
		const Eigen::Vector3d &orientation_reference
		/* = Eigen::Vector3d(0.0, 0.0, 1.0)*/)
{
	if (cloud.HasNormals() == false) {
		PrintDebug("[OrientNormalsToAlignWithDirection] No normals in the PointCloud. Call EstimateNormals() first.\n");
	}
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)cloud.points_.size(); i++) {
		auto &normal = cloud.normals_[i];
		if (normal.norm() == 0.0) {
			normal = orientation_reference;
		} else if (normal.dot(orientation_reference) < 0.0) {
			normal *= -1.0;
		}
	}
	return true;
}

bool OrientNormalsTowardsCameraLocation(PointCloud &cloud,
		const Eigen::Vector3d &camera_location/* = Eigen::Vector3d::Zero()*/)
{
	if (cloud.HasNormals() == false) {
		PrintDebug("[OrientNormalsTowardsCameraLocation] No normals in the PointCloud. Call EstimateNormals() first.\n");
	}
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)cloud.points_.size(); i++) {
		Eigen::Vector3d orientation_reference = camera_location -
				cloud.points_[i];
		auto &normal = cloud.normals_[i];
		if (normal.norm() == 0.0) {
			normal = orientation_reference;
			if (normal.norm() == 0.0) {
				normal = Eigen::Vector3d(0.0, 0.0, 1.0);
			} else {
				normal.normalize();
			}
		} else if (normal.dot(orientation_reference) < 0.0) {
			normal *= -1.0;
		}
	}
	return true;
}

}	// namespace three


#include <iostream>
#include <memory>
#include <random>
#include <string>

#include <Eigen/Sparse>

#include "GeometricMultigridOperators.h"
#include "GeometricMultigridPoissonSolver.h"
#include "InitialMultigridTestDomains.h"
#include "Renderer.h"
#include "ScalarGrid.h"
#include "Transform.h"
#include "UniformGrid.h"
#include "Utilities.h"

using namespace FluidSim2D::RenderTools;
using namespace FluidSim2D::SimTools;

std::unique_ptr<Renderer> renderer;

static constexpr int gridSize = 512;
static constexpr bool useComplexDomain = true;
static constexpr bool useSolidSphere = true;

int main(int argc, char** argv)
{
	using namespace GeometricMultigridOperators;

	using StoreReal = double;
	using SolveReal = double;

	UniformGrid<CellLabels> domainCellLabels;
	VectorGrid<StoreReal> boundaryWeights;

	int mgLevels;
	{
		UniformGrid<CellLabels> baseDomainCellLabels;
		VectorGrid<StoreReal> baseBoundaryWeights;

		// Complex domain set up
		if (useComplexDomain)
			buildComplexDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								useSolidSphere);
		// Simple domain set up
		else
			buildSimpleDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								1 /*dirichlet band*/);

		// Build expanded domain
		std::pair<Vec2i, int> mgSettings = buildExpandedDomain(domainCellLabels, boundaryWeights, baseDomainCellLabels, baseBoundaryWeights);

		mgLevels = mgSettings.second;
	}

	SolveReal dx = boundaryWeights.dx();

	UniformGrid<StoreReal> rhsGrid(domainCellLabels.size(), 0);

	UniformGrid<StoreReal> solutionGrid(domainCellLabels.size(), 0);

	tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = domainCellLabels.unflatten(cellIndex);

			if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
				domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
			{
				Vec2f point(dx * Vec2f(cell));
				solutionGrid(cell) = 4. * (std::sin(2 * PI * point[0]) * std::sin(2 * PI * point[1]) +
											std::sin(4 * PI * point[0]) * std::sin(4 * PI * point[1]));
			}
		}
	});

	auto l1Norm = [&](const UniformGrid<StoreReal> &grid)
	{
		assert(grid.size() == domainCellLabels.size());

		tbb::enumerable_thread_specific<SolveReal> parallelAccumulatedValue(SolveReal(0));
		tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			auto& localAccumulatedValue = parallelAccumulatedValue.local();

			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = domainCellLabels.unflatten(cellIndex);

				if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
					domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
				{
					localAccumulatedValue += fabs(grid(cell));
				}
			}
		});

		SolveReal accumulatedValue = 0;
		parallelAccumulatedValue.combine_each([&](const SolveReal localAccumulatedValue)
		{
			accumulatedValue += localAccumulatedValue;
		});

		return accumulatedValue;
	};

	// Print initial guess
	solutionGrid.printAsOBJ("initialGuess");

	std::cout << "L-1 initial: " << l1Norm(solutionGrid) << std::endl;

	// Pre-build multigrid preconditioner
	GeometricMultigridPoissonSolver mgSolver(domainCellLabels, boundaryWeights, mgLevels, dx);

	for (int iteration = 0; iteration < 50; ++iteration)
	{
		mgSolver.applyMGVCycle(solutionGrid, rhsGrid, true /* use initial guess */);

		std::cout << "L-1 v-cycle " << iteration << ": " << l1Norm(solutionGrid) << std::endl;

		// Print corrected solution after one v-cycle
		//solutionGrid.printAsOBJ("solutionGrid" + std::to_string(iteration));
	}

	// Print domain labels to make sure they are set up correctly
	int pixelHeight = 1080;
	int pixelWidth = pixelHeight;
	renderer = std::make_unique<Renderer>("One level correction test", Vec2i(pixelWidth, pixelHeight), Vec2f(0), 1, &argc, argv);

	ScalarGrid<float> tempGrid(Transform(dx, Vec2f(0)), domainCellLabels.size());

	tbb::parallel_for(tbb::blocked_range<int>(0, tempGrid.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int flatIndex = range.begin(); flatIndex != range.end(); ++flatIndex)
		{
			Vec2i cell = tempGrid.unflatten(flatIndex);

			tempGrid(cell) = float(domainCellLabels(cell));
		}
	});

	tempGrid.drawVolumetric(*renderer, Vec3f(0), Vec3f(1), float(CellLabels::INTERIOR_CELL), float(CellLabels::BOUNDARY_CELL));

	renderer->run();
}


#ifndef SCALARTYPE_HPP
#define SCALARTYPE_HPP

#include <boost/rational.hpp>
#include <boost/operators.hpp>
#include <sstream>
#include <iomanip>
#include <iostream>

struct scalarreal : boost::operators<scalarreal> {
	template<typename... T>
	using variant = boost::variant<T...>;
	using rtype = boost::rational<int>;

	struct eulerconst {};
	struct piconst {};

	variant<rtype,double,eulerconst,piconst> v;

	scalarreal(double val=0.0) : v(val) {}
	explicit scalarreal(eulerconst ec) : v(ec) {}
	explicit scalarreal(piconst pc) : v(pc) {}
	explicit scalarreal(rtype rt) : v(std::move(rt)) {}

	scalarreal(int i) : v(rtype(i)) {}

	template<typename I1, typename I2>
	explicit scalarreal(I1 &&num, I2 &&den) : v(rtype(std::forward<I1>(num),std::forward<I2>(den))) {}

	/*
	scalarreal(scalarreal &r) : v(r.v) {}
	scalarreal(const scalarreal &r) : v(r.v) {}
	scalarreal(scalarreal &&r) : v(std::move(r.v)) {}
	*/

	explicit operator double() const {
		switch(v.which()) {
			case 0: return (double)(boost::get<rtype>(v).numerator())/(double)(boost::get<rtype>(v).denominator());
			case 1: return boost::get<double>(v);
			case 2: return std::exp(1.0);
			case 3: return std::acos(-1.0);
			default: return 0.0;
		}
	}

	explicit operator int() const {
		return asint();
	}

	scalarreal operator-() const {
		if (v.which()==0) return scalarreal{-boost::get<rtype>(v)};
		else return scalarreal{-(double)(*this)};
		return *this;
	}

	scalarreal &operator+=(const scalarreal &s) {
		if (v.which()==0 && s.v.which()==0) boost::get<rtype>(v) += boost::get<rtype>(s.v);
		else v = (double)(*this) + (double)(s);
		return *this;
	}
	scalarreal &operator-=(const scalarreal &s) {
		if (v.which()==0 && s.v.which()==0) boost::get<rtype>(v) -= boost::get<rtype>(s.v);
		else v = (double)(*this) - (double)(s);
		return *this;
	}
	scalarreal &operator*=(const scalarreal &s) {
		if (v.which()==0 && s.v.which()==0) boost::get<rtype>(v) *= boost::get<rtype>(s.v);
		else v = (double)(*this) * (double)(s);
		return *this;
	}
	scalarreal &operator/=(const scalarreal &s) {
		if (v.which()==0 && s.v.which()==0 && s) boost::get<rtype>(v) /= boost::get<rtype>(s.v);
		else v = (double)(*this) / (double)(s);
		return *this;
	}
	bool operator<(const scalarreal &s) const {
		if (v.which()==0 && s.v.which()==0) return boost::get<rtype>(v) < boost::get<rtype>(s.v);
		else return (double)(*this) < (double)(s);
	}
	bool operator==(const scalarreal &s) const {
		if (v.which()==0 && s.v.which()==0) return boost::get<rtype>(v) == boost::get<rtype>(s.v);
		else return (double)(*this) == (double)(s);
	}
	bool operator==(const double &d) const {
		return (double)(*this) == d;
	}
	bool operator<(const double &d) const {
		return (double)(*this) < d;
	}
	bool operator==(const int &i) const {
		switch (v.which()) {
			case 0: return boost::get<rtype>(v)==i;
			case 1: return (double)(*this) == i;
			default: return false;
		}
	}
	bool operator<(const int &i) const {
		if (v.which()==0) return boost::get<rtype>(v)<i;
		else return (double)(*this) < i;
	}
	const scalarreal &operator++() {
		if (v.which()==0) boost::get<rtype>(v)++;
		else v = (double)(*this)+1.0;
		return *this;
	}
	const scalarreal &operator--() {
		if (v.which()==0) boost::get<rtype>(v)--;
		else v = (double)(*this)-1.0;
		return *this;
	}

	explicit operator bool() const {
		switch(v.which()) {
			case 0: return (bool)(boost::get<rtype>(v));
			case 1: return boost::get<double>(v)!=0.0;
			case 2: return true;
			case 3: return true;
			default: return true;
		}
	}
	bool operator!() const {
		switch(v.which()) {
			case 0: return !(boost::get<rtype>(v));
			case 1: return boost::get<double>(v)==0.0;
			case 2: return false;
			case 3: return false;
			default: return false;
		}
	}

	bool isint() const {
		switch(v.which()) {
			case 0: return boost::get<rtype>(v).numerator() % boost::get<rtype>(v).denominator() == 0;
			case 1: return std::fmod(boost::get<double>(v),1.0) == 0.0;
			default: return false;
		}
	}

	bool iseven() const {
		return isint() && asint()%2==0;
	}
	bool isodd() const {
		return isint() && asint()%2==1;
	}

	scalarreal round() const {
		switch(v.which()) {
			case 0: {
				   auto &r = boost::get<rtype>(v);
				   auto div = std::div(r.numerator(),r.denominator());
				   // away from zero... as in std::round
				   if (div.rem*2<=-abs(r.denominator()))
					   return div.quot - 1;
				   else if (div.rem*2>=abs(r.denominator()))
					   return div.quot + 1;
				   else return div.quot;
			   }
			default: return std::round((double)(*this));
		}
	}

	scalarreal floor() const {
		switch(v.which()) {
			case 0: {
				   auto &r = boost::get<rtype>(v);
				   auto div = std::div(r.numerator(),r.denominator());
				   if (div.rem<0)
					   return div.quot - 1;
				   else return div.quot;
			   }
			default: return std::floor((double)(*this));
		}
	}

	scalarreal ceil() const {
		switch(v.which()) {
			case 0: {
				   auto &r = boost::get<rtype>(v);
				   auto div = std::div(r.numerator(),r.denominator());
				   if (div.rem>0)
					   return div.quot + 1;
				   else return div.quot;
			   }
			default: return std::ceil((double)(*this));
		}
	}

	int asint() const {
		switch(v.which()) {
			case 0: return boost::get<rtype>(v).numerator() / boost::get<rtype>(v).denominator();
			default: return std::round((double)(*this));
		}
	}

	bool iszero() const {
		switch(v.which()) {
			case 0: return boost::get<rtype>(v).numerator() ==0;
			case 1: return boost::get<double>(v) == 0.0;
			default: return false;
		}
	}
};

namespace std {
	template<> struct numeric_limits<scalarreal> {
		static scalarreal infinity() {
			return scalarreal{std::numeric_limits<double>::infinity()};
		}
		static constexpr bool is_specialized = true;
		static constexpr bool is_signed = true;
		static constexpr bool is_integer = false;
		static constexpr bool is_exact = false;
		static constexpr int digits = numeric_limits<double>::digits;
		static constexpr int digits10 = numeric_limits<double>::digits10;
		static constexpr int radix = numeric_limits<double>::radix;
	};

	bool isfinite(const scalarreal &s) {
		switch(s.v.which()) {
			case 1: return std::isfinite(boost::get<double>(s.v));
			default: return true;
		}
	}
}

auto round(const scalarreal &s) {
	return s.round();
}
auto ceil(const scalarreal &s) {
	return s.ceil();
}
auto floor(const scalarreal &s) {
	return s.floor();
}

std::string tostring(const scalarreal &s) {
	if (s.isint()) return std::to_string(s.asint());
	switch (s.v.which()) {
		case 0: {
				   auto &r = boost::get<scalarreal::rtype>(s.v);
				   return std::string("(")+std::to_string(r.numerator())+"/"+std::to_string(r.denominator())+")";
			   }
		case 1: {
				   std::ostringstream ss;
				   ss << std::setprecision(17) << boost::get<double>(s.v);
				   return {ss.str()};
			   }
		case 2: return {"e"};
		case 3: return {"pi"};
		default: return {"?"};
	}
}

/*
std::ostream &operator<<(std::ostream &os, const scalarreal &s) {
	return os << tostring(s);
}

std::istream &operator>>(std::istream &is, scalarreal &s) {
	// not implemented
	return is;
}
*/

scalarreal abs(const scalarreal &s) {
	if (s.v.which()==0)
		return scalarreal{abs(boost::get<scalarreal::rtype>(s.v))};
	return scalarreal{(double)std::abs((double)(s))};
}

scalarreal log(const scalarreal &e) {
	if (e==1.0) return scalarreal{0};
	switch(e.v.which()) {
		case 2: return scalarreal{1};
		default: return log((double)e);
	}
}

template<typename T>
T intpow1(T b, int e) {
	if (e==1) return b;
	T ret = intpow1(b,e/2);
	ret *= ret;
	if (e&1) ret *= b;
	return ret;
}

template<typename T>
T intpow(T b, int e) {
	if (e==0) return T{1};
	if (e<0) return T{1}/intpow1(b,-e);
	return intpow1(b,e);
}

scalarreal pow(const scalarreal &b, const int &e) {
	if (e==1) return b;
	if (e==0) return scalarreal{1};
	if (e==-1) return 1/b;
	if (b.v.which()==2)
		return scalarreal{std::exp((double)e)};
	if (b.v.which()==1)
		return scalarreal{ std::pow((double)b,(double)e) };
	if (b==1) return scalarreal{1};
	if (b.v.which()==0)
		return intpow(b,e);
	else
		return scalarreal{ std::pow((double)b,(double)e) };
}

scalarreal pow(const scalarreal &b, const scalarreal &e) {
	if (e==1) return b;
	if (e==0) return scalarreal{1};
	if (e==-1) return 1/b;
	if (b.v.which()==2)
		return scalarreal{std::exp((double)e)};
	if (b.v.which()==1 || e.v.which()==1)
		return scalarreal{ std::pow((double)b,(double)e) };
	if (b==1) return scalarreal{1};
	if (b.v.which()==0 && e.isint())
		return intpow(b,e.asint());
	else
		return scalarreal{ std::pow((double)b,(double)e) };
}

#endif


#include <vector>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <string>
#include <iterator>
#include <Eigen/Dense>

class Point {
  public:
    Point();
    Point(std::istream& in);
    std::istream& read(std::istream&);
    void print( std::ostream& );
    double getX() const { return x; };
    double getY() const { return y; };

  private:
    double x;
    double y;
    std::string name;
};

Point::Point() : x(0), y(0) {}

Point::Point(std::istream& in){
  read( in );
}

std::istream& Point::read( std::istream& in){
  in >> name >> x >> y;
  return in;
}

void Point::print( std::ostream& out ) {
  out << name << " " << x << " " << y << std::endl;
}

std::istream& operator>>(std::istream& in, Point& p){
  p.read(in);
  return in;
}

void parse_args(int argc , char* argv[], std::string &fname)
{
  if ( 1 == argc ) {
    fname = "data.txt";
  } else {
    fname = argv[1];
  }
}

int main(int argc, char* argv[])
{
  std::string fname;

  parse_args(argc, argv, fname);

  std::ifstream in(fname);
  if (!in) {
    std::cout << "File not found " << fname << std::endl;
    return 1;
  }

  // Use STL to read numbers from file into a vector
  std::vector<Point> data;

  std::copy( std::istream_iterator<Point>(in),
    std::istream_iterator<Point>(), std::back_inserter(data));

  // Transform data
  std::vector<double> x;
  std::transform( data.begin(), data.end(), std::back_inserter(x),
    [](Point p){ return p.getX(); });

  std::vector<double> y;
  std::transform( data.begin(), data.end(), std::back_inserter(y),
    [](Point p){ return p.getY(); });

  Eigen::Map<Eigen::VectorXd> X(x.data(),x.size());
  Eigen::Map<Eigen::VectorXd> Y(y.data(),y.size());

  // Store x and y coordinates as 2 x N matrix [x1, x2, ...; y1, y2, ...]
  Eigen::MatrixXd m( 2, x.size());
  m.row(0) = X;
  m.row(1) = Y;
  std::cout << m << std::endl;

  // Shear transformation
  Eigen::Matrix2d A;
  A << 1, 2, 0, 1;
  m = A*m;
  std::cout << m << std::endl;
  X = m.row(0);

  for( auto d : data){
    d.print( std::cout );
  }
  std::cout << std::endl;

  for( auto xv : x){
    std::cout << xv << std::endl;
  }

  return 0;
}


/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

#ifndef GEOREFERENCING_HPP
#define GEOREFERENCING_HPP

#include <Eigen/Dense>
#include "../math/CoordinateTransform.hpp"
#include "Raytracing.hpp"
#include "../Ping.hpp"

/*!
* \brief Georeferencing class
* \author Guillaume Labbe-Morissette, Jordan McManus, Emile Gagne
* \date October 2, 2018, 9:39 AM
*/
class Georeferencing{
public:
  /**
  * Georeferences a ping
  *
  * @param georeferencedPing georeferenced ping in vector form
  * @param attitude the attitude of the ship in the IMU frame
  * @param position the position of the ship in the TRF
  * @param ping the ping of the georeference in the sonar frame
  * @param svp the SoundVelocityProfile
  * @param leverArm vector from the position reference point (PRP) to the acoustic center
  *
  */
  virtual void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight){};
};

/*!
* \brief TRF Georeferencing class
*
* Extends Georeferencing class
*/
class GeoreferencingTRF : public Georeferencing{
public:

  /**
  * Georeferences a ping in the TRF
  *
  * @param georeferencedPing vector of a ping georeferenced
  * @param attitude the attitude of the ship in the IM frame
  * @param position the position of the ship in the TRF
  * @param ping the ping of the georeference in the sonar frame
  * @param svp the sound velocity profile
  * @param leverArm vector from the position reference point (PRP) to the acoustic center
  *
  */
  void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight) {
    //Compute transform matrixes
    Eigen::Matrix3d ned2ecef;
    CoordinateTransform::ned2ecef(ned2ecef,position);

#ifdef DEBUG
        std::cerr << "NED 2 ECEF: " << std::endl << ned2ecef << std::endl << std::endl;
#endif

    Eigen::Matrix3d imu2ned;
    CoordinateTransform::getDCM(imu2ned,attitude);

#ifdef DEBUG
        std::cerr << "IMU 2 NED: " << std::endl << imu2ned << std::endl << std::endl;
#endif

    //Convert position to ECEF
    Eigen::Vector3d positionECEF;
    CoordinateTransform::getPositionECEF(positionECEF,position);

#ifdef DEBUG
        std::cerr << "Position ECEF: " << std::endl << positionECEF << std::endl << std::endl;
#endif

    //Convert ping to ECEF
    Eigen::Vector3d pingVectorNED;
    Raytracing::rayTrace(pingVectorNED,ping,svp,boresight,imu2ned);

#ifdef DEBUG
        std::cerr << "Raytraced ping: " << std::endl << pingVectorNED << std::endl << std::endl;
#endif

    Eigen::Vector3d pingECEF = ned2ecef * pingVectorNED;

#ifdef DEBUG
        std::cerr << "Ping ECEF: " << std::endl << pingECEF << std::endl << std::endl;
#endif

    //Convert lever arm to ECEF
    Eigen::Vector3d leverArmECEF =  ned2ecef * (imu2ned * leverArm);

    //Compute total ECEF vector

    georeferencedPing = positionECEF + pingECEF + leverArmECEF;
  }
};

/*!
* \brief LGF Georeferencing class
*/
class GeoreferencingLGF : public Georeferencing{
public:

    /**
     * Georeferences a ping in the LGF (NED)
     *
     * @param georeferencedPing vector of a ping georeferenced
     * @param attitude the attitude of the ship in the IM frame
     * @param position the position of the ship in the TRF
     * @param ping the ping of the georeference in the sonar frame
     * @param svp the sound velocity profile
     * @param leverArm vector from the position reference point (PRP) to the acoustic center
     *
     */

    virtual void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight) {
        Eigen::Matrix3d imu2ned;
        CoordinateTransform::getDCM(imu2ned,attitude);

	//Convert position's geographic coordinates to ECEF, and then from ECEF to NED
        Eigen::Vector3d positionECEF;
        CoordinateTransform::getPositionECEF(positionECEF,position);

#ifdef DEBUG
        std::cerr << "Position ECEF: " << std::endl << positionECEF << std::endl << std::endl;
#endif

        Eigen::Vector3d centered = positionECEF-centroidECEF;

	Eigen::Vector3d positionNED = ecef2ned * centered;

        //Convert ping to NED
        Eigen::Vector3d pingNED;
        Raytracing::rayTrace(pingNED,ping,svp,boresight,imu2ned);

        //Convert lever arm to NED
        Eigen::Vector3d leverArmNED =  imu2ned * leverArm;

        //Compute total NED vector
        georeferencedPing = positionNED + pingNED + leverArmNED;
    }

    /**
     * Sets centroid and inits ECEF 2 NED matrix
     */
    void setCentroid(Position & c){
	if(this->centroid) delete centroid;

	this->centroid=new Position(c.getTimestamp(), c.getLatitude(), c.getLongitude(), c.getEllipsoidalHeight());
        CoordinateTransform::getPositionECEF(centroidECEF,*this->centroid);
	CoordinateTransform::ned2ecef(ecef2ned,*this->centroid);
        ecef2ned.transposeInPlace();
    }

    /**
    *  Get a pointer to the centroid
    */

    Position * getCentroid(){ return centroid;};

protected:

        Eigen::Vector3d centroidECEF;
	Eigen::Matrix3d ecef2ned;

private:
	Position * centroid = NULL; //in geographic coordinates
};

#endif


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

/*
 Copyright (C) 2016 Klaus Spanderen

 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.
*/

/*! \file squarerootclvmodel.cpp
    \brief CLV model with a square root kernel process
*/

#include <ql/processes/blackscholesprocess.hpp>
#include <ql/processes/squarerootprocess.hpp>
#include <ql/math/integrals/gaussianquadratures.hpp>

#include <ql/experimental/models/squarerootclvmodel.hpp>
#include <ql/methods/finitedifferences/utilities/gbsmrndcalculator.hpp>

#include <boost/math/distributions/non_central_chi_squared.hpp>

#include <utility>

namespace QuantLib {
    SquareRootCLVModel::SquareRootCLVModel(
        const ext::shared_ptr<GeneralizedBlackScholesProcess>& bsProcess,
        ext::shared_ptr<SquareRootProcess> sqrtProcess,
        std::vector<Date> maturityDates,
        Size lagrangeOrder,
        Real pMax,
        Real pMin)
    : pMax_(pMax), pMin_(pMin), bsProcess_(bsProcess), sqrtProcess_(std::move(sqrtProcess)),
      maturityDates_(std::move(maturityDates)), lagrangeOrder_(lagrangeOrder),
      rndCalculator_(ext::make_shared<GBSMRNDCalculator>(bsProcess)) {}

    Real SquareRootCLVModel::cdf(const Date& d, Real k) const {
        return rndCalculator_->cdf(k, bsProcess_->time(d));
    }

    Real SquareRootCLVModel::invCDF(const Date& d, Real q) const {
        return rndCalculator_->invcdf(q, bsProcess_->time(d));
    }

    std::pair<Real, Real> SquareRootCLVModel::nonCentralChiSquaredParams(
        const Date& d) const {

        const Time t = bsProcess_->time(d);

        const Real kappa = sqrtProcess_->a();
        const Real theta = sqrtProcess_->b();
        const Real sigma = sqrtProcess_->sigma();

        const Real df  = 4*theta*kappa/(sigma*sigma);
        const Real ncp = 4*kappa*std::exp(-kappa*t)
            / (sigma*sigma*(1-std::exp(-kappa*t)))*sqrtProcess_->x0();

        return std::make_pair(df, ncp);
    }

    Array SquareRootCLVModel::collocationPointsX(const Date& d) const {

        const std::pair<Real, Real> p = nonCentralChiSquaredParams(d);

        Array x = GaussianQuadrature(lagrangeOrder_,
            GaussNonCentralChiSquaredPolynomial(p.first, p.second))
             .x();

        std::sort(x.begin(), x.end());

        const boost::math::non_central_chi_squared_distribution<Real>
            dist(p.first, p.second);

        const Real xMin = std::max(x.front(),
            (pMin_ == Null<Real>())
                ? 0.0 : boost::math::quantile(dist, pMin_));

        const Real xMax = std::min(x.back(),
            (pMax_ == Null<Real>())
            ? QL_MAX_REAL : boost::math::quantile(dist, pMax_));

        const Real b = xMin - x.front();
        const Real a = (xMax - xMin)/(x.back() - x.front());

        for (double& i : x) {
            i = a * i + b;
        }

        return x;
    }

    Array SquareRootCLVModel::collocationPointsY(const Date& d) const {

        const Array x = collocationPointsX(d);
        const std::pair<Real, Real> params = nonCentralChiSquaredParams(d);
        const boost::math::non_central_chi_squared_distribution<Real>
            dist(params.first, params.second);

        Array s(x.size());
        for (Size i=0, n=s.size(); i < n; ++i) {
            const Real q = boost::math::cdf(dist, x[i]);

            s[i] = invCDF(d, q);
        }

        return s;
    }

    ext::function<Real(Time, Real)> SquareRootCLVModel::g() const {
        calculate();
        return g_;
    }

    void SquareRootCLVModel::performCalculations() const {
        g_ = ext::function<Real(Time, Real)>(MappingFunction(*this));
    }

    SquareRootCLVModel::MappingFunction::MappingFunction(
        const SquareRootCLVModel& model)
    : s_(ext::make_shared<Matrix>(
         model.maturityDates_.size(), model.lagrangeOrder_)),
      x_(ext::make_shared<Matrix>(
         model.maturityDates_.size(), model.lagrangeOrder_)) {

        std::vector<Date> maturityDates = model.maturityDates_;
        std::sort(maturityDates.begin(), maturityDates.end());

        const ext::shared_ptr<GeneralizedBlackScholesProcess>&
            bsProcess = model.bsProcess_;

        for (Size i=0, n = maturityDates.size(); i < n; ++i) {
            const Date maturityDate = maturityDates[i];

            const Array x = model.collocationPointsX(maturityDate);
            const Array y = model.collocationPointsY(maturityDate);

            std::copy(x.begin(), x.end(), x_->row_begin(i));
            std::copy(y.begin(), y.end(), s_->row_begin(i));

            const Time maturity = bsProcess->time(maturityDate);

            interpl.insert(
                std::make_pair(maturity,
                    ext::make_shared<LagrangeInterpolation>(
                        x_->row_begin(i), x_->row_end(i),
                        s_->row_begin(i))));
        }
    }

    Real SquareRootCLVModel::MappingFunction::operator()(Time t,Real x) const {
        const interpl_type::const_iterator ge = interpl.lower_bound(t);

        if (close_enough(ge->first, t)) {
            return (*ge->second)(x, true);
        }

        QL_REQUIRE(ge != interpl.end() && ge != interpl.begin(),
             "extrapolation to large or small t is not allowed");

        const Time t1 = ge->first;
        const Real y1 = (*ge->second)(x, true);

        interpl_type::const_iterator lt = ge;
        std::advance(lt, -1);

        const Time t0 = lt->first;
        const Real y0 = (*lt->second)(x, true);

        return y0 + (y1 - y0)/(t1 - t0)*(t - t0);
    }
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_REM_PIO2_CEPHES_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_REM_PIO2_CEPHES_HPP_INCLUDED

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/quadrant.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/twoopi.hpp>
#include <boost/simd/detail/constant/pio2_1.hpp>
#include <boost/simd/detail/constant/pio2_2.hpp>
#include <boost/simd/detail/constant/pio2_3.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( rem_pio2_cephes_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
//    using i_t = bd::as_integer_t<A0>;
    using result_t = std::pair<A0, A0>              ;

    BOOST_FORCEINLINE result_t operator() ( A0 const& x) const
    {
      A0 xi =  bs::nearbyint(x*bs::Twoopi<A0>());
      A0 xr  = fnms(xi, bs::Pio2_1<A0>(), x);
      xr -= xi*bs::Pio2_2<A0>();
      xr -= xi*bs::Pio2_3<A0>();
      return {quadrant(xi), xr};
    }
  };

} } }

#endif


// solving A * X = B
// using driver function gesv()

#include <cstddef>
#include <iostream>
#include <vector>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

#if !defined(TEST_MATLIB_UBLAS) && !defined(TEST_MATLIB_GLAS) && !defined(TEST_MATLIB_MTL) && !defined(TEST_MATLIB_EIGEN)
#define TEST_MATLIB_UBLAS
#endif

#if defined(TEST_MATLIB_UBLAS)

#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
namespace ublas = boost::numeric::ublas;
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef std::size_t size_type;

#elif defined(TEST_MATLIB_GLAS)

#include <boost/numeric/bindings/glas/dense_matrix.hpp>
#include <glas/toolbox/la/algorithm/operators.hpp>
using namespace glas::la;
typedef glas::dense_matrix<double, glas::column_orientation> m_t;
typedef std::ptrdiff_t size_type;

#elif defined(TEST_MATLIB_MTL)

#include <boost/numeric/bindings/mtl/dense2D.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
typedef mtl::dense2D<double, mtl::matrix::parameters<mtl::tag::col_major> > m_t;
typedef std::ptrdiff_t size_type;

#elif defined(TEST_MATLIB_EIGEN)

#include <boost/numeric/bindings/eigen/matrix.hpp>
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> m_t;
typedef int size_type;

#endif

#include "utils.h"
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;
using std::cout;
using std::endl;

int main() {

  cout << endl;

  size_type n = 5;
  m_t a (n, n);   // system matrix

  size_type nrhs = 2;
  m_t x (n, nrhs), b (n, nrhs);  // b -- right-hand side matrix

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  m_t aa (a); // copy of a, because a is `lost' after gesv()

#if defined(TEST_MATLIB_UBLAS)
  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  for (int i = 0; i < xc0.size(); ++i) {
    xc0 (i) = 1.;
    xc1 (i) = 2.;
  }
  b = prod (a, x);
#elif defined(TEST_MATLIB_GLAS) || defined(TEST_MATLIB_MTL) || defined(TEST_MATLIB_EIGEN)
  for (int i = 0; i < bindings::size_row (x); ++i) {
    x (i,0) = 1.;
    x (i,1) = 2.;
  }
  b = a * x;
#endif

  print_m (a, "A");
  cout << endl;
  print_m (b, "B");
  cout << endl;

//  lapack::gesv (a, b);  // solving the system, b contains x
//  no ipiv less version is currently provided, so fall back to using ipiv
  std::vector<fortran_int_t> ipiv(n);
  lapack::gesv (a, ipiv, b);  // solving the system, b contains x

  print_m (b, "X");
  cout << endl;

#if defined(TEST_MATLIB_UBLAS)
  x = prod (aa, b);
#elif defined(TEST_MATLIB_GLAS) || defined(TEST_MATLIB_MTL) || defined(TEST_MATLIB_EIGEN)
  x = aa * b;
#endif
  print_m (x, "B = A X");

  cout << endl;

}


#include <Eigen/Core>
#include <iostream>

using namespace Eigen;

// [functor]
template<class ArgType, class RowIndexType, class ColIndexType>
class indexing_functor {
  const ArgType &m_arg;
  const RowIndexType &m_rowIndices;
  const ColIndexType &m_colIndices;
 public:
  typedef Matrix<typename ArgType::Scalar,
                 RowIndexType::SizeAtCompileTime,
                 ColIndexType::SizeAtCompileTime,
                 ArgType::Flags & RowMajorBit ? RowMajor : ColMajor,
                 RowIndexType::MaxSizeAtCompileTime,
                 ColIndexType::MaxSizeAtCompileTime> MatrixType;

  indexing_functor(const ArgType &arg, const RowIndexType &row_indices, const ColIndexType &col_indices)
      : m_arg(arg), m_rowIndices(row_indices), m_colIndices(col_indices) {}

  const typename ArgType::Scalar &operator()(Index row, Index col) const {
    return m_arg(m_rowIndices[row], m_colIndices[col]);
  }
};
// [functor]

// [function]
template<class ArgType, class RowIndexType, class ColIndexType>
CwiseNullaryOp<indexing_functor<ArgType, RowIndexType, ColIndexType>, typename indexing_functor<ArgType,
                                                                                RowIndexType,
                                                                                ColIndexType>::MatrixType>
indexing(const Eigen::MatrixBase<ArgType> &arg, const RowIndexType &row_indices, const ColIndexType &col_indices) {
  typedef indexing_functor<ArgType, RowIndexType, ColIndexType> Func;
  typedef typename Func::MatrixType MatrixType;
  return MatrixType::NullaryExpr(row_indices.size(), col_indices.size(), Func(arg.derived(), row_indices, col_indices));
}
// [function]

int main() {
  std::cout << "[main1]\n";
  Eigen::MatrixXi A = Eigen::MatrixXi::Random(4, 4);
  Array3i ri(1, 2, 1);
  ArrayXi ci(6);
  ci << 3, 2, 1, 0, 0, 2;
  Eigen::MatrixXi B = indexing(A, ri, ci);
  std::cout << "A =" << std::endl;
  std::cout << A << std::endl << std::endl;
  std::cout << "A([" << ri.transpose() << "], [" << ci.transpose() << "]) =" << std::endl;
  std::cout << B << std::endl;
  std::cout << "[main1]\n";

  std::cout << "[main2]\n";
  B = indexing(A, ri + 1, ci);
  std::cout << "A(ri+1,ci) =" << std::endl;
  std::cout << B << std::endl << std::endl;
#if __cplusplus >= 201103L
  B = indexing(A, ArrayXi::LinSpaced(13, 0, 12).unaryExpr([](int x) { return x % 4; }), ArrayXi::LinSpaced(4, 0, 3));
  std::cout << "A(ArrayXi::LinSpaced(13,0,12).unaryExpr([](int x){return x%4;}), ArrayXi::LinSpaced(4,0,3)) ="
            << std::endl;
  std::cout << B << std::endl << std::endl;
#endif
  std::cout << "[main2]\n";
}