#include "check_particle.h"
#include "opic_fwd.h"
#include "config.h"
#include "io_utilities.h"
#include "grid.h"
#include "particles.h"
#include "gather_scatter.h"
#include "call_lua_function.h"

#include <boost/format.hpp>
#include <omp.h>
#include <stdexcept>

 /*********************************************************
 * Check if particle is active.                           *
 * equation of motion is solved for active particles only *
 * return true if particles is active, otherwise - false  *
 *********************************************************/

bool is_particle_can_scatter(Particles& particles, index_t p, const Grid& grid, const DblVector& dr)
{
    if (particles.is_inactive(p))
        return false;

    const Particle& particle = particles[p];
    const double h = PIC::Config::h();
    const index_t pi = static_cast<index_t>(floor((particle.r.x + dr.x)/h));
    const index_t pj = static_cast<index_t>(floor((particle.r.y + dr.y)/h));
    const index_t pk = static_cast<index_t>(floor((particle.r.z + dr.z)/h));
    const PIC::CellState home_cell_state = grid(pi, pj, pk).state();

    if (pi < 1 || pi > grid.size_x() - 2 ||
        pj < 1 || pj > grid.size_y() - 2 ||
        pk < 1 || pk > grid.size_z() - 2 ||
        home_cell_state == PIC::cs_absorptive)
    {
        particles.remove_later(p);
        return false;
    }

    return true;
}

bool is_particle_can_move(Particles& particles, index_t p, const Grid& grid)
{
    if (!is_particle_can_scatter(particles, p, grid, DblVector()))
        return false;

    Particle& particle = particles[p];
    const double h = PIC::Config::h();
    const index_t pi = static_cast<index_t>(floor(particle.r.x/h));
    const index_t pj = static_cast<index_t>(floor(particle.r.y/h));
    const index_t pk = static_cast<index_t>(floor(particle.r.z/h));
    const PIC::CellState home_cell_state = grid(pi,pj,pk).state();

    if (home_cell_state == PIC::cs_active)
        return true;

    if (home_cell_state == PIC::cs_custom)
        return lua_validate_particle(particle);

    // never reach here
    return false;
}

bool check_particle_move(const Particle& particle, const Grid& grid, const DblVector& dr)
{
    if (PIC::Config::CFL_severity() == PIC::Ignore)
        return true;

    const double h = PIC::Config::h();
    const double h_2 = PIC::Config::h_2();

    if (fabs(dr.x) > h_2 || fabs(dr.y) > h_2 || fabs(dr.z) > h_2)
    {
        const index_t step = PIC::Config::current_time_step();

        Grid::NodeType point_val;
        PIC::from_grid_to_point(grid, particle.r, point_val);

        const index_t pi = static_cast<index_t>(particle.r.x/h);
        const index_t pj = static_cast<index_t>(particle.r.y/h);
        const index_t pk = static_cast<index_t>(particle.r.z/h);

        const std::string msg = boost::str(boost::format("Step = %1%:\n\tError CFL, particle shift dr should be  < h/2:"
                                                  "\n\tparticle's home cell = (%2%, %3%, %4%);"
                                                  "\n\tgrid size = (%5% x %6% x %7%);"
                                                  "\n\tdr = (%8%, %9%, %10%),\n\tdr.abs() = %11%; h/2 = %12%;"
                                                  "\n\tparticle [%13%] at (%14%, %15%, %16%)"
                                                  "\n\tgrid values at particle position:"
                                                  "\n\t\t B = %17%, Bx = %18%, By = %19%, Bz = %20%"
                                                  "\n\t\t E = %21%, Ex = %22%, Ey = %23%, Ez = %24%"
                                                  "\n\t\t NP = %25%, UPx = %26%, UPy = %27%, UPz = %28%"
                                                  "\n\t\t UEx = %29%, UEy = %30%, UEz = %31%"
                                                  "\n\t\t cell state = %32% (0 - active, 1 - absorptive, 2 - custom)")
                                                  % step
                                                  % pi % pj % pk
                                                  % grid.size_x() % grid.size_y() % grid.size_z()
                                                  % dr.x % dr.y % dr.z % (dr.abs())
                                                  % h_2
                                                  % particle.group_name
                                                  % particle.r.x % particle.r.y % particle.r.z
                                                  % point_val.B.abs() % point_val.B.x % point_val.B.y % point_val.B.z
                                                  % point_val.E.abs() % point_val.E.x % point_val.E.y % point_val.E.z
                                                  % point_val.NP % point_val.UP.x % point_val.UP.y % point_val.UP.z
                                                  % point_val.UE.x % point_val.UE.y % point_val.UE.z
                                                  % grid(pi,pj,pk).state());

        const int tid = omp_get_thread_num();
        const std::string log_file_name = str(boost::format("opic_thread_%1%_check_particle_move_err.log") % tid);
        std::ofstream ofs_log(log_file_name.c_str(), std::ios_base::app);
        ofs_log << msg << std::endl;
        return false;
    }

    return true;
}


// -*- C++ -*-

// The MIT License (MIT)
//
// Copyright (c) 2017 Alexander Samoilov
//
// 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.

// ==========================================
//  FDLIB, CFDLAB, BEMLIB
//
//  Copyright by C. Pozrikidis, 1999
//  All rights reserved.
//
//  This program is to be used only under the
//  stipulations of the licensing agreement.
// ==========================================
//
// ---------------------------------------------------
//  Axisymmetric streaming (uniform)
//  potential flow past a stationary body.
//
//  This program solves
//  an integral equation of the second kind
//  for the disturbance harmonic potential
//  over the body contour
//  and computes streamlines
//
//  The contour of the body in the xy upper half-plane
//  consists of a Number of SeGments (NSG)
//
//  The segments may be straight lines or circular arcs.
//
//  Each segment is discretized into a number of elements
//
//
//  Symbols:
//  --------
//
//  NSG: Number of segments defining the body contour
//
//  NE(i): Number of elements on the ith segment
//
//  RT(i): Stretch ratio of elements on ith segment
//
//  Itp(i): Index for shape of the ith segment:
//          1 for a straight segment
//          2 for a circular arc
//
//  (Xe, Ye):  end-nodes of elements on a segment
//  (Xm, Ym):  mid-nodes of elements on a segment
//  (Xw, Yw):  end-nodes of elements on all segments
//
//  (X0, Y0):  coordinates of collocation points
//
//  T0(i): angle subtended from the center of a circular element
//         at ith collocation point
//
//  arel(i):  axisymmetric surface area of ith element
//
//  phi: disturbance potential at collocation points
//
//  dphidn0: normal derivative of disturbance potential
//                  at collocation points
//
//  dphids0: tangential derivative of potential
//                  at collocation points
//
//  cp:      pressure coefficient at collocation points
//
//  NGL: Number of Gaussian points for integration
//       over each element
//
//  Icross: Index for stopping the computation of the streamlines
//          Default value is 0
//          If Icross = 1, computation stops when a streamline
//          crosses the yz plane
//
//  Notes:
//  ------
//
//  Normal vector points into the flow
//
// ----------------------------

#include <iostream>
#include <fstream>
#include <string>
#include <chrono>
#include <boost/program_options.hpp>
#include "parameters.hpp"
#include "program_options.hpp"
#include "lin_alg_types.hpp"
#include "body_ax_geo.hpp"
#include "body_ax_sdlp.tcc"
#include "body_ax_vel.tcc"

program_options parse_command_line(int argc, char** argv)
{
    namespace po = boost::program_options;
    po::options_description desc("allowed options");
    desc.add_options()
            ("help",                   "describe arguments")
            ("verbose",                "be verbose")
            ("flow_type",              po::value<int>(), "legacy Fortran constants for flow type, 50 for sphere, 51 for torus")
            ("input_data",             po::value<std::string>(), "name of the input file e.g. `sphere.dat`, `torus_trgl.dat`")
            ("asy_name",               po::value<std::string>(), "dump streamlines to vector `.asy` file if name is given");
    try
    {
        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);
        program_options popt;

        if (vm.count("help")) {
            std::cout << desc << std::endl;
            std::exit(1);
        }

        popt.verbose = vm.count("verbose");
        if (vm.count("flow_type")) {
            popt.flow_type = vm["flow_type"].as<int>();
        }
        if (vm.count("input_data")) {
            popt.input_data = vm["input_data"].as<std::string>();
        }
        if (vm.count("asy_name")) {
            popt.asy_name = vm["asy_name"].as<std::string>();
        }

        return popt;
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << std::endl;
        std::cout << desc << std::endl;
        std::exit(-1);
    }
}

using Time = std::chrono::high_resolution_clock;
using ms = std::chrono::milliseconds;
using fsec = std::chrono::duration<float>;

int main(int argc, char **argv)
{
    auto popt = parse_command_line(argc, argv);
    if (popt.verbose) {
        std::cout << popt;
    }

    matg_t<FLOATING_TYPE> phi;
    vecg_t<FLOATING_TYPE> velt, veln, cp;
    matg_t<FLOATING_TYPE> al;      // for the linear system
    vecg_t<FLOATING_TYPE> bl, sol; // ditto

    auto const& run_params = body_ax_geo<FLOATING_TYPE>(popt);
    if (popt.verbose) {
        std::cout << "-I- ngl: " << run_params.ngl << "\n";
        std::cout << "-I- vx: " << run_params.vx << "\n";
        std::cout << "-I- cr: " << run_params.cr << "\n";
        std::cout << "-I- ne[0]: " << run_params.ne[0] << "\n";
        std::cout << "-I- xwmin: " << run_params.xwmin << "\n";
        std::cout << "-I- ywmin: " << run_params.ywmin << "\n";
    }

    // ...

    //---------------------------------------------
    // Generate the linear system
    // for the potential at the collocation points
    //
    // Generate the influence matrix
    // consisting of integrals of the
    // single-layer potential
    //
    // Compute the rhs by evaluating the dlp
    //-------------------------------------------------------

    // resizing
    al.resize(run_params.ncl, run_params.ncl);
    bl.resize(run_params.ncl);
    // stiffness matrix and rhs
    for (int i = 0; i < run_params.ncl; ++i) {           // loop over collocation points
        bl(i) = ZERO<FLOATING_TYPE>;
        int j = -1;
        for (int k = 0; k < run_params.nsg; ++k) {       // loop over segments
            FLOATING_TYPE rad = NaN<FLOATING_TYPE>, xcnt = NaN<FLOATING_TYPE>, ycnt = NaN<FLOATING_TYPE>;
            if (run_params.itp[k] == 2) {
                rad  = run_params.actis[k];
                xcnt = run_params.xcntr[k];
                ycnt = run_params.ycntr[k];
            }
            for (int l = 0; l < run_params.ne[k]; ++l) { // loop over elements
                FLOATING_TYPE x1 = run_params.xw(k,l);
                FLOATING_TYPE y1 = run_params.yw(k,l);
                FLOATING_TYPE t1 = run_params.tw(k,l);

                FLOATING_TYPE x2 = run_params.xw(k,l+1);
                FLOATING_TYPE y2 = run_params.yw(k,l+1);
                FLOATING_TYPE t2 = run_params.tw(k,l+1);

                j = j+1;
                bool ising = (i == j);
                FLOATING_TYPE qqq, www;
                body_ax_sdlp (run_params.x0[i], run_params.y0[i], run_params.t0[i],
                              x1, y1, t1, x2, y2, t2, run_params.gl, ising,
                              run_params.itp[k], rad,xcnt,ycnt,
                              qqq, www);

                al(i, j) = www;
                bl(i) += qqq * run_params.dphidn0[j];

            }

        }
        al(i, i) -= HALF<FLOATING_TYPE>;

    }

    auto t0 = Time::now();
    //------------------------
    // Solve the linear system
    //------------------------
    sol = al.lu().solve(bl.transpose());
    auto t1 = Time::now();
    fsec fs = t1 - t0;
    ms d = std::chrono::duration_cast<ms>(fs);
    std::cout << "linear system solution took " << fs.count() << "s\n";
    std::cout <<  "linear system solution took " << d.count() << "ms\n";

    if (popt.verbose) {
        for (size_t i = 0; i < run_params.ncl; ++i) {
            std::cout << "sol(" << i << ") : " << sol(i) << std::endl;
        }
    }

    //------------------------
    // Distribute the solution
    //------------------------

    size_t max_elems = run_params.ne.maxCoeff();
    phi.resize(run_params.nsg, max_elems);
    velt.resize(run_params.nsg * max_elems);
    veln.resize(run_params.nsg * max_elems);
    cp.resize(run_params.nsg * max_elems);

    size_t k = 0;        // counter
    for (size_t i = 0; i < run_params.nsg; ++i) {
        for (size_t j = 0; j < run_params.ne[i]; ++j) {
            phi(i, j) = sol(k++);
        }
    }

    //------------------------------------------------
    // Compute the tangential disturbance velocity
    //             by taking tangential derivative of \phi
    //
    // Compute the pressure coefficient cp
    //             drag force
    //------------------------------------------------

    FLOATING_TYPE forcex = ZERO<FLOATING_TYPE>, dphids0;
    k = 0;        // counter
    for (size_t i = 0; i < run_params.nsg; ++i) {
        for (size_t j = 0; j < run_params.ne[i]; ++j) {
            //--------------------------------
            // tangential disturbance velocity
            // computed by differentiating phi
            //--------------------------------
            if (j == 0) {                          // forward differencing
                dphids0 = (phi(i,j+1)-phi(i,j))/(run_params.s0(k+1)-run_params.s0(k));
            } else if(j == run_params.ne(i) - 1) { // backward differencing
                dphids0 = (phi(i,j)-phi(i,j-1))/(run_params.s0(k)-run_params.s0(k-1));
            } else {                               // second-order differencing
                FLOATING_TYPE g1 = phi(i,j-1);
                FLOATING_TYPE g2 = phi(i,j);
                FLOATING_TYPE g3 = phi(i,j+1);
                FLOATING_TYPE h1 = run_params.s0(k-1);
                FLOATING_TYPE h2 = run_params.s0(k);
                FLOATING_TYPE h3 = run_params.s0(k+1);
                FLOATING_TYPE aa = ((g3-g2)/(h3-h2)-(g1-g2)/(h1-h2))/(h3-h1);
                FLOATING_TYPE bb = (g3-g2)/(h3-h2)-aa*(h3-h2);
                dphids0 = bb;
            }
            //---------------
            // total velocity
            //---------------

            FLOATING_TYPE velx = run_params.dphidn0(k)*run_params.vnx0(k)+dphids0*run_params.tnx0(k);
            FLOATING_TYPE vely = run_params.dphidn0(k)*run_params.vny0(k)+dphids0*run_params.tny0(k);

            velx = velx + run_params.vx;        // add incident flow

            //---
            // add line vortex ring
            //---

            int iopt = 1;
            FLOATING_TYPE ulvr, vlvr, psi;
            lvr_fs<FLOATING_TYPE>(iopt, run_params.x0[k], run_params.y0[k], run_params.xlvr, run_params.ylvr, ulvr, vlvr, psi);

            velx = velx + run_params.cr*ulvr;
            vely = vely + run_params.cr*vlvr;

            //-------------------------------
            // tangential and normal velocity
            // and pressure coefficient
            //-------------------------------

            // ...
            velt(k) = velx*run_params.tnx0(k)+vely*run_params.tny0(k);
            veln(k) = velx*run_params.vnx0(k)+vely*run_params.vny0(k);

            //------------
            // axial force
            //------------

            cp(k) = ONE<FLOATING_TYPE> - (velt(k)*velt(k)) / (run_params.vx*run_params.vx);

            forcex = forcex + cp(k)*run_params.vnx0(k)*run_params.arel(k);
            if (popt.verbose) {
                std::cout << "velt(" << k << "): " << velt(k) << " veln(" << k << "): " << veln(k)
                          << " cp(" << k << "): " << cp(k) << std::endl;
            }

            ++k;
        }
    }

    std::cout << "Axial Force: " << forcex << std::endl;

    //-------------------
    // print the solution
    //-------------------
    std::ofstream sol_out("body_ax.out");
    if (sol_out) {
        k = 0;
        for (size_t i = 0; i < run_params.nsg; ++i) {
            sol_out << run_params.ne[i] << std::endl;
            for (size_t j = 0; j < run_params.ne[i]; ++j) {
                sol_out << j << " "
                        << run_params.x0(k) << " "
                        << run_params.y0(k) << " "
                        << run_params.s0(k) << " "
                        << phi(i, j) << " " << velt(k) << " " << veln(k) << " " << cp(k) << std::endl;

                ++k;
            }
        }
    }
    sol_out << 0 << std::endl;
    sol_out.close();

    bool detailed_iso = true; //false;
    FLOATING_TYPE dl, ux1, uy1, ux2, uy2;
    int irk;

    if (detailed_iso) {
        dl    = static_cast<FLOATING_TYPE>(0.05) * static_cast<FLOATING_TYPE>(0.125);
        irk   = 4;   // Runge-Kutta 4th order
    } else {
        dl    = static_cast<FLOATING_TYPE>(0.05);
        irk   = 2;
    }

    //--------------------------
    // prepare for crossing test
    //--------------------------
    bool icross;
    if (popt.flow_type == to_underlying(FlowType::SPHERE)) {
        icross = true;
    } else if (popt.flow_type == to_underlying(FlowType::THORUS)) {
        icross = false;
    }

    //--------------------------
    // begin drawing streamlines
    //--------------------------
    std::ofstream fstr;
    fstr.open("body_ax.asy");
    if (fstr) {
        fstr <<
                "import graph;\n"
                "import patterns;\n"
                "size(350,350,IgnoreAspect);\n"
                "add(\"crosshatch\",crosshatch(3mm));\n"
                "\n"
                "pair reflect_y(pair pt)\n"
                "{\n"
                "  return (pt.x, -pt.y);\n"
                "}\n"
                "\n"
                "pair[] reflect_y(pair[] pts)\n"
                "{\n"
                "  pair[] ret;\n"
                "  for (int i = 0; i < pts.length; ++i) {\n"
                "    ret.push((pts[i].x, -pts[i].y));\n"
                "  }\n"
                "  return ret;\n"
                "}\n"
                "\n"
                "pair[][] toru = {\n";

        for (size_t i = 0; i < run_params.nsg; ++i) {
            fstr << "  {\n";
            for (size_t j = 0; j < run_params.ne[i]; ++j) {
                fstr << "(" << run_params.xw(i,j) << ","  << run_params.yw(i,j) << "),\n";
            }
            fstr << "  },\n";
        }

        fstr << "};\n"
                "pair[][] stream_lines = {\n";
    }

    size_t n_streamlines = run_params.x00.size();
    constexpr size_t mstr = 1200;
    std::vector<FLOATING_TYPE> xstr, ystr; // for streamlines
    xstr.reserve(mstr);
    ystr.reserve(mstr);
    for (size_t i = 0; i < n_streamlines; ++i) {
        FLOATING_TYPE x00s = run_params.x00[i], y00s = run_params.y00[i];
        FLOATING_TYPE xcross = x00s;   // to be used for crossing check

        //------
        xstr.clear();
        ystr.clear();
        for (size_t k = 0; k < mstr; ++k) {

            xstr.push_back(x00s);
            ystr.push_back(y00s);

            //---------------
            // integrate ODEs
            //---------------

            velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux1,uy1);

            FLOATING_TYPE step = dl / std::sqrt(ux1*ux1 + uy1*uy1);     // set the frozen-time step

            FLOATING_TYPE xsv = x00s;  // save
            FLOATING_TYPE ysv = y00s;  // save

            //----------------------
            if (irk == 2) {
            //----------------------

                FLOATING_TYPE steph = HALF<FLOATING_TYPE> * step;

                x00s = xsv + step * ux1;
                y00s = ysv + step * uy1;

                velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux2,uy2);

                x00s = xsv + steph * (ux1 + ux2);
                y00s = ysv + steph * (uy1 + uy2);

            //---------------------------
            } else if (irk == 4) {
            //---------------------------

                FLOATING_TYPE steph = HALF<FLOATING_TYPE> * step, ux3, uy3, ux4, uy4;
                FLOATING_TYPE step6 = step / FLOATING_TYPE(6.0);

                x00s = xsv + steph * ux1;
                y00s = ysv + steph * uy1;

                velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux2,uy2);

                x00s = xsv + steph * ux2;
                y00s = ysv + steph * uy2;

                velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux3,uy3);

                x00s = xsv + step * ux3;
                y00s = ysv + step * uy3;

                velocity<FLOATING_TYPE>(run_params, phi, run_params.dphidn0, x00s,y00s, ux4,uy4);

                x00s = xsv + step/FLOATING_TYPE(6.0) * (ux1+FLOATING_TYPE(2.0)*ux2+FLOATING_TYPE(2.0)*ux3+ux4);
                y00s = ysv + step/FLOATING_TYPE(6.0) * (uy1+FLOATING_TYPE(2.0)*uy2+FLOATING_TYPE(2.0)*uy3+uy4);

            //-----------
            }
            //-----------

            //---------------------------
            // test for x=0 plane crossing
            //---------------------------

            if (icross) {
                auto test = xcross*x00s;
                if (test < ZERO<FLOATING_TYPE>) {
                    std::cout << " Crossed the x=0 plane: I will stop\n";
                    break; // to the next streamline
                }
            }

            //-------------------------
            // test for sphere crossing
            //-------------------------
            if (icross) {
                FLOATING_TYPE crosss = std::hypot(x00s-run_params.xcntr[0], y00s-run_params.ycntr[0]);
                if (crosss < run_params.actis[0]) {
                    xstr.pop_back();
                    ystr.pop_back();
                    goto finish_streamline;
                }
            }

            //-----------------------
            // window crossing checks
            //-----------------------

            if (x00s < run_params.xwmin) break;
            if (x00s > run_params.xwmax) break;
            if (y00s < run_params.ywmin) break;
            if (y00s > run_params.ywmax) break;

        } // k loop

        xstr.push_back(x00s);
        ystr.push_back(y00s);

finish_streamline:;
        std::cout << " One streamline with " << xstr.size() << " points completed\n";
        if (fstr) {
            fstr << "{\n";
            for (size_t i = 0; i < xstr.size(); ++i) {
                fstr << '(' << xstr[i] << ',' << ystr[i] << "),\n";
            }
            fstr << "},\n";
        }
    }

    if (fstr) {
        fstr <<
            "};\n"
            "\n"
            "filldraw((toru[0][0]--toru[1][0]--toru[2][0]--cycle),magenta+0.7);\n"
            "filldraw((reflect_y(toru[0][0])--reflect_y(toru[1][0])--reflect_y(toru[2][0])--cycle),magenta+0.7);\n"
            "\n"
            "for (int i = 0; i < stream_lines.length; ++i) {\n"
            "  draw(graph(stream_lines[i]),            heavygreen+0.2);\n"
            "  draw(graph(reflect_y(stream_lines[i])), heavygreen+0.2);\n"
            "}\n"
            "\n"
            "real ticks[] = {-2,-1.5,-1,-0.5,0,0.5,1,1.5,2};\n"
            "\n"
            "xaxis(\"X\", BottomTop, LeftTicks(Label(fontsize(6pt)), ticks));\n"
            "yaxis(rotate(90)*\"Y\", LeftRight, RightTicks(Label(fontsize(6pt)), ticks));\n";
    }

}


/*
	Copyright (C) 2013-2014 by Kristina Simpson <sweet.kristas@gmail.com>

	This software is provided 'as-is', without any express or implied
	warranty. In no event will the authors be held liable for any damages
	arising from the use of this software.

	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:

	   1. The origin of this software must not be misrepresented; you must not
	   claim that you wrote the original software. If you use this software
	   in a product, an acknowledgement in the product documentation would be
	   appreciated but is not required.

	   2. Altered source versions must be plainly marked as such, and must not be
	   misrepresented as being the original software.

	   3. This notice may not be removed or altered from any source
	   distribution.
*/

#if defined(_MSC_VER) && !defined(_USE_MATH_DEFINES)
#	define _USE_MATH_DEFINES 1
#endif
#include <cmath>
#include <glm/ext.hpp>
#if defined(_MSC_VER)
#if _MSC_VER >= 1800
using std::round;
#else
#include <boost/math/special_functions/round.hpp>
using boost::math::round;
#endif // _MSC_VER >= 1800
#endif // defined(_MSC_VER)

#include "asserts.hpp"
#include "CameraObject.hpp"
#include "DisplayDevice.hpp"
#include "SceneGraph.hpp"
#include "SceneNode.hpp"
#include "WindowManager.hpp"

#include "variant_utils.hpp"

namespace KRE
{
	namespace
	{
		const float default_fov					= 45.0f;
		const float default_horizontal_angle	= float(M_PI);
		const float default_vertical_angle		= 0.0f;
		const float default_speed				= 0.1f;
		const float default_mouse_speed			= 0.005f;
		const float default_near_clip			= 0.1f;
		const float default_far_clip			= 300.0f;

		//static SceneObjectRegistrar<Camera> camera_registrar("camera");
	}

	Camera::Camera(const std::string& name)
		: SceneObject(name),
		  fov_(default_fov),
		  horizontal_angle_(default_horizontal_angle),
		  vertical_angle_(default_vertical_angle),
		  speed_(default_speed),
		  mouse_speed_(default_mouse_speed),
		  near_clip_(default_near_clip),
		  far_clip_(default_far_clip),
		  type_(CAMERA_PERSPECTIVE),
		  ortho_left_(0),
		  ortho_bottom_(0),
		  ortho_top_(0),
		  ortho_right_(0),
		  clip_planes_set_(false),
		  view_mode_(VIEW_MODE_AUTO)
	{
		auto wnd = WindowManager::getMainWindow();
		ortho_top_ = wnd->logicalHeight();
		ortho_right_ = wnd->logicalWidth();

		up_ = glm::vec3(0.0f, 1.0f, 0.0f);
		position_ = glm::vec3(0.0f, 0.0f, 0.7f);
		aspect_ = float(wnd->logicalWidth())/float(wnd->logicalHeight());

		//computeView();
		//computeProjection();
	}

	Camera::Camera(const variant& node)
		: SceneObject(node["name"].as_string()),
		  fov_(default_fov),
		  horizontal_angle_(default_horizontal_angle),
		  vertical_angle_(default_vertical_angle),
		  speed_(default_speed),
		  mouse_speed_(default_mouse_speed),
		  near_clip_(default_near_clip),
		  far_clip_(default_far_clip),
		  type_(CAMERA_PERSPECTIVE),
		  ortho_left_(0),
		  ortho_bottom_(0),
		  ortho_top_(0),
		  ortho_right_(0),
		  view_(1.0f),
		  clip_planes_set_(false),
		  view_mode_(VIEW_MODE_AUTO)
	{
		auto wnd = WindowManager::getMainWindow();
		ortho_top_ = wnd->logicalHeight();
		ortho_right_ = wnd->logicalWidth();

		position_ = glm::vec3(0.0f, 0.0f, 10.0f);
		if(node.has_key("fov")) {
			fov_ = std::min(90.0f, std::max(15.0f, float(node["fov"].as_float())));
		}
		if(node.has_key("horizontal_angle")) {
			horizontal_angle_ = float(node["horizontal_angle"].as_float());
		}
		if(node.has_key("vertical_angle")) {
			vertical_angle_ = float(node["vertical_angle"].as_float());
		}
		if(node.has_key("speed")) {
			speed_ = float(node["speed"].as_float());
		}
		if(node.has_key("mouse_speed")) {
			mouse_speed_ = float(node["mouse_speed"].as_float());
		}
		if(node.has_key("aspect")) {
			aspect_ = float(node["aspect"].as_float());
		} else {
			aspect_ = float(wnd->logicalWidth())/float(wnd->logicalHeight());
		}

		if(node.has_key("position")) {
			ASSERT_LOG(node["position"].is_list() && node["position"].num_elements() == 3,
				"position must be a list of 3 decimals.");
			position_ = glm::vec3(float(node["position"][0].as_float()),
				float(node["position"][1].as_float()),
				float(node["position"][2].as_float()));
		}

		if(node.has_key("type")) {
			if(node["type"].as_string() == "orthogonal") {
				type_ = CAMERA_ORTHOGONAL;
			}
		}
		if(node.has_key("ortho_window")) {
			ASSERT_LOG(node["ortho_window"].is_list() && node["ortho_window"].num_elements() == 4, "Attribute 'ortho_window' must be a 4 element list. left,right,top,bottom");
			ortho_left_ = node["ortho_window"][0].as_int32();
			ortho_right_ = node["ortho_window"][1].as_int32();
			ortho_top_ = node["ortho_window"][2].as_int32();
			ortho_bottom_ = node["ortho_window"][3].as_int32();
		}

		// If lookat key is specified it overrides the normal compute.
		if(node.has_key("lookat")) {
			const variant& la = node["lookat"];
			ASSERT_LOG(la.has_key("position") && la.has_key("target") && la.has_key("up"),
				"lookat must be a map having 'position', 'target' and 'up' as tuples");
			glm::vec3 position(la["position"][0].as_float(),
				la["position"][1].as_float(),
				la["position"][2].as_float());
			glm::vec3 target(la["target"][0].as_float(),
				la["target"][1].as_float(),
				la["target"][2].as_float());
			glm::vec3 up(la["up"][0].as_float(),
				la["up"][1].as_float(),
				la["up"][2].as_float());
			lookAt(position, target, up);
			view_mode_ = VIEW_MODE_MANUAL;
		} else {
			if(type_ != CAMERA_ORTHOGONAL) {
				computeView();
			}
		}
		computeProjection();

		LOG_DEBUG("creating camera of type: " << static_cast<int>(type_));
	}

	Camera::Camera(const std::string& name, int left, int right, int top, int bottom)
		: SceneObject(name),
		  fov_(default_fov),
		  horizontal_angle_(default_horizontal_angle),
		  vertical_angle_(default_vertical_angle),
		  speed_(default_speed),
		  mouse_speed_(default_mouse_speed),
		  near_clip_(default_near_clip),
		  far_clip_(default_far_clip),
		  type_(CAMERA_ORTHOGONAL),
		  ortho_left_(left),
		  ortho_bottom_(bottom),
		  ortho_top_(top),
		  ortho_right_(right),
		  clip_planes_set_(false),
		  view_(1.0f),
		  view_mode_(VIEW_MODE_AUTO)
	{
		up_ = glm::vec3(0.0f, 1.0f, 0.0f);
		position_ = glm::vec3(0.0f, 0.0f, 0.70f);
		aspect_ = float(right - left)/float(top - bottom);

		computeProjection();
	}

	Camera::Camera(const std::string& name, const rect& r)
		: SceneObject(name),
		  fov_(default_fov),
		  horizontal_angle_(default_horizontal_angle),
		  vertical_angle_(default_vertical_angle),
		  speed_(default_speed),
		  mouse_speed_(default_mouse_speed),
		  near_clip_(default_near_clip),
		  far_clip_(default_far_clip),
		  type_(CAMERA_ORTHOGONAL),
		  ortho_left_(r.x()),
		  ortho_bottom_(r.y2()),
		  ortho_top_(r.y()),
		  ortho_right_(r.x2()),
		  clip_planes_set_(false),
		  view_(1.0f),
		  view_mode_(VIEW_MODE_AUTO)
	{
		up_ = glm::vec3(0.0f, 1.0f, 0.0f);
		position_ = glm::vec3(0.0f, 0.0f, 0.70f);
		aspect_ = float(ortho_right_ - ortho_left_)/float(ortho_top_ - ortho_bottom_);

		computeProjection();
	}

	Camera::Camera(const std::string& name, float fov, float aspect, float near_clip, float far_clip)
		: SceneObject(name),
		  fov_(fov),
		  horizontal_angle_(default_horizontal_angle),
		  vertical_angle_(default_vertical_angle),
		  speed_(default_speed),
		  mouse_speed_(default_mouse_speed),
		  near_clip_(near_clip),
		  far_clip_(far_clip),
		  aspect_(aspect),
		  type_(CAMERA_PERSPECTIVE),
		  ortho_left_(0),
		  ortho_bottom_(0),
		  ortho_top_(0),
		  ortho_right_(0),
		  clip_planes_set_(true),
		  view_mode_(VIEW_MODE_AUTO)
	{
		auto wnd = WindowManager::getMainWindow();
		ortho_top_ = wnd->logicalHeight();
		ortho_right_ = wnd->logicalWidth();

		up_ = glm::vec3(0.0f, 1.0f, 0.0f);
		position_ = glm::vec3(0.0f, 0.0f, 10.0f);

		computeView();
		computeProjection();
	}

	variant Camera::write()
	{
		variant_builder res;
		if(type_ == CAMERA_PERSPECTIVE) {
			if(fov_ != default_fov) {
				res.add("fov", fov_);
			}
			if(horizontal_angle_ != default_horizontal_angle) {
				res.add("horizontal_angle", horizontal_angle_);
			}
			if(vertical_angle_ != default_vertical_angle) {
				res.add("vertical_angle", vertical_angle_);
			}
			if(speed_ != default_speed) {
				res.add("speed", speed_);
			}
			if(mouse_speed_ != default_mouse_speed) {
				res.add("mouse_speed", mouse_speed_);
			}
			res.add("position", position_.x);
			res.add("position", position_.y);
			res.add("position", position_.z);
		} else {
			res.add("type", "orthogonal");
			res.add("ortho_window", ortho_left_);
			res.add("ortho_window", ortho_right_);
			res.add("ortho_window", ortho_top_);
			res.add("ortho_window", ortho_bottom_);
		}
		if(view_mode_ == VIEW_MODE_MANUAL) {
			variant_builder la_map;
			la_map.add("position", position_.x);
			la_map.add("position", position_.y);
			la_map.add("position", position_.z);
			la_map.add("target", target_.x);
			la_map.add("target", target_.y);
			la_map.add("target", target_.z);
			la_map.add("up", up_.x);
			la_map.add("up", up_.y);
			la_map.add("up", up_.z);
			res.add("lookat", la_map.build());
		}
		return res.build();
	}

	void Camera::computeView()
	{
		view_mode_ = VIEW_MODE_AUTO;
		direction_ = glm::vec3(
			cos(vertical_angle_) * sin(horizontal_angle_),
			sin(vertical_angle_),
			cos(vertical_angle_) * cos(horizontal_angle_)
		);
		right_ = glm::vec3(
			sin(horizontal_angle_ - float(M_PI)/2.0f),
			0,
			cos(horizontal_angle_ - float(M_PI)/2.0f)
		);

		// Up vector
		up_ = glm::cross(right_, direction_);
		target_ = position_ + direction_;

		view_ = glm::lookAt(position_, target_, up_);
		if(frustum_) {
			frustum_->updateMatrices(projection_, view_);
		}
	}

	void Camera::setType(CameraType type)
	{
		type_ = type;
		computeProjection();
	}

	void Camera::setOrthoWindow(int left, int right, int top, int bottom)
	{
		ortho_left_ = left;
		ortho_right_ = right;
		ortho_top_ = top;
		ortho_bottom_ = bottom;

		if(type_ == CAMERA_ORTHOGONAL) {
			computeProjection();
		}
	}

	void Camera::createFrustum()
	{
		attachFrustum(std::make_shared<Frustum>());
	}

	/*BEGIN_DEFINE_CALLABLE_NOBASE(Camera)
	CompressedData(screen_to_world, "(int,int,int=0,int=0) -> [decimal,decimal,decimal]")
		int wx = preferences::actual_screen_width();
		int wy = preferences::actual_screen_height();
		if(NUM_FN_ARGS > 2) {
			wx = FN_ARG(2).as_int();
			if(NUM_FN_ARGS > 3) {
				wy = FN_ARG(3).as_int();
			}
		}
		return vec3_to_variant(obj.screen_to_world(FN_ARG(0).as_int(), FN_ARG(1).as_int(),wx,wy));
	END_DEFINE_FN

	DEFINE_FIELD(position, "[decimal,decimal,decimal]")
		std::vector<variant> v;
		v.push_back(variant(obj.position().x));
		v.push_back(variant(obj.position().y));
		v.push_back(variant(obj.position().z));
		return variant(&v);
	DEFINE_SET_FIELD
		ASSERT_LOG(value.is_list() && value.num_elements() == 3, "position must be a list of 3 elements");
		obj.setPosition(glm::vec3(float(value[0].as_decimal().as_float()),
			float(value[1].as_decimal().as_float()),
			float(value[2].as_decimal().as_float())));
		if(obj.mode_ == MODE_MANUAL) {
			obj.view_ = glm::lookAt(obj.position_, obj.target_, obj.up_);
			obj.frustum_.update_matrices(obj.projection_, obj.view_);
		} else {
			obj.compute_view();
		}

	DEFINE_FIELD(speed, "decimal")
		return variant(obj.speed());
	DEFINE_SET_FIELD
		obj.setSpeed(value.as_decimal().as_float());

	DEFINE_FIELD(right, "[decimal,decimal,decimal]")
		std::vector<variant> v;
		v.push_back(variant(obj.right().x));
		v.push_back(variant(obj.right().y));
		v.push_back(variant(obj.right().z));
		return variant(&v);

	DEFINE_FIELD(direction, "[decimal,decimal,decimal]")
		std::vector<variant> v;
		v.push_back(variant(obj.direction().x));
		v.push_back(variant(obj.direction().y));
		v.push_back(variant(obj.direction().z));
		return variant(&v);

	DEFINE_FIELD(horizontal_angle, "decimal")
		return variant(obj.hangle());
	DEFINE_SET_FIELD
		obj.set_hangle(value.as_decimal().as_float());
		obj.compute_view();

	DEFINE_FIELD(hangle, "decimal")
		return variant(obj.hangle());
	DEFINE_SET_FIELD
		obj.set_hangle(value.as_decimal().as_float());
		obj.compute_view();

	DEFINE_FIELD(vertical_angle, "decimal")
		return variant(obj.vangle());
	DEFINE_SET_FIELD
		obj.set_vangle(value.as_decimal().as_float());
		obj.compute_view();

	DEFINE_FIELD(vangle, "decimal")
		return variant(obj.vangle());
	DEFINE_SET_FIELD
		obj.set_vangle(value.as_decimal().as_float());
		obj.compute_view();

	DEFINE_FIELD(mouse_speed, "decimal")
		return variant(obj.mousespeed());
	DEFINE_SET_FIELD
		obj.set_mousespeed(value.as_decimal().as_float());

	DEFINE_FIELD(target, "[decimal,decimal,decimal]")
		std::vector<variant> v;
		v.push_back(variant(obj.target_.x));
		v.push_back(variant(obj.target_.y));
		v.push_back(variant(obj.target_.z));
		return variant(&v);

	DEFINE_FIELD(up, "[decimal,decimal,decimal]")
		std::vector<variant> v;
		v.push_back(variant(obj.up_.x));
		v.push_back(variant(obj.up_.y));
		v.push_back(variant(obj.up_.z));
		return variant(&v);

	DEFINE_FIELD(fov, "decimal")
		return variant(obj.fov());
	DEFINE_SET_FIELD
		obj.set_fov(value.as_decimal().as_float());

	DEFINE_FIELD(aspect, "decimal")
		return variant(obj.aspect());
	DEFINE_SET_FIELD
		obj.set_aspect(value.as_decimal().as_float());

	DEFINE_FIELD(clip_planes, "[decimal,decimal]")
		std::vector<variant> v;
		v.push_back(variant(obj.near_clip_));
		v.push_back(variant(obj.far_clip_));
		return variant(&v);
	DEFINE_SET_FIELD
		ASSERT_LOG(value.is_list() && value.num_elements() == 2, "clip_planes takes a tuple of two decimals");
		obj.set_clip_planes(value[0].as_decimal().as_float(), value[1].as_decimal().as_float());

	DEFINE_FIELD(type, "string")
		if(obj.type() == ORTHOGONAL_CAMERA) {
			return variant("orthogonal");
		}
		return variant("perspective");
	DEFINE_SET_FIELD
		if(value.as_string() == "orthogonal") {
			obj.setType(ORTHOGONAL_CAMERA);
		} else {
			obj.setType(PERSPECTIVE_CAMERA);
		}

	DEFINE_FIELD(ortho_window, "[int,int,int,int]")
		std::vector<variant> v;
		v.push_back(variant(obj.ortho_left()));
		v.push_back(variant(obj.ortho_right()));
		v.push_back(variant(obj.ortho_top()));
		v.push_back(variant(obj.ortho_bottom()));
		return variant(&v);
	DEFINE_SET_FIELD
		ASSERT_LOG(value.is_list() && value.num_elements() == 4, "Attribute 'ortho_window' must be a 4 element list. left,right,top,bottom");
		obj.set_ortho_window(value[0].as_int(), value[1].as_int(), value[2].as_int(), obj.ortho_bottom_ = value[3].as_int());

	END_DEFINE_CALLABLE(Camera)
	*/

	void Camera::lookAt(const glm::vec3& position, const glm::vec3& target, const glm::vec3& up)
	{
		view_mode_ = VIEW_MODE_MANUAL;
		position_ = position;
		target_ = target;
		up_ = up;
		direction_ = target_ - position_;
		view_ = glm::lookAt(position_, target_, up_);
		if(frustum_) {
			frustum_->updateMatrices(projection_, view_);
		}
	}

	void Camera::setFov(float fov)
	{
		fov_ = fov;
		computeProjection();
	}

	void Camera::setClipPlanes(float z_near, float z_far)
	{
		near_clip_ = z_near;
		far_clip_ = z_far;
		clip_planes_set_ = true;
		computeProjection();
	}

	void Camera::setAspect(float aspect)
	{
		aspect_ = aspect;
		computeProjection();
	}

	void Camera::attachFrustum(const FrustumPtr& frustum)
	{
		frustum_ = frustum;
		if(frustum_) {
			frustum_->updateMatrices(projection_, view_);
		}
	}

	void Camera::computeProjection()
	{
		if(type_ == CAMERA_ORTHOGONAL) {
			if(clip_planes_set_) {
				projection_ = glm::frustum(float(ortho_left_), float(ortho_right_), float(ortho_bottom_), float(ortho_top_), getNearClip(), getFarClip());
			} else {
				projection_ = glm::ortho(float(ortho_left_), float(ortho_right_), float(ortho_bottom_), float(ortho_top_));
			}
		} else {
			projection_ = glm::perspective(getFov(), aspect_, getNearClip(), getFarClip());
		}
		if(frustum_) {
			frustum_->updateMatrices(projection_, view_);
		}
	}

	CameraPtr Camera::clone()
	{
		auto cam = std::make_shared<Camera>(*this);
		// If we have the frustum we make a new version of that as well.
		if(frustum_) {
			cam->frustum_ = std::make_shared<Frustum>(*frustum_);
		}
		return cam;
	}

	// Convert from a screen position (assume +ve x to right, +ve y down) to world space.
	// Assumes the depth buffer was enabled.
	glm::vec3 Camera::screenToWorld(int x, int y, int wx, int wy) const
	{
		// XXX I tend to think this might be better abstracted into DisplayDevice.
		glm::vec4 view_port(0, 0, wx, wy);
		std::vector<float> depth;
		DisplayDevice::getCurrent()->readPixels(x, wy - y, 1, 1, ReadFormat::DEPTH, AttrFormat::FLOAT, depth, wx * sizeof(float));
		glm::vec3 screen(x, wy - y, depth[0]);
		return glm::unProject(screen, view_, projection_, view_port);
	}

	namespace
	{
		float dti(float val)
		{
			return std::abs(val - round(val));
		}
	}

	glm::ivec3 Camera::getFacing(const glm::vec3& coords) const
	{
		if(dti(coords.x) < dti(coords.y)) {
			if(dti(coords.x) < dti(coords.z)) {
				if(direction_.x > 0) {
					return glm::ivec3(-1,0,0);
				} else {
					return glm::ivec3(1,0,0);
				}
			} else {
				if(direction_.z > 0) {
					return glm::ivec3(0,0,-1);
				} else {
					return glm::ivec3(0,0,1);
				}
			}
		} else {
			if(dti(coords.y) < dti(coords.z)) {
				if(direction_.y > 0) {
					return glm::ivec3(0,-1,0);
				} else {
					return glm::ivec3(0,1,0);
				}
			} else {
				if(direction_.z > 0) {
					return glm::ivec3(0,0,-1);
				} else {
					return glm::ivec3(0,0,1);
				}
			}
		}
	}
}


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

/*
 Copyright (C) 2006 Ferdinando Ametrano
 Copyright (C) 2006 StatPro Italia srl
 Copyright (C) 2012 Peter Caspers

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

#ifndef quantlib_test_market_model_hpp
#define quantlib_test_market_model_hpp

#include <ql/qldefines.hpp>
#include <boost/test/unit_test.hpp>
#include "speedlevel.hpp"

/* remember to document new and/or updated tests in the Doxygen
   comment block of the corresponding class */

class MarketModelTest {
  public:
    enum MarketModelType { ExponentialCorrelationFlatVolatility,
        ExponentialCorrelationAbcdVolatility/*, CalibratedMM*/
    };
    static void testInverseFloater();
    static void testPeriodAdapter();
    static void testAllMultiStepProducts();
    static void testOneStepForwardsAndOptionlets();
    static void testOneStepNormalForwardsAndOptionlets();
    static void testCallableSwapNaif();
    static void testCallableSwapLS();
    static void testCallableSwapAnderson(
        MarketModelType marketModel, unsigned testedFactor);
    static void testGreeks();
    static void testPathwiseGreeks();
    static void testPathwiseVegas();
    static void testPathwiseMarketVegas();
    static void testStochVolForwardsAndOptionlets();
    static void testAbcdVolatilityIntegration();
    static void testAbcdVolatilityCompare();
    static void testAbcdVolatilityFit();
    static void testDriftCalculator();
    static void testIsInSubset();
    static void testAbcdDegenerateCases();
    static void testCovariance();
    static boost::unit_test_framework::test_suite* suite(SpeedLevel);
};

#endif


#pragma once

#include <futurehead/lib/config.hpp>
#include <futurehead/lib/locks.hpp>
#include <futurehead/lib/numbers.hpp>
#include <futurehead/lib/utility.hpp>

#include <boost/optional.hpp>
#include <boost/thread/thread.hpp>

#include <atomic>
#include <memory>

namespace futurehead
{
enum class work_version
{
	unspecified,
	work_1
};
std::string to_string (futurehead::work_version const version_a);

class block;
class block_details;
bool work_validate_entry (futurehead::block const &);
bool work_validate_entry (futurehead::work_version const, futurehead::root const &, uint64_t const);

uint64_t work_difficulty (futurehead::work_version const, futurehead::root const &, uint64_t const);

uint64_t work_threshold_base (futurehead::work_version const);
uint64_t work_threshold_entry (futurehead::work_version const);
// Ledger threshold
uint64_t work_threshold (futurehead::work_version const, futurehead::block_details const);

namespace work_v1
{
	uint64_t value (futurehead::root const & root_a, uint64_t work_a);
	uint64_t threshold_base ();
	uint64_t threshold_entry ();
	uint64_t threshold (futurehead::block_details const);
}

double normalized_multiplier (double const, uint64_t const);
double denormalized_multiplier (double const, uint64_t const);
class opencl_work;
class work_item final
{
public:
	work_item (futurehead::work_version const version_a, futurehead::root const & item_a, uint64_t difficulty_a, std::function<void(boost::optional<uint64_t> const &)> const & callback_a) :
	version (version_a), item (item_a), difficulty (difficulty_a), callback (callback_a)
	{
	}
	futurehead::work_version const version;
	futurehead::root const item;
	uint64_t const difficulty;
	std::function<void(boost::optional<uint64_t> const &)> const callback;
};
class work_pool final
{
public:
	work_pool (unsigned, std::chrono::nanoseconds = std::chrono::nanoseconds (0), std::function<boost::optional<uint64_t> (futurehead::work_version const, futurehead::root const &, uint64_t, std::atomic<int> &)> = nullptr);
	~work_pool ();
	void loop (uint64_t);
	void stop ();
	void cancel (futurehead::root const &);
	void generate (futurehead::work_version const, futurehead::root const &, uint64_t, std::function<void(boost::optional<uint64_t> const &)>);
	boost::optional<uint64_t> generate (futurehead::work_version const, futurehead::root const &, uint64_t);
	// For tests only
	boost::optional<uint64_t> generate (futurehead::root const &);
	boost::optional<uint64_t> generate (futurehead::root const &, uint64_t);
	size_t size ();
	futurehead::network_constants network_constants;
	std::atomic<int> ticket;
	bool done;
	std::vector<boost::thread> threads;
	std::list<futurehead::work_item> pending;
	std::mutex mutex;
	futurehead::condition_variable producer_condition;
	std::chrono::nanoseconds pow_rate_limiter;
	std::function<boost::optional<uint64_t> (futurehead::work_version const, futurehead::root const &, uint64_t, std::atomic<int> &)> opencl;
	futurehead::observer_set<bool> work_observers;
};

std::unique_ptr<container_info_component> collect_container_info (work_pool & work_pool, const std::string & name);
}


/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2003-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include <boost/version.hpp>
#include <boost/python.hpp>
#include <boost/noncopyable.hpp>
#include "Python/esys/lsm/geometry/SimpleBlockPy.h"
#include "Python/esys/lsm/geometry/SimpleSphereCollectionPy.h"
#include "Python/esys/lsm/geometry/SimpleSpherePy.h"
#include "Geometry/SimpleBlock.h"

namespace esys
{
  namespace lsm
  {
    SimpleBlockPy::SimpleBlockPy() : SimpleSphereCollectionPy()
    {
    }

    SimpleBlockPy::SimpleBlockPy(
      const boost::python::list &dimCount,
      double radius
    )
      : SimpleSphereCollectionPy()
    {
      SimpleBlockGenerator<SimpleSpherePy>
        generator = SimpleBlockGenerator<SimpleSpherePy>(
            boost::python::extract<int>(dimCount[0]),
            boost::python::extract<int>(dimCount[1]),
            boost::python::extract<int>(dimCount[2]),
            radius
        );
      generator.createParticles(*this);
    }

    using boost::python::arg;
    void exportSimpleBlock()
    {
      // Disable autogeneration of C++ signatures (Boost 1.34.0 and higher)
      // for Epydoc which stumbles over indentation in the automatically generated strings.
      boost::python::docstring_options no_autogen(true,false);

      boost::python::class_<
        SimpleBlockPy,
        boost::python::bases<SimpleSphereCollectionPy>,
        boost::noncopyable
      >(
        "SimpleBlock",
        "A regular packing of spheres.",
        boost::python::init<
          const boost::python::list &,
          boost::python::optional<double>
        >(
          (arg("dimCount"), arg("radius")),
          "Creates a collection of C{dimCount[0]*dimCount[2]*dimCount[2]}"
          " spheres arranged in a regular rectangular lattice packing.\n"
          "@type dimCount: list of 3 elements\n"
          "@kwarg dimCount: number of particles in each packing dimension\n"
          "@type radius: float\n"
          "@kwarg radius: Radius of spheres in the packing\n"
        )
      )
      .def(
        boost::python::init<>()
      )
      ;
    }
  }
}


#include "by_edges_sssp.hpp"
#include "thread_local_allocator.hpp"
#include <boost/heap/fibonacci_heap.hpp>
#include <iostream>

#if defined(CRAUSER_IN) && defined(CRAUSER_INDYN)
#error CRAUSER_IN and CRAUSER_INDYN cannot be combined
#endif

#if defined(CRAUSER_OUT) && defined(CRAUSER_OUTDYN)
#error CRAUSER_OUT and CRAUSER_OUTDYN cannot be combined
#endif

using namespace boost::heap;

namespace {
enum class state : unsigned char { unexplored, fringe, settled };
}

void sssp::by_edges_sssp::run_collective(thread_group& threads,
                                         int thread_rank,
                                         int group_count,
                                         int group_rank,
                                         thread_group& group_threads,
                                         int group_thread_rank,
                                         size_t node_count,
                                         size_t edge_count,
                                         array_slice<const array_slice<const edge>> thread_edges_by_node,
                                         array_slice<double> out_thread_distances,
                                         array_slice<size_t> out_thread_predecessors) {
    BOOST_ASSERT(node_count == thread_edges_by_node.size());
    BOOST_ASSERT(node_count == out_thread_distances.size());
    BOOST_ASSERT(node_count == out_thread_predecessors.size());
    perf_counter perf;

    perf.first_timeblock("init");
    double time = 0;
    auto distances = out_thread_distances;
    auto predecessors = out_thread_predecessors;
    std::vector<state> states(node_count, state::unexplored);
    std::vector<int> last_updated(node_count, INT_MIN);
    std::vector<size_t, thread_local_allocator<size_t>> updated;
    std::vector<size_t, thread_local_allocator<size_t>> todo;

    auto cmp_distance = [&](size_t a, size_t b) { return distances[a] > distances[b]; };
    using distance_queue_t =
        fibonacci_heap<size_t, compare<decltype(cmp_distance)>, allocator<thread_local_allocator<size_t>>>;
    distance_queue_t distance_queue(cmp_distance);
    carray<distance_queue_t::handle_type> distance_queue_handles(node_count);

    threads.single_collective(
        [&] {
            m_global_distances = carray<std::atomic<double>>(node_count);
            m_global_updated = carray<size_t>(node_count * threads.thread_count());
            m_global_updated_at.store(0, std::memory_order_relaxed);
        },
        true);
    for (size_t i = threads.chunk_start(thread_rank, node_count), end = i + threads.chunk_size(thread_rank, node_count);
         i < end;
         ++i) {
        new (&m_global_distances[i]) std::atomic<double>(INFINITY);
    }
    threads.barrier_collective(true);

#if defined(CRAUSER_OUT)
    carray<double> min_outgoing(node_count);
    auto min_outgoing_value = [&](size_t n) { return min_outgoing[n]; };
    auto cmp_crauser_out = [&](size_t a, size_t b) {
        return distances[a] + min_outgoing_value(a) > distances[b] + min_outgoing_value(b);
    };

    using crauser_out_queue_t =
        fibonacci_heap<size_t, compare<decltype(cmp_crauser_out)>, allocator<thread_local_allocator<size_t>>>;
    crauser_out_queue_t crauser_out_queue(cmp_crauser_out);
    carray<crauser_out_queue_t::handle_type> crauser_out_queue_handles(node_count);

    for (size_t i = threads.chunk_start(thread_rank, node_count), end = i + threads.chunk_size(thread_rank, node_count);
         i < end;
         ++i) {
        m_global_distances[i].store(INFINITY, std::memory_order_relaxed);
    }
    threads.barrier_collective(true);

    for (size_t node = 0; node < node_count; ++node) {
        double min_outgoing = INFINITY;
        for (const edge& edge : thread_edges_by_node[node]) {
            if (edge.cost < min_outgoing) {
                min_outgoing = edge.cost;
            }
        }
        if (min_outgoing != INFINITY) {
            atomic_min(m_global_distances[node], min_outgoing);
        }
    }
    threads.barrier_collective(true);

    for (size_t node = 0; node < node_count; ++node) {
        min_outgoing[node] = m_global_distances[node].load(std::memory_order_relaxed);
    }
    threads.barrier_collective(true);
#endif

    for (size_t i = threads.chunk_start(thread_rank, node_count), end = i + threads.chunk_size(thread_rank, node_count);
         i < end;
         ++i) {
        m_global_distances[i].store(INFINITY, std::memory_order_relaxed);
    }
    threads.barrier_collective(true);

    distances[0] = 0.0;
    predecessors[0] = -1;
    states[0] = state::fringe;
    distance_queue_handles[0] = distance_queue.push(0);
#if defined(CRAUSER_OUT)
    crauser_out_queue_handles[0] = crauser_out_queue.push(0);
#endif
    m_global_distances[0].store(0.0, std::memory_order_relaxed);
    threads.barrier_collective(true);

    // Phase has to start at 1 not at 0, because I utilize +phase and -phase to
    // distinguish the two subphases.
    for (int phase = 1;; ++phase) {
        perf.next_timeblock("find_thresholds");
        m_global_updated_at.store(0, std::memory_order_relaxed);
        updated.clear();
        todo.clear();

#if defined(CRAUSER_OUT)
        if (crauser_out_queue.empty()) {
            break;
        }
        const double out_threshold = distances[crauser_out_queue.top()] + min_outgoing_value(crauser_out_queue.top());
        perf.next_timeblock("fill_todo");
        while (!distance_queue.empty() && distances[distance_queue.top()] <= out_threshold) {
            size_t node = distance_queue.top();
            todo.push_back(node);
            distance_queue.pop();
            crauser_out_queue.erase(crauser_out_queue_handles[node]);
        }
#endif

        perf.counter_add("phases", 1);
        perf.next_timeblock("relax");
        for (size_t node : todo) {
            BOOST_ASSERT(states[node] == state::fringe);
            states[node] = state::settled;
            for (const edge& e : thread_edges_by_node[node]) {
                if (states[e.destination] != state::settled && distances[node] + e.cost < distances[e.destination]) {
                    distances[e.destination] = distances[node] + e.cost;
                    predecessors[e.destination] = node;
                    if (states[e.destination] != state::fringe) {
                        states[e.destination] = state::fringe;
                        distance_queue_handles[e.destination] = distance_queue.push(e.destination);
#if defined(CRAUSER_OUT)
                        crauser_out_queue_handles[e.destination] = crauser_out_queue.push(e.destination);
#endif
                    } else {
                        distance_queue.update(distance_queue_handles[e.destination]);
#if defined(CRAUSER_OUT)
                        crauser_out_queue.update(crauser_out_queue_handles[e.destination]);
#endif
                    }
                    if (last_updated[e.destination] != phase) {
                        last_updated[e.destination] = phase;
                        updated.push_back(e.destination);
                    }
                }
            }
        }

        perf.next_timeblock("relax_barrier");
        threads.barrier_collective(true);

        perf.next_timeblock("announce_updated");
        for (size_t node : updated) {
            BOOST_ASSERT(states[node] == state::fringe);
            atomic_min(m_global_distances[node], distances[node]);
            m_global_updated[m_global_updated_at.fetch_add(1, std::memory_order_relaxed)] = node;
        }

        perf.next_timeblock("announce_updated_barrier");
        threads.barrier_collective(true);

        perf.next_timeblock("merge_updated");
        for (size_t i = 0, end = m_global_updated_at.load(std::memory_order_relaxed); i < end; ++i) {
            size_t node = m_global_updated[i];
            BOOST_ASSERT(states[node] != state::settled);
            if (last_updated[node] != -phase) {
                last_updated[node] = -phase;
                double global_distance = m_global_distances[node].load(std::memory_order_relaxed);
                if (global_distance < distances[node]) {
                    distances[node] = global_distance;
                    predecessors[node] = -2;
                    if (states[node] != state::fringe) {
                        states[node] = state::fringe;
                        distance_queue_handles[node] = distance_queue.push(node);
#if defined(CRAUSER_OUT)
                        crauser_out_queue_handles[node] = crauser_out_queue.push(node);
#endif
                    } else {
                        distance_queue.update(distance_queue_handles[node]);
#if defined(CRAUSER_OUT)
                        crauser_out_queue.update(crauser_out_queue_handles[node]);
#endif
                    }
                }
            }
        }

        perf.next_timeblock("merge_updated_barrier");
        threads.barrier_collective(true);
    }

    perf.end_timeblock();
    threads.single_collective([&] { m_perf.resize(threads.thread_count()); }, true);
    m_perf[thread_rank] = std::move(perf);
}


////////////////////////////////////////////////////////////////////
//
// Copyright Vicente J. Botet Escriba 2010
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.
//
////////////////////////////////////////////////////////////////////
#ifndef BOOST_MPL_SIGN_HPP_INCLUDED
#define BOOST_MPL_SIGN_HPP_INCLUDED

#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/aux_/na_spec.hpp>
#include <boost/mpl/aux_/lambda_support.hpp>
#include <boost/mpl/aux_/config/integral.hpp>
#include <boost/mpl/aux_/config/static_constant.hpp>

#if    !defined(BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2) \
    && !defined(BOOST_MPL_PREPROCESSING_MODE) \
    && ( defined(BOOST_MSVC) \
        || BOOST_WORKAROUND(__EDG_VERSION__, <= 238) \
        )

#   define BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2

#endif

namespace boost { namespace mpl {

template< typename Tag > struct sign_impl;

template< typename T > struct sign_tag
{
    typedef typename T::tag type;
};

template<
      typename BOOST_MPL_AUX_NA_PARAM(N)
    >
struct sign
    : sign_impl<
          typename sign_tag<N>::type
        >::template apply<N>::type
{
    BOOST_MPL_AUX_LAMBDA_SUPPORT(1, sign, (N))
};

BOOST_MPL_AUX_NA_SPEC(1, sign)

template<
      typename T
    , T n1
    >
struct sign_c
    : sign<integral_c<T,n1> >
{
};

#if defined(BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2)
namespace aux {
template< typename T, T n > struct sign_wknd
{
    BOOST_STATIC_CONSTANT(T, value = (n == 0 ? 0 : (n < 0 ? -1 : 1)));
    typedef integral_c<T,value> type;
};
}
#endif

template<>
struct sign_impl<integral_c_tag>
{
#if defined(BOOST_MPL_CFG_NO_NESTED_VALUE_ARITHMETIC_2)
    template< typename N > struct apply
        : aux::sign_wknd< typename N::value_type, N::value >
#else
    template< typename N > struct apply
        : integral_c< typename N::value_type, (N::value == 0 ? 0 : (N::value < 0 ? -1 : 1)) >
#endif
    {
    };
};

}}

#endif // BOOST_MPL_SIGN_HPP_INCLUDED


#define BIORBD_API_EXPORTS
#include "Utils/Quaternion.h"

#include <Eigen/Dense>
#include <rbdl/Quaternion.h>

biorbd::utils::Quaternion::Quaternion () :
    RigidBodyDynamics::Math::Quaternion(),
    m_Kstab(100)
{

}

biorbd::utils::Quaternion::Quaternion (const Eigen::Vector4d &vec4) :
    RigidBodyDynamics::Math::Quaternion(vec4),
    m_Kstab(100)
{

}

biorbd::utils::Quaternion::Quaternion (
        double x,
        double y,
        double z,
        double w) :
    RigidBodyDynamics::Math::Quaternion(x, y, z, w),
    m_Kstab(100)
{

}

biorbd::utils::Quaternion::Quaternion (
        const Eigen::Vector3d &vec4,
        double w) :
    RigidBodyDynamics::Math::Quaternion(vec4(0), vec4(1), vec4(2), w),
    m_Kstab(100)
{

}

biorbd::utils::Quaternion& biorbd::utils::Quaternion::operator=(
        const Eigen::Vector4d& vec4)
{
    if (this==&vec4) // check for self-assigment
        return *this;

    *this = vec4;
    return *this;
}

double biorbd::utils::Quaternion::w() const
{
    return (*this)(3);
}
double biorbd::utils::Quaternion::x() const
{
    return (*this)(0);
}
double biorbd::utils::Quaternion::y() const
{
    return (*this)(1);
}
double biorbd::utils::Quaternion::z() const
{
    return (*this)(2);
}

void biorbd::utils::Quaternion::derivate(
        const Eigen::VectorXd &w)
{

    // Création du quaternion de "préproduit vectoriel"
    double qw = (*this)(3);
    double qx = (*this)(0);
    double qy = (*this)(1);
    double qz = (*this)(2);
    Eigen::Matrix4d Q;
    Q <<    qw, -qx, -qy, -qz,
            qx,  qw, -qz,  qy,
            qy,  qz,  qw, -qx,
            qz, -qy,  qx,  qw;

    // Ajout du paramètre de stabilisation
    Eigen::Vector4d w_tp (m_Kstab*w.norm()*(1-this->norm()), w(0), w(1), w(2));
    biorbd::utils::Quaternion quatDot(0.5 * Q * w_tp);
    biorbd::utils::Quaternion quatDot_tp(Eigen::Vector4d(quatDot(1), quatDot(2), quatDot(3), quatDot(0)));
    *this =  quatDot_tp; // Le quaternion est tourné à cause de la facon dont est faite la multiplication
}


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//! \file
//! \ingroup openOR_core
//****************************************************************************

#ifndef openOR_core_math_access_hpp
#define openOR_core_math_access_hpp

#include <boost/mpl/bool.hpp>
#include <openOR/Math/traits.hpp>
#include <openOR/Math/vector.hpp>
#include <openOR/Math/matrix.hpp>

namespace openOR {
   namespace Math {

      namespace Impl {

         /**
          * \internal
          */
         template<class Type, class IsVector>
         struct GetData {

          typename MathTraits<Type>::ConstPointer operator()(const Type& vec)
          {
            BOOST_MPL_ASSERT((typename VectorTraits<Type>::IsVector));
            return &get<0>(vec);
          }
        };

         /**
          * \internal
          */
         template<class Type>
         struct GetData<Type, boost::mpl::false_> {

          typename MathTraits<Type>::ConstPointer operator()(const Type& mat)
          {
            BOOST_MPL_ASSERT((typename MatrixTraits<Type>::IsMatrix));
            return &get<0, 0>(mat);
          }
        };

         /**
          * \internal
          */
         template<class Type, class IsVector>
         struct GetMutableData {

          typename MathTraits<Type>::Pointer operator()(Type& vec)
          {
            BOOST_MPL_ASSERT((typename VectorTraits<Type>::IsVector));
            return &get<0>(vec);
          }
        };

         /**
          * \internal
          */
         template<class Type>
         struct GetMutableData<Type, boost::mpl::false_> {

          typename MathTraits<Type>::Pointer operator()(Type& mat)
          {
            BOOST_MPL_ASSERT((typename MatrixTraits<Type>::IsMatrix));
            return &get<0, 0>(mat);
          }
        };

      }

      /**
       * @brief Read-only access to memory of math types like vectors or matrices.
       * @param var matrix or vector object
       * @return const pointer to the first element of the vector or the matrix
       * @ingroup openOR_core
       */
      template <class Type>
      inline
      typename MathTraits<Type>::ConstPointer
      data(const Type& var) {
        return Impl::GetData<Type, typename VectorTraits<Type>::IsVector>()(var);
      }

      /**
      * @brief Access to memory of math types like vectors or matrices.
      * @param vec matrix or vector object
      * @return pointer to the first element of the vector or the matrix
      * @ingroup openOR_core
      */
      template <class Type>
      inline
      typename MathTraits<Type>::Pointer
      mutableData(Type& vec) {
          return Impl::GetMutableData<Type, typename VectorTraits<Type>::IsVector>()(vec);
      }

   }
}

#endif


/* Copyright (C) 7/30/18 Julian Stobbe - All Rights Reserved
 * You may use, distribute and modify this code under the
 * terms of the MIT license.
 *
 * You should have received a copy of the MIT license with
 * this file.
 */

#ifndef VALUATION_GENRNDER_HPP
#define VALUATION_GENRNDER_HPP

#include <algorithm>

#include "easylogging++.h"
// Tina's Random Number Generator
#include "trng/yarn2.hpp"
#include "trng/uniform01_dist.hpp"
#include "trng/lognormal_dist.hpp"
#include "trng/correlated_normal_dist.hpp"
#include "Eigen/Dense"
#include "Eigen/Sparse"
#include <Eigen/SparseLU>

namespace Utils {

    /*!
     * @brief               Generates matrix from ER model and rejects if Sinkhorn does not converge
     * @param M             Adjacency matrix
     * @param gen_u         0-1-uniform generator
     * @param p             probability of edge (ER model)
     * @param val           sum_i M_ij
     * @param which_to_set  1 equity (left), 2 debt (right), 0 both
     */
    void gen_basic_rejection(Eigen::MatrixXd* M, trng::yarn2& gen_u, const double p,
                             const double val, const int which_to_set);

    /*!
     * @brief               Generates matrix from ER model and does some initial checks before applying sinkhorn
     * @param M             Adjacency matrix
     * @param gen_u         0-1-uniform generator
     * @param p             probability of edge (ER model)
     * @param val           sum_i M_ij
     * @param which_to_set  1 equity (left), 2 debt (right), 0 both
     */
    void gen_sinkhorn(Eigen::MatrixXd* M, trng::yarn2& gen_u, const double p, const double val,
                       const int which_to_set);

    /*!
     * @brief               Generates matrix according to configuration model with fixed degree sequence (all N*p)
     * @param M             Adjacency matrix
     * @param gen_u         0-1-uniform generator
     * @param p             "probability" of edge. Fixed degree N*p
     * @param val           sum_i M_ij
     * @param which_to_set  1 equity (left), 2 debt (right), 0 both
     */
    void gen_configuration_model(Eigen::MatrixXd* M, trng::yarn2& gen_u, const double p, const double val,
                       const int which_to_set);

    /*!
     * @brief               Generates matrix with fixed degree according to algorithm in thesis
     * @param M             Adjacency matrix
     * @param gen_u         0-1-uniform generator
     * @param p             "probability" of edge. Fixed degree N*p
     * @param val           sum_i M_ij
     * @param which_to_set  1 equity (left), 2 debt (right), 0 both
     */
    void gen_fixed_degree(Eigen::MatrixXd* M, trng::yarn2& gen_u, const double p, const double val, const int which_to_set);

    // helper functions
    /*!
     * @brief
     * @param M
     * @return
     */
    Eigen::MatrixXd in_out_degree(Eigen::MatrixXd* M);
    Eigen::MatrixXd avg_row_col_sums(Eigen::MatrixXd* M);

    int fixed_degree(Eigen::MatrixXd* M);

    std::pair<double,double> avg_io_deg(Eigen::MatrixXd* M);

    void fixed_2d(Eigen::MatrixXd* M, const double vs01, const double vs10, const double vr01, const double vr10);

    void gen_ring(Eigen::MatrixXd* M, trng::yarn2& gen_u, const double p, const double val, const int which_to_set);

    void gen_star(Eigen::MatrixXd* M, trng::yarn2& gen_u, const double p, const double val, const int which_to_set);

    void gen_uniform(Eigen::MatrixXd* M, trng::yarn2& gen_u, const double p, const double val, const int which_to_set);
}

#endif //VALUATION_GENRNDER_HPP


#include "extractChessboard.hpp"
#include "extractImageFeature.hpp"
#include "extractLidarFeature.hpp"
#include <pcl/common/geometry.h>
#include <sophus/so3.hpp>
#include <boost/program_options.hpp>
#include <opencv2/core/eigen.hpp>
#include "optimization.hpp"
#include "utils.hpp"
#include "config.hpp"
#include "evaluation.hpp"
using namespace std;
using namespace Eigen;

int main(int argc, char** argv)
{
    string config_file;
    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("config,c", boost::program_options::value<std::string>(&config_file)->default_value("config.yaml"), "config file.");
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);
    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 1;
    }
    Config::loadFromFile(config_file); //
    if(Config::calibType()!= "stereo_lidar"){
        std::cout<< "配置文件应是stereo_lidar类型 ！！"<<std::endl;
        exit(-1);
    }
    std::string left_images_dir = Config::leftImageDatasetPath();
    std::string right_images_dir = Config::rightImageDatasetPath();
    std::string clouds_dir = Config::lidarCloudDatasetPath();
    std::vector<std::string> left_images, right_images;
    get_data_by_path(left_images, left_images_dir, Config::imageFormat());
    get_data_by_path(right_images, right_images_dir, Config::imageFormat());
    std::vector<std::string> clouds;
    get_data_by_path(clouds, clouds_dir, Config::cloudFormat());
    if(!check_data_by_index(left_images, clouds)){
        cerr <<"图像和点云文件应存在且文件名相同！！"<<endl;
        exit(-1);
    }
    if(!check_data_by_index(right_images, clouds)){
        cerr <<"图像和点云文件应存在且文件名相同！！"<<endl;
        exit(-1);
    }

    ImageResults left_images_features, right_images_features;
    processImage(left_images, left_images_features, Config::leftCameraMatrix(), Config::leftCameraDistCoeffs());
    cout<< "有效的左目图像帧有： "<<endl;
    for(auto& id:left_images_features.valid_index){
        cout<< id<<" ";
    }
    cout<<endl;

    processImage(right_images, right_images_features, Config::rightCameraMatrix(), Config::rightCameraDistCoeffs());
    cout<< "有效的右目图像帧有： "<<endl;
    for(auto& id:right_images_features.valid_index){
        cout<< id<<" ";
    }
    cout<<endl;

    CloudResults cloud_features;
    processCloud(clouds, cloud_features);
    cout<< "有效的点云帧有： "<<endl;
    for(auto& id:cloud_features.valid_index){
        cout<< id<<" ";
    }
    cout<<endl;
    getValidDataSet(left_images_features,right_images_features, cloud_features);
    auto& left_image_3d_corners = left_images_features.corners_3d; // 图像3d角点
    auto& left_chessboard_points_2d = left_images_features.chessboard_points_2d;
    auto& left_chessboard_points_3d = left_images_features.chessboard_points_3d;
    auto& right_image_3d_corners = right_images_features.corners_3d;
    auto& right_chessboard_points_2d = right_images_features.chessboard_points_2d;
    auto& right_chessboard_points_3d = right_images_features.chessboard_points_3d;
    auto& cloud_3d_corners = cloud_features.corners_3d; // 点云3d角点
    auto& plane_clouds_3d  = cloud_features.plane_points_3d;

    VectorXd Rt_l1_c1(6), Rt_l1_c2(6), Rt_c1_c2(6);
    calculateInitialRt(cloud_3d_corners, left_image_3d_corners, Rt_l1_c1);
    calculateInitialRt(cloud_3d_corners, right_image_3d_corners, Rt_l1_c2);
    Eigen::Matrix4d mat_l1_c1,mat_l1_c2,mat_c1_c2;
    vector2Matrix(Rt_l1_c1, mat_l1_c1);
    vector2Matrix(Rt_l1_c2, mat_l1_c2);
    mat_c1_c2 =  mat_l1_c2* mat_l1_c1.inverse();
    matrix2Vector(mat_c1_c2, Rt_c1_c2);

    std::cout<< "initial Rt_l1_c1 \n"<< Rt_l1_c1<<std::endl;
    std::cout<< "initial Rt_l1_c2 \n"<< Rt_l1_c2<<std::endl;
    std::cout<< "initial Rt_c1_c2 \n"<< Rt_c1_c2<<std::endl;

    std::cout<< "initial mat_l1_c1 \n"<< mat_l1_c1<<std::endl;
    std::cout<< "initial mat_l1_c2 \n"<< mat_l1_c2<<std::endl;
    std::cout<< "initial mat_c1_c2 \n"<< mat_c1_c2<<std::endl;

    // debug
    // {
    //     for(auto& corners: left_image_3d_corners){
    //         for(auto& corner: corners)
    //         std::cout<< corner.transpose()<<std::endl;
    //     }
    //     exit(17);
    // }
    // debug
    {
        std::cout<<"close up : \n"<< Config::tformL1C1()*Config::tformL1C2().inverse()*Config::tformC1C2() << std::endl;
    }

    // debug
    {
        Rt_c1_c2<<0,0,0,0,0,0;
    }

    Optimizer optimizer_stereo;
    ceres::Problem problem_stereo;
    int n_samples = left_image_3d_corners.size();
    for(int i=0;i<n_samples;i++){
        auto& P1 = left_chessboard_points_3d[i];
        auto& p1  = left_chessboard_points_2d[i];
        auto& P2 = right_chessboard_points_3d[i];
        auto& p2  = right_chessboard_points_2d[i];
        optimizer_stereo.addStereoMatchingConstraints(problem_stereo, P1,p1,P2,p2,Rt_c1_c2);
    }
    ceres::Solver::Options options_stereo;
    options_stereo.max_num_iterations = 100;
    // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    // options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary_stereo;
    ceres::Solve(options_stereo, &problem_stereo, &summary_stereo);
    std::cout << summary_stereo.BriefReport() << "\n";
    // debug
    {
        vector2Matrix(Rt_c1_c2, mat_c1_c2);
        std::cout<<"mat_c1_c2: \n"<< mat_c1_c2<< std::endl;
    }

    // debug
    // {
    //     Rt_c1_c2<<0,0,0,-0.77,0,0;
    // }

    Optimizer optimizer;
    ceres::Problem problem;

    for(int i=0;i<n_samples;++i){
        auto& image_corners = left_image_3d_corners[i];
        auto& cloud_corners = cloud_3d_corners[i];
        optimizer.addPointToPointConstriants(problem,cloud_corners,image_corners,Rt_l1_c1);
    }

    for(int i=0;i<n_samples;++i){
        auto& image_corners = right_image_3d_corners[i];
        auto& cloud_corners = cloud_3d_corners[i];
        optimizer.addPointToPointConstriants(problem,cloud_corners,image_corners,Rt_l1_c2);
    }

    for(int i=0;i<n_samples;i++){
        auto& P1 = left_chessboard_points_3d[i];
        auto& p1  = left_chessboard_points_2d[i];
        auto& P2 = right_chessboard_points_3d[i];
        auto& p2  = right_chessboard_points_2d[i];
        optimizer.addStereoMatchingConstraints(problem, P1,p1,P2,p2,Rt_c1_c2);
    }

    for(int i = 0;i<n_samples;++i){
        optimizer.addClosedLoopConstraints(problem,Rt_l1_c1, Rt_l1_c2, Rt_c1_c2);
    }

    ceres::Solver::Options options;
    options.max_num_iterations = 500;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << "\n";

    std::cout<< "optimized Rt_l1_c1 \n"<< Rt_l1_c1<<std::endl;
    std::cout<< "optimized Rt_l1_c2 \n"<< Rt_l1_c2<<std::endl;
    std::cout<< "optimized Rt_c1_c2 \n"<< Rt_c1_c2<<std::endl;

    std::cout<<"----------------------------"<<std::endl;
    mat_l1_c1.block(0,0,3,3) = Sophus::SO3d::exp(Rt_l1_c1.head(3)).matrix();
    mat_l1_c1.block(0,3,3,1) = Rt_l1_c1.tail(3);
    std::cout << mat_l1_c1 << std::endl;
    std::cout << Config::tformL1C1() << std::endl;
    std::cout << mat_l1_c1-Config::tformL1C1()<<endl;
    std::cout<<"----------------------------"<<std::endl;

    std::cout<<"----------------------------"<<std::endl;
    mat_l1_c2.block(0,0,3,3) = Sophus::SO3d::exp(Rt_l1_c2.head(3)).matrix();
    mat_l1_c2.block(0,3,3,1) = Rt_l1_c2.tail(3);
    std::cout << mat_l1_c2 << std::endl;
    std::cout << Config::tformL1C2() << std::endl;
    std::cout << mat_l1_c2-Config::tformL1C2()<<endl;
    std::cout<<"----------------------------"<<std::endl;

    std::cout<<"----------------------------"<<std::endl;
    mat_c1_c2.block(0,0,3,3) = Sophus::SO3d::exp(Rt_c1_c2.head(3)).matrix();
    mat_c1_c2.block(0,3,3,1) = Rt_c1_c2.tail(3);
    std::cout << mat_c1_c2 << std::endl;
    std::cout << Config::tformC1C2()<<endl;
    std::cout << mat_c1_c2-Config::tformC1C2()<<endl;
    std::cout<<"----------------------------"<<std::endl;

    // debug
    {
        std::cout<<"close up : \n"<< mat_l1_c1*mat_l1_c2.inverse()*mat_c1_c2 << std::endl;
    }

    bool eval = false;
    if(eval){
        // debug
        // {
        //     auto mat = Config::tformL1C1();
        //     Eigen::Matrix3d mat_R = mat.block(0,0,3,3);
        //     Eigen::AngleAxisd aa;
        //     aa.fromRotationMatrix(mat_R);
        //     Rt_l1_c1.head(3) = aa.angle() * aa.axis();
        //     Rt_l1_c1.tail(3) = mat.block(0,3,3,1);
        // }

        cv::Mat rvec = cv::Mat::zeros(cv::Size(1,3), CV_64FC1);
        rvec.at<double>(0,0) = Rt_l1_c1(0);
        rvec.at<double>(1,0) = Rt_l1_c1(1);
        rvec.at<double>(2,0) = Rt_l1_c1(2);
        cv::Mat tvec = cv::Mat::zeros(cv::Size(1,3), CV_64FC1);
        tvec.at<double>(0,0) = Rt_l1_c1(3);
        tvec.at<double>(1,0) = Rt_l1_c1(4);
        tvec.at<double>(2,0) = Rt_l1_c1(5);

        auto& valid_index = cloud_features.valid_index;

        for(int i = 0;i<valid_index.size();++i)
        {
            int k = valid_index[i];
            cv::Mat img = cv::imread(left_images[k], cv::IMREAD_COLOR);

            // display on images
            // projectLidarOnImage(img, plane_clouds_3d[i] , rvec, tvec, Config::leftCameraMatrix(), Config::leftCameraDistCoeffs());
            // cv::imshow("eval", img);
            // cv::waitKey(0);

            // display on cloud
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
            if (!loadPointXYZ(clouds[k], cloud)) continue;
            // debug
            // {
            //     std::vector<pcl::PointCloud<pcl::PointXYZ>> show_cloud;
            //     show_cloud.push_back(*cloud);
            //     show_cloud.push_back(plane_clouds_3d[i]);
            //     display_multi_clouds(show_cloud);
            // }

            auto color_pcd = colorizeCloud(img, *cloud , rvec, tvec, Config::leftCameraMatrix(), Config::leftCameraDistCoeffs());
            boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
            pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(color_pcd);
            viewer->addPointCloud<pcl::PointXYZRGB> (color_pcd, "sample cloud");
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud"); // 设置点云大小
            viewer->addCoordinateSystem (1.0);
            viewer->initCameraParameters ();
            while (!viewer->wasStopped())
            {
                viewer->spinOnce(100);
                sleep(0.1);
            }
        }
    }

    bool compute_error = true;
    if(compute_error){
        auto translation_error = computeTranslationError(left_image_3d_corners, cloud_3d_corners,mat_l1_c1 );
        std::cout<< " translation_error:\n ";
        for(auto& error:translation_error){
            std::cout<< error <<" ";
        }
        std::cout<<std::endl;
        auto rotation_error = computeRotationError(left_image_3d_corners, cloud_3d_corners,mat_l1_c1 );
        std::cout<< " rotation_error:\n ";
        for(auto& error:rotation_error){
            std::cout<< error <<" ";
        }
        std::cout<<std::endl;
        auto reprojection_error = computeReprojectionError(left_image_3d_corners, cloud_3d_corners,mat_l1_c1 );
        std::cout<< " reprojection_error:\n ";
        for(auto& error:reprojection_error){
            std::cout<< error <<" ";
        }
        std::cout<<std::endl;
    }

    return 0;
}


/*
 [auto_generated]
 boost/numeric/odeint/external/mpi/mpi_nested_algebra.hpp

 [begin_description]
 Nested parallelized algebra for MPI.
 [end_description]

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MPI_MPI_NESTED_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_MPI_MPI_NESTED_ALGEBRA_HPP_INCLUDED

#include <boost/numeric/odeint/algebra/norm_result_type.hpp>
#include <boost/numeric/odeint/util/n_ary_helper.hpp>

namespace boost {
namespace numeric {
namespace odeint {

/** \brief MPI-parallelized algebra, wrapping another algebra.
 */
template< class InnerAlgebra >
struct mpi_nested_algebra
{

// execute the InnerAlgebra on each node's local data.
#define BOOST_ODEINT_GEN_BODY(n) \
    InnerAlgebra::for_each##n( \
        BOOST_PP_ENUM_BINARY_PARAMS(n, s, () BOOST_PP_INTERCEPT) , \
        op \
    );
BOOST_ODEINT_GEN_FOR_EACH(BOOST_ODEINT_GEN_BODY)
#undef BOOST_ODEINT_GEN_BODY

    template< class NestedState >
    static typename norm_result_type< typename NestedState::value_type >::type norm_inf( const NestedState &s )
    {
        typedef typename norm_result_type< typename NestedState::value_type >::type result_type;
        // local maximum
        result_type value = InnerAlgebra::norm_inf( s() );
        // global maximum
        return boost::mpi::all_reduce(s.world, value, boost::mpi::maximum<result_type>());
    }

};

}
}
}

#endif


// CG:SHOP 2021 Coordinated Motion Planning - Shadoks Team
//
// Elementary classes
//
/**
 * MIT License
 *
 * Copyright (c) 2020 Guilherme Dias da Fonseca <gfonsecabr@gmail.com>
 *
 * 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.
 */
#ifndef PRIMITIVES
#define PRIMITIVES

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <functional>
#include <string>
#include <iomanip>      // std::put_time
#include <boost/unordered_set.hpp> // hash_combine
#include "tsl/hopscotch_map.h"
#include "tsl/hopscotch_set.h"

typedef short int pint;

using namespace std;

class Point {
public:
  pint x,y;

  Point() {
  }

  Point(pint _x, pint _y) : x(_x), y(_y) {
  }

  int l1(const Point &p = Point(0,0)) const {
    return abs(x-p.x) + abs(y-p.y);
  }

  int linf(const Point &p = Point(0,0)) const {
    return max(abs(x-p.x), abs(y-p.y));
  }

  friend bool operator==(const Point &p, const Point &q) {
    return p.x == q.x && p.y == q.y;
  }

  auto operator<=>(const Point& p) const {
    return make_tuple(x,y) <=> make_tuple(p.x,p.y);
  }

  Point operator-(const Point &p) const {
    return Point(x-p.x, y-p.y);
  }

  Point operator+(const Point &p) const {
    return Point(x+p.x, y+p.y);
  }

  // Dot product
  int operator*(const Point &p) const {
    return x*p.x + y*p.y;
  }

  string toString() const {
    string s = "(" + to_string(x) + "," + to_string(y) + ")";
    return s;
  }

  void rotate(int rot = 1) {
    while(rot < 0)
      rot += 4;
    rot %= 4;
    for(int i = 0; i < rot; i++) {
      swap(x,y);
      x = -x;
    }
  }

  bool inside(Point minxy, Point maxxy) {
    return x >= minxy.x && x <= maxxy.x && y >= minxy.y && y <= maxxy.y;
  }

  friend ostream& operator<<(ostream& os, const Point& p) {
    os << p.toString();
    return os;
  }
};

namespace std {
  template <> struct hash<Point> {
    size_t operator()(const Point& p) const {
      size_t seed = 0;
      boost::hash_combine(seed, p.x);
      boost::hash_combine(seed, p.y);
      return seed;
    }
  };
}

class Move {
public:
  Point p,q;
  int t0;

  Move() {
  }

  Move(Point _p, Point _q, int _t0 = -1) : p(_p), q(_q), t0(_t0) {
  }

  bool compatible(const Move &m) {
    if(m.q == q || m.p == p) {
      return false;
    }
    if(m.p == q || m.q == p) {
      return m.q - m.p == q - p;
    }
    return true;
  }

  string toString() const {
     string s;
     s = p.toString() + "-[" + to_string(t0) + "]->" + q.toString();
     return s;
  }

  friend ostream& operator<<(ostream& os, const Move& m) {
    os << m.toString();
    return os;
  }

  vector<Move> conflicts() const {
    vector<Move> ret;
    const vector<Point> displacements{{Point(0,1), Point(0,-1), Point(1,0), Point(-1,0)}};

    // Append movements that end in q
    for(Point v : displacements) {
      Move m(*this);
      m.p = q + v;
      if(m.p != p) {
        ret.push_back(m);
      }
    }

    if(p.l1(q) != 0) {
      // add movements that end in p but comme from other directions
      for(Point v : displacements) {
        Move m(*this);
        m.p = p + v;
        m.q = p;
        if(p - q != m.p - m.q) {
          ret.push_back(m);
        }
      }
    }

    return ret;
  }

  bool operator==(const Move &m) const = default;

// needs -std=c++20
  auto operator<=>(const Move& m) const {
    return make_tuple(p.x,q.x,p.y,q.y,t0) <=> make_tuple(m.p.x, m.q.x, m.p.y, m.q.y, m.t0);
  }
};

namespace std {
  template <> struct hash<Move> {
    size_t operator()(const Move& m) const {
      size_t seed = 0;
      boost::hash_combine(seed, m.p.x);
      boost::hash_combine(seed, m.p.y);
      boost::hash_combine(seed, m.q.x);
      boost::hash_combine(seed, m.q.y);
      boost::hash_combine(seed, m.t0);
      return seed;
    }
  };
}

class Valuer {
public:
  virtual double get(Point key, int t) {return 1.0;}
};

Valuer theValuer;

class Randomner : public Valuer {
  tsl::hopscotch_map<Point, double> memory;

public:
  virtual double get(Point key, int t) {
    if(memory.contains(key))
      return memory[key];

    double r = ((double) rand() / (RAND_MAX)) + 1.0 ;
    memory[key] = r;
    return r;
  }
};

string timeString() {
  std::time_t tt = std::chrono::system_clock::to_time_t (std::chrono::system_clock::now());
  struct std::tm * ptm = std::localtime(&tt);
  std::ostringstream oss;
  oss << std::put_time(ptm, "%Y%m%d-%H%M%S");
  string s = oss.str();

  return s;
}

const auto start_time = std::chrono::steady_clock::now();

double elapsedSec() {
  auto cur_time = std::chrono::steady_clock::now();
  std::chrono::duration<double> elapsed_seconds = cur_time - start_time;
  return elapsed_seconds.count();
}

#endif


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

#if !defined(OCTOPUS_1B486746_2FA0_455E_B9D5_84D457F3533C)
#define OCTOPUS_1B486746_2FA0_455E_B9D5_84D457F3533C

#include <octopus/assert.hpp>
#include <octopus/array.hpp>

#include <boost/move/move.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/access.hpp>

namespace octopus
{

template <
    typename T
  , boost::uint64_t TotalLength = OCTOPUS_GRID_NODE_LENGTH
                                * OCTOPUS_GRID_NODE_LENGTH
                                * OCTOPUS_GRID_NODE_LENGTH
    >
struct array3d
{
    typedef boost::uint64_t size_type;
    typedef boost::array<T, TotalLength> storage_type;

  private:
    size_type x_length_;
    size_type y_length_;
    size_type z_length_;
    boost::shared_ptr<storage_type> data_;

    BOOST_COPYABLE_AND_MOVABLE(array3d);

    friend class boost::serialization::access;

    template <typename Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
        ar & x_length_ & y_length_ & z_length_ & data_;
    }

    size_type index(size_type x, size_type y, size_type z) const
    {
        OCTOPUS_ASSERT_FMT_MSG(x < x_length_,
            "x coordinate (%1%) is larger than the x length (%2%)",
            x % x_length_);
        OCTOPUS_ASSERT_FMT_MSG(y < y_length_,
            "y coordinate (%1%) is larger than the y length (%2%)",
            y % y_length_);
        OCTOPUS_ASSERT_FMT_MSG(z < z_length_,
            "z coordinate (%1%) is larger than the z length (%2%)",
            z % z_length_);
        return x + y * x_length_ + z * x_length_ * y_length_;
    }

  public:
    array3d() {}

    array3d(
        size_type length
      , T const& dflt = T()
        )
      : x_length_(length)
      , y_length_(length)
      , z_length_(length)
      , data_(new storage_type)
    {
        OCTOPUS_ASSERT(    (x_length_ * y_length_ * z_length_)
                        == TotalLength);

        for (boost::uint64_t i = 0; i < data_->size(); ++i)
            (*data_)[i] = dflt;
    }

    array3d(
        size_type x_length
      , size_type y_length
      , size_type z_length
      , T const& dflt = T()
        )
      : x_length_(x_length)
      , y_length_(y_length)
      , z_length_(z_length)
      , data_(new storage_type)
    {
        OCTOPUS_ASSERT(    (x_length_ * y_length_ * z_length_)
                        == TotalLength);

        for (boost::uint64_t i = 0; i < data_->size(); ++i)
            (*data_)[i] = dflt;
    }

    array3d(array3d const& other)
      : x_length_(other.x_length_)
      , y_length_(other.y_length_)
      , z_length_(other.z_length_)
      , data_(new storage_type)
    {
        for (boost::uint64_t i = 0; i < data_->size(); ++i)
            (*data_)[i] = (*other.data_)[i];
    }

    array3d(BOOST_RV_REF(array3d) other)
      : x_length_(other.x_length_)
      , y_length_(other.y_length_)
      , z_length_(other.z_length_)
      , data_(boost::move(other.data_))
    {}

    array3d& operator=(BOOST_COPY_ASSIGN_REF(array3d) other)
    {
        x_length_ = other.x_length_;
        y_length_ = other.y_length_;
        z_length_ = other.z_length_;

        if (!data_)
            data_.reset(new storage_type);

        for (boost::uint64_t i = 0; i < data_->size(); ++i)
            (*data_)[i] = (*other.data_)[i];

        return *this;
    }

    array3d& operator=(BOOST_RV_REF(array3d) other)
    {
        x_length_ = other.x_length_;
        y_length_ = other.y_length_;
        z_length_ = other.z_length_;
        data_ = boost::move(other.data_);

        return *this;
    }

    array3d& operator=(T const& value)
    {
        if (!data_)
            data_.reset(new storage_type);

        for (size_type i = 0; i < data_->size(); ++i)
            (*data_)[i] = value;

        return *this;
    }

    size_type size() const
    {
        return TotalLength;
    }

    size_type x_length() const
    {
        return x_length_;
    }

    size_type y_length() const
    {
        return y_length_;
    }

    size_type z_length() const
    {
        return z_length_;
    }

    T&
    operator()(size_type x, size_type y, size_type z)
    {
        return (*data_)[index(x, y, z)];
    }

    T const&
    operator()(size_type x, size_type y, size_type z) const
    {
        return (*data_)[index(x, y, z)];
    }

    T&
    operator()(array<size_type, 3> const& xyz)
    {
        return (*this)(xyz[0], xyz[1], xyz[2]);
    }

    T const&
    operator()(array<size_type, 3> const& xyz) const
    {
        return (*this)(xyz[0], xyz[1], xyz[2]);
    }
};

}

#endif // OCTOPUS_1B486746_2FA0_455E_B9D5_84D457F3533C


#include <boost/numeric/odeint/util/multi_array_adaption.hpp>


//==================================================================================================
/*!
  @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_FUNCTION_LOGICAL_NOTAND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOGICAL_NOTAND_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object returns the logical and of the negation of the
    first argument with the second argument

    @par Header <boost/simd/function/logical_notand.hpp>

    @par Note

    - The result type is the logical type associated to the first argument.

    @pre The operands must share the same @ref cardinal_of value.

    @see logical_not,  logical_xor, logical_andnot,  logical_notor,  logical_ornot, logical_and,  logical_or

    @par Example:

      @snippet logical_notand.cpp logical_notand

    @par Possible output:

      @snippet logical_notand.txt logical_notand

  **/
  as_logical_t<Value0> logical_notand(Value0 const& x, Value1 const& y);
} }
#endif

#include <boost/simd/function/scalar/logical_notand.hpp>
#include <boost/simd/function/simd/logical_notand.hpp>

#endif