/*Copyright (c) 2021 James Gayvert

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

/*
 * AOCAP.cpp
 */

#include "AOCAP.h"

#include "grid_radial.h"
#include "bragg.h"
#include <numgrid.h>
#include <omp.h>
#include <chrono>
#include <ctime>
#include <Eigen/Dense>
#include <iomanip>
#include <iostream>
#include <thread>
#include <vector>
#include <cmath>
#include <limits>

#include "BasisSet.h"
#include "gto_ordering.h"
#include "opencap_exception.h"
#include "utils.h"

AOCAP::AOCAP(std::vector<Atom> geometry,std::map<std::string, std::string> params)
{
	verify_cap_parameters(params);
	double capx,capy,capz,rcut,radial,angpts;
	std::stringstream capxss(params["cap_x"]);
	std::stringstream capyss(params["cap_y"]);
	std::stringstream capzss(params["cap_z"]);
	std::stringstream rcutss(params["r_cut"]);
	std::stringstream radialss(params["radial_precision"]);
	std::stringstream angularss(params["angular_points"]);
	capxss >> capx;
	capyss >> capy;
	capzss >> capz;
	rcutss >> rcut;
	radialss >> radial;
	angularss >> angpts;
	//now fill class members
	cap_type = params["cap_type"];
	cap_x = capx;
	cap_y= capy;
	cap_z = capz;
	r_cut = rcut;
	radial_precision = pow(10,-1.0*radial);
	angular_points = angpts;
	atoms = geometry;
    num_atoms = atoms.size();
}

// Smooth Voronoi CAP
// Thommas Sommerfeld and Masahiro Ehara
// DOI: 10.1021/acs.jctc.5b00465
void AOCAP::eval_voronoi_cap(double* x, double* y, double* z, double *grid_w, int num_points,Eigen::VectorXd &cap_values)
{
    #pragma omp parallel for
	for(size_t i=0;i<num_points;i++)
	{
		double atom_distances[num_atoms];
        double r_closest=1000;
		//find r closest and fill up our distances array
		for(size_t j=0;j<num_atoms;j++)
		{
			if(atoms[j].Z!=0)
			{
				  double dist_x= (x[i]-atoms[j].coords[0]) * (x[i]-atoms[j].coords[0]);
				  double dist_y= (y[i]-atoms[j].coords[1]) * (y[i]-atoms[j].coords[1]);
				  double dist_z= (z[i]-atoms[j].coords[2]) * (z[i]-atoms[j].coords[2]);
				  double dist=sqrt(dist_x+dist_y+dist_z);
				  if(dist<r_closest || j==0)
					 r_closest=dist;
				  atom_distances[j]=dist;
			}
		}
		double weights[num_atoms];
		for(size_t q=0;q<num_atoms;q++)
		{
		  if(atoms[q].Z!=0)
          {
              double weight = atom_distances[q]*atom_distances[q]-r_closest*r_closest+1;
              weights[q] = 1/(weight*weight);
          }
		  else
			  weights[q]=0;
		}
		double numerator=0.0;
		double denominator=0.0;
		for(size_t k=0;k<num_atoms;k++)
		{
		  numerator+=atom_distances[k]*atom_distances[k]*weights[k];
		  denominator+=weights[k];
		}
		double r=sqrt(numerator/denominator);
		double result=0;
		if(r>r_cut)
		  result = (r-r_cut)*(r-r_cut);
		cap_values(i) = result * grid_w[i];
	}
}

void AOCAP::eval_box_cap(double* x, double* y, double* z, double *grid_w, int num_points,Eigen::VectorXd &cap_values)
{
	for(size_t i=0;i<num_points;i++)
	{
		double result = 0;
		if(abs(x[i])>cap_x)
		 result += (abs(x[i])-cap_x) * (abs(x[i])-cap_x);
		if(abs(y[i])>cap_y)
		 result += (abs(y[i])-cap_y) * (abs(y[i])-cap_y);
		if(abs(z[i])>cap_z)
		 result += (abs(z[i])-cap_z) * (abs(z[i])-cap_z);
		cap_values(i) = result * grid_w[i];
	}
}

void AOCAP::eval_pot(double* x, double* y, double* z, double *grid_w, int num_points,Eigen::VectorXd &cap_values)
{
	if(compare_strings(cap_type, "box"))
		 eval_box_cap(x,y,z,grid_w,num_points,cap_values);
	else if (compare_strings(cap_type,"voronoi"))
		 eval_voronoi_cap(x,y,z,grid_w,num_points,cap_values);;
}

void AOCAP::compute_ao_cap_mat(Eigen::MatrixXd &cap_mat, BasisSet bs)
{
    std::cout << "Calculating CAP matrix in AO basis using " << std::to_string(omp_get_max_threads()) << " threads." << std::endl;
    std::cout << std::setprecision(2) << std::scientific  << "Radial precision: " << radial_precision
              << " Angular points: " << angular_points << std::endl;
	double x_coords_bohr[num_atoms];
	double y_coords_bohr[num_atoms];
	double z_coords_bohr[num_atoms];
	int nuc_charges[num_atoms];
	for(size_t i=0;i<num_atoms;i++)
	{
		x_coords_bohr[i]=atoms[i].coords[0];
		y_coords_bohr[i]=atoms[i].coords[1];
		z_coords_bohr[i]=atoms[i].coords[2];
		nuc_charges[i]=atoms[i].Z;
		if (atoms[i].Z==0)
			nuc_charges[i]=1; //choose bragg radius for H for ghost atoms
	}
    //double radial_precision = radial_precision;
    int min_num_angular_points = angular_points;
    int max_num_angular_points = angular_points;
	for(size_t i=0;i<num_atoms;i++)
	{

        // check radial parameters
        double alpha_max = bs.alpha_max(atoms[i]);
        std::vector<double> alpha_min = bs.alpha_min(atoms[i]);
        double r_inner = get_r_inner(radial_precision,
                                     alpha_max * 2.0); // factor 2.0 to match DIRAC
        double h = std::numeric_limits<float>::max();
        double r_outer = 0.0;
        for (int l = 0; l <= bs.max_L(); l++)
        {
            if (alpha_min[l] > 0.0)
            {
                r_outer =
                std::max(r_outer,
                         get_r_outer(radial_precision,
                                     alpha_min[l],
                                     l,
                                     4.0 * get_bragg_angstrom(nuc_charges[i])));
                if(r_outer < r_inner)
                {
                    opencap_throw("Error: r_outer < r_inner, grid cannot be allocated for this basis.");
                }
                else
                {
                    h = std::min(h,
                                 get_h(radial_precision, l, 0.1 * (r_outer - r_inner)));
                    if(r_outer < h)
                        opencap_throw("Error: r_outer < h, grid cannot be allocated for this basis.");
                }
            }
        }
        context_t *context = numgrid_new_atom_grid(radial_precision,
		                                 min_num_angular_points,
		                                 max_num_angular_points,
		                                 nuc_charges[i],
		                                 bs.alpha_max(atoms[i]),
		                                 bs.max_L(),
		                                 alpha_min.data());
		int num_points = numgrid_get_num_grid_points(context);
        double *grid_x_bohr = new double[num_points];
        double *grid_y_bohr = new double[num_points];
        double *grid_z_bohr = new double[num_points];
        double *grid_w = new double[num_points];
        numgrid_get_grid(  context,
                           num_atoms,
                           i,
                           x_coords_bohr,
                           y_coords_bohr,
                           z_coords_bohr,
                           nuc_charges,
                           grid_x_bohr,
                           grid_y_bohr,
                           grid_z_bohr,
                           grid_w);
        int num_radial_points = numgrid_get_num_radial_grid_points(context);
    	std::cout << "Grid for atom " + std::to_string(i+1) + " has: " << num_radial_points << " radial points, "
                  << angular_points << " angular points, " << num_points << " total points." << std::endl;
		evaluate_grid_on_atom(cap_mat,bs,grid_x_bohr,grid_y_bohr,grid_z_bohr,grid_w,num_points);
	}
}

void AOCAP::evaluate_grid_on_atom(Eigen::MatrixXd &cap_mat,BasisSet bs,double* grid_x_bohr,
		double *grid_y_bohr,double *grid_z_bohr,double *grid_w,int num_points)
{
	Eigen::VectorXd cap_values; Eigen::MatrixXd bf_values;
	cap_values = Eigen::VectorXd::Zero(num_points);
	bf_values = Eigen::MatrixXd::Zero(num_points,bs.num_carts());
	eval_pot(grid_x_bohr,grid_y_bohr,grid_z_bohr,grid_w,num_points,cap_values);
	size_t bf_idx = 0;
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell my_shell = bs.basis[i];
		std::vector<std::array<size_t,3>> order = opencap_carts_ordering(my_shell.l);
		for(size_t j=0;j<my_shell.num_carts();j++)
		{
			std::array<size_t,3> cart = order[j];
			my_shell.evaluate_on_grid(grid_x_bohr,grid_y_bohr,grid_z_bohr,num_points,cart[0],cart[1],cart[2],bf_values.col(bf_idx));
			bf_idx++;
		}
	}
	Eigen::MatrixXd bf_prime;
	bf_prime =  Eigen::MatrixXd::Zero(num_points,bs.num_carts());
	for(size_t i=0;i<bf_prime.cols();i++)
		bf_prime.col(i) = bf_values.col(i).array()*cap_values.array();
	cap_mat+=bf_prime.transpose()*bf_values;
}

void AOCAP::verify_cap_parameters(std::map<std::string,std::string> &parameters)
{
	std::vector<std::string> missing_keys;
	if(parameters.find("cap_type")==parameters.end())
		opencap_throw("Error: Missing cap_type keyword.");
	if(compare_strings(parameters["cap_type"],"box"))
	{
		if(parameters.find("cap_x")==parameters.end())
			missing_keys.push_back("cap_x");
		if(parameters.find("cap_y")==parameters.end())
			missing_keys.push_back("cap_y");
		if (parameters.find("cap_z")==parameters.end())
			missing_keys.push_back("cap_z");
	}
	else if (compare_strings(parameters["cap_type"],"voronoi"))
	{
		if(parameters.find("r_cut")==parameters.end())
			missing_keys.push_back("r_cut");
	}
	else
		opencap_throw("Error: only box and voronoi CAPs supported.");
	if(missing_keys.size()!=0)
	{
		std::string error_str = "Missing CAP keywords: ";
		for (auto key: missing_keys)
			error_str+=key+" ";
		opencap_throw(error_str);
	}
	std::map<std::string, std::string> defaults = {{"radial_precision", "14"}, {"angular_points", "590"}};
	for (const auto &pair:defaults)
	{
		if(parameters.find(pair.first)==parameters.end())
			parameters[pair.first]=pair.second;
	}
}


#include "simulate.h"
#include "config.h"
#include "call_lua_function.h"
#include "grid.h"
#include "particles.h"
#include "check_particle.h"
#include "particle_groups.h"
#include "gather_scatter.h"
#include "save_grid.h"
#include "save_particles.h"

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

namespace PIC {

/************************************************************
*                                                           *
* I step                                                    *
*                                                           *
* Calculate magnetic field in half time step  (m+1/2) step  *
*                                                           *
************************************************************/
void calc_magnetic_field_half_time(Grid& grid)
{
    const index_t kd = 1;

    const index_t to_kx = (grid.size_x() - kd);
    const index_t to_ky = (grid.size_y() - kd);
    const index_t to_kz = (grid.size_z() - kd);

    const double ctau_2h = Config::ctau_2h();

    for (index_t kx = kd; kx != to_kx; ++kx)
    for (index_t ky = kd; ky != to_ky; ++ky)
    for (index_t kz = kd; kz != to_kz; ++kz)
    {
        Cell& cell = grid(kx,ky,kz);

        cell.B.x -= ctau_2h*((cell.E.z - grid(kx,ky - 1,kz).E.z) -
                             (cell.E.y - grid(kx,ky,kz - 1).E.y));

        cell.B.y -= ctau_2h*((cell.E.x - grid(kx,ky,kz - 1).E.x) -
                             (cell.E.z - grid(kx - 1,ky,kz).E.z));

        cell.B.z -= ctau_2h*((cell.E.y - grid(kx - 1,ky,kz).E.y) -
                             (cell.E.x - grid(kx,ky - 1,kz).E.x));
    }
}

/*******************************************
*                                          *
* II step                                  *
*                                          *
* Move particles in half time step (m+1/2) *
*                                          *
*******************************************/

void push_particle_std(const Grid& grid, Particle& rp)
{
    DblVector Bp;
    gather_edge(grid, rp.r, &Cell::B, Bp);

    DblVector Ep;
    gather_face(grid, rp.r, &Cell::E, Ep);

    ParticleGroups part_groups;
    const ParticleGroups::ParticleGroup& part_group = part_groups[rp.group_name];

    const double qm = part_group.charge/part_group.mass*Constants::e_mp();

    const double tau_qm = Config::tau()*qm;
    const double tau_2qmc = 0.5*tau_qm/Constants::c();

    const double a = tau_2qmc*Bp.x;
    const double b = tau_2qmc*Bp.y;
    const double c = tau_2qmc*Bp.z;

    const double A = rp.v.x + tau_qm*Ep.x + c*rp.v.y - b*rp.v.z;
    const double B = rp.v.y + tau_qm*Ep.y + a*rp.v.z - c*rp.v.x;
    const double C = rp.v.z + tau_qm*Ep.z + b*rp.v.x - a*rp.v.y;

    const double a2 = a*a;
    const double b2 = b*b;
    const double c2 = c*c;

    const double D = 1.0/(a2 + b2 + c2 + 1.0);

    const double ab = a*b;
    const double ac = a*c;
    const double bc = b*c;

    /*
    Mathematica solution:
    x -> (A + a^2 A + a b B + B c - b C + a c C)/(1 + a^2 + b^2 + c^2),
    y -> (a A b + B + b^2 B - A c + a C + b c C)/(1 + a^2 + b^2 + c^2),
    z -> (A b - a B + a A c + b B c + C + c^2 C)/(1 + a^2 + b^2 + c^2).
    */

    rp.v.x = D*(A + a2*A + ab*B + B*c - b*C + ac*C);
    rp.v.y = D*(ab*A + B + b2*B - A*c + a*C + bc*C);
    rp.v.z = D*(A*b - a*B + ac*A + bc*B + C + c2*C);
}

void push_particle_boris(const Grid& grid, Particle& rp)
{
    DblVector B;
    gather_edge(grid, rp.r, &Cell::B, B);

    DblVector E;
    gather_face(grid, rp.r, &Cell::E, E);

    ParticleGroups part_groups;
    const ParticleGroups::ParticleGroup& part_group = part_groups[rp.group_name];

    const double qm = part_group.charge/part_group.mass*Constants::e_mp();
    const double tau_2qm = 0.5*Config::tau()*qm;
    const double tau_2qmc = tau_2qm/Constants::c();

    const DblVector Vm = rp.v + tau_2qm*E;
    const DblVector V0 = Vm + tau_2qmc*(Vm % B);
    const double d = 2.0*tau_2qmc/(1.0 + tau_2qmc*tau_2qmc*B.get_sqr_len());
    const DblVector Vp = Vm + d*(V0 % B);

    rp.v = Vp + tau_2qm*E;
}

void (*push_particle)(const Grid&, Particle&);

vector<DensityGrid> density_grids;

void (*scatter_particle)(const Particle&, DensityGrid&, const DblVector&);

namespace {

void log_error(const char* msg)
{
    const int tid = omp_get_thread_num();
    const std::string log_file_name =
        str(boost::format("opic_thread_%1%_move_particles_half_time_err.log") % tid);
    std::ofstream ofs_log(log_file_name.c_str(), std::ios_base::app);
    ofs_log << msg << std::endl;
};

}

void move_particles_half_time(const Grid& grid, Particles& particles, const std::string& group_name)
{
    particles.remove_inactives();

    const long particles_num = particles.size();

    long err_count = 0;

    #pragma omp parallel for reduction(+: err_count)
    for (long p = 0; p < particles_num; ++p)
    {
        try
        {
            Particle& particle = particles[p];

            if ((group_name == ParticleGroups::all_particles_name ||
                 group_name == particle.group_name) &&
                (err_count == 0))
            {
                if (is_particle_can_move(particles, p, grid))
                {
                    push_particle(grid, particle);
                    const DblVector dr = PIC::Config::tau_2()*particle.v;
                    if (!check_particle_move(particle, grid, dr))
                    {
                        ++err_count;
                    }
                    else if (is_particle_can_scatter(particles, p, grid, dr))
                    {
                        const int thread_num = omp_get_thread_num();
                        scatter_particle(particle, density_grids[thread_num], dr);
                        particle.r += dr;
                    }
                }
            }
        }
        catch (std::exception & e)
        {
            log_error(e.what());
            ++err_count;
        }
        catch (...)
        {
            log_error(boost::current_exception_diagnostic_information().c_str());
            ++err_count;
        }
    } // for (long p = 0; p < particles_num; ++p)

   if (err_count != 0)
   {
      throw domain_error("Error occured during the simulation!\nSee opic_thread_*_err.log files for details.");
   }
}

/***********************************
*                                  *
* IV step                          *
*                                  *
* Move particles in full time step *
*                                  *
***********************************/
void move_particles_full_time(const Grid& grid, Particles& particles,
                              const std::string& group_name) // step m+1
{
    const double tau_2 = Config::tau_2();
    const long particles_num = particles.size();

    #pragma omp parallel for
    for (long p = 0; p < particles_num; ++p)
    {
        Particle& particle = particles[p];
        if ((group_name == ParticleGroups::all_particles_name ||
             particle.group_name == group_name) &&
             is_particle_can_move(particles, p, grid))
        {
            particle.r += tau_2*particle.v;
        }
    }
}

/***************************************************
*                                                  *
* V step ( repeat step I )                         *
*                                                  *
* Calculate magnetic field in full time step (m+1) *
*                                                  *
***************************************************/

/***************************************************
*                                                  *
* VI step                                          *
*                                                  *
* Calculate electron velocities                    *
*                                                  *
***************************************************/

void calc_electrons_velocity(Grid& grid)
{
    const index_t kd = 1;

    const index_t to_kx = (grid.size_x() - kd);
    const index_t to_ky = (grid.size_y() - kd);
    const index_t to_kz = (grid.size_z() - kd);

    const double c_4pi_e_h = Config::c_4pi_e_h();

    double NP = 0.0;
    for (index_t kx = kd; kx != to_kx; ++kx)
    for (index_t ky = kd; ky != to_ky; ++ky)
    for (index_t kz = kd; kz != to_kz; ++kz)
    {
        Cell& cell = grid(kx, ky, kz);

        // NP for UEx
        NP = 0.5*(cell.NP + grid(kx - 1, ky, kz).NP);
        cell.UE.x = cell.UP.x - c_4pi_e_h/NP*((grid(kx, ky + 1, kz).B.z - cell.B.z) -
                                              (grid(kx, ky, kz + 1).B.y - cell.B.y));
        // NP for UEy
        NP = 0.5*(cell.NP + grid(kx, ky - 1, kz).NP);
        cell.UE.y = cell.UP.y - c_4pi_e_h/NP*((grid(kx, ky, kz + 1).B.x - cell.B.x) -
                                              (grid(kx + 1, ky, kz).B.z - cell.B.z));
        //NP for UEz
        NP = 0.5*(cell.NP + grid(kx, ky, kz - 1).NP);
        cell.UE.z = cell.UP.z - c_4pi_e_h/NP*((grid(kx + 1, ky, kz).B.y - cell.B.y) -
                                              (grid(kx, ky + 1, kz).B.x - cell.B.x));
    }
}

/***************************************************
*                                                  *
* VI step                                          *
*                                                  *
* Calculate electric field                         *
*                                                  *
***************************************************/
void calc_electric_field(Grid& grid)
{
    DblVector UE, B;

    const index_t kd = 1;

    const index_t to_kx = (grid.size_x() - kd);
    const index_t to_ky = (grid.size_y() - kd);
    const index_t to_kz = (grid.size_z() - kd);

    for (index_t kx = kd; kx != to_kx; ++kx)
    for (index_t ky = kd; ky != to_ky; ++ky)
    for (index_t kz = kd; kz != to_kz; ++kz)
    {
        Cell& cell = grid(kx, ky, kz);

        // calculate Ex
        B.y = 0.5*(cell.B.y + grid(kx, ky, kz + 1).B.y);
        B.z = 0.5*(cell.B.z + grid(kx, ky + 1, kz).B.z);

        UE.y = 0.25*(cell.UE.y + grid(kx, ky + 1, kz).UE.y +
                     grid(kx - 1, ky, kz).UE.y + grid(kx - 1, ky + 1, kz).UE.y);

        UE.z = 0.25*(cell.UE.z + grid(kx, ky, kz + 1).UE.z +
                     grid(kx - 1, ky, kz).UE.z + grid(kx - 1, ky, kz + 1).UE.z);

        cell.E.x = (UE.z*B.y - UE.y*B.z)/Constants::c();

        // calculate Ey
        B.z = 0.5*(cell.B.z + grid(kx + 1, ky, kz).B.z);
        B.x = 0.5*(cell.B.x + grid(kx, ky, kz + 1).B.x);

        UE.x = 0.25*(cell.UE.x + grid(kx + 1, ky, kz).UE.x +
                     grid(kx, ky - 1, kz).UE.x + grid(kx + 1, ky - 1, kz).UE.x);

        UE.z = 0.25*(cell.UE.z + grid(kx, ky, kz + 1).UE.z +
                     grid(kx, ky - 1, kz).UE.z + grid(kx, ky - 1, kz + 1).UE.z);

        cell.E.y = (UE.x*B.z - UE.z*B.x)/Constants::c();

        // calculate Ez
        B.x = 0.5*(cell.B.x + grid(kx, ky + 1, kz).B.x);
        B.y = 0.5*(cell.B.y + grid(kx + 1, ky, kz).B.y);

        UE.y = 0.25*(cell.UE.y + grid(kx, ky + 1, kz).UE.y +
                     grid(kx, ky, kz - 1).UE.y + grid(kx, ky + 1, kz - 1).UE.y);

        UE.x = 0.25*(cell.UE.x + grid(kx + 1, ky, kz).UE.x +
                     grid(kx, ky, kz - 1).UE.x + grid(kx + 1, ky, kz - 1).UE.x);

        cell.E.z = (UE.y*B.x - UE.x*B.y)/Constants::c();
    }
}

template <class DensityGridType>
void set_grid_UP(const Grid& grid, DensityGridType& dg_group)
{
    double NP = 0.0;

    const index_t m = 1;

    for (index_t i = m; i != (grid.size_x() - m); ++i)
    for (index_t j = m; j != (grid.size_y() - m); ++j)
    for (index_t k = m; k != (grid.size_z() - m); ++k)
    {
        typename DensityGridType::NodeType& dn = dg_group(i,j,k);

        // NP for UPx
        NP = 0.5*(dn.NP + dg_group(i-1,j,k).NP);
        NP > 1.0e-6 ? dn.UP.x /= NP : dn.UP.x = 0.0;

        // NP for UPy
        NP = 0.5*(dn.NP + dg_group(i,j-1,k).NP);
        NP > 1.0e-6 ? dn.UP.y /= NP : dn.UP.y = 0.0;

        // NP for UPz
        NP = 0.5*(dn.NP + dg_group(i,j,k-1).NP);
        NP > 1.0e-6 ? dn.UP.z /= NP : dn.UP.z = 0.0;
    }
}

template <class DensityGridType>
void normalize_NP(const Grid& grid, DensityGridType& dg_group)
{
    const double cell_volume = grid.cell_volume();

    for (index_t i = 0; i != grid.size_x(); ++i)
    for (index_t j = 0; j != grid.size_y(); ++j)
    for (index_t k = 0; k != grid.size_z(); ++k)
    {
        dg_group(i,j,k).NP /= cell_volume;
    }
}

void local_Alfven_CFL(Grid& grid, index_t i, index_t j, index_t k)
{
    Cell& cell = grid(i,j,k);

    const double h = grid.step();
    DblVector vec_to_point(i*h, j*h, k*h);
    DblVector vec_B;
    gather_edge(grid, vec_to_point, &Cell::B, vec_B);
    const double B = vec_B.abs();

    const double mp = ParticleGroups::max_mp()*PIC::Constants::mp();
    const double pi = Constants::pi();
    const double tau_2 = Config::tau_2();

    const double min_dens = 1.5*B*B*tau_2*tau_2/(4*pi*h*h*mp);
    const double cell_volume = grid.cell_volume();

    // At this stage grid stores NP*cell_volume
    if (cell.NP < min_dens*cell_volume)
    {
        PIC::Config::logger() << "\nlocal_Alfven_CFL:"
                              << "(i,j,k) = (" << i << "," << j << "," << k << ")"
                              << " cell.NP = " << cell.NP
                              << " min_dens = " << min_dens
                              << std::endl;

        cell.NP = min_dens*cell_volume;
    }
}

void backgr_fracture(Grid& grid, index_t i, index_t j, index_t k)
{
    Cell& cell = grid(i,j,k);

    const double dens_cutoff = Config::dens_cutoff();
    const double cell_volume = grid.cell_volume();

     // At this stage grid stores NP*cell_volume
    if (cell.NP < dens_cutoff*cell_volume)
    {
        Config::logger() << "time step = " << Config::current_time_step()
                         << ": backgr_fracture: "
                         << "(i,j,k) = (" << i << "," << j << "," << k << ")"
                         << " cell.NP = " << cell.NP/grid.cell_volume()
                         << " dens_cutoff = " << dens_cutoff
                         << std::endl;

        cell.NP = dens_cutoff*cell_volume;
    }
}

void (*grid_threshold)(Grid& grid, index_t i, index_t j, index_t k);

void set_threshold(Grid& grid)
{
    // At this stage grid stores NP*grid.cell_volume()
    for (index_t i = 1; i != grid.size_x()-1; ++i)
    for (index_t j = 1; j != grid.size_y()-1; ++j)
    for (index_t k = 1; k != grid.size_z()-1; ++k)
    {
        grid_threshold(grid, i, j, k);
    }
}

void simulate(Grid& grid, Particles& particles)
{
    std::cout << "\n\nPIC simulation started ...\n\n";

    scatter_particle = &scatter_particle_std;

    push_particle = &push_particle_std;
    if (Config::particle_push_alg() == Boris)
        push_particle = push_particle_boris;

    grid_threshold = &backgr_fracture;
    if (Config::grid_threshold() == Local_CFL)
        grid_threshold = &local_Alfven_CFL;

    const int max_threads = omp_get_max_threads();
    density_grids = vector<DensityGrid>(max_threads,
                                       DensityGrid(grid.size_x(), grid.size_y(), grid.size_z(), grid.step()));

    omp_set_dynamic(0); // Explicitly disable dynamic teams
    omp_set_num_threads(max_threads); // Use max threads for all consecutive parallel regions

    DensityGrid dg_group(grid.size_x(), grid.size_y(), grid.size_z(), grid.step());

    const index_t steps_num = Config::time_steps();

    for (index_t step = 0; step != steps_num; ++step)
    {
        Config::set_current_time_step(step);

        std::cout << "\nstep " << step << " of " << PIC::Config::time_steps()
                  << " : " << particles.size() << " particle(s)" << std::endl;

        // reset charge and current densities on grid.
        grid.reset_current();

        print_tm("Call \"on iteration begin\" handler");
        call_lua_function("on_iteration_begin");

        print_tm("Calculate magnetic field with half time step");
        calc_magnetic_field_half_time(grid);

        // move particle on half time step.
        if (Config::is_on_save_step())
        {
            ParticleGroups part_groups;
            const std::vector<std::string>& group_names = part_groups.group_names();

            // move particle groups half time step separately
            // to allow saving their grid data in separate files.
            for (size_t g = 0; g != group_names.size(); ++g)
            {
                const std::string& group_name = group_names[g];

                print_tm("Call \"particles moved on half time\" handler");
                call_lua_function("on_particles_moved_half_time");

                print_tm("Move particles in half time step: ", group_name);
                move_particles_half_time(grid, particles, group_name);

                // reset particle group charge and current densities.
                dg_group.reset_current();

                // accumulate particle group charge and current densities
                // scattered in particles push threads.
                for (int t = 0; t != max_threads; ++t)
                {
                    DensityGrid& dg_thread = density_grids[t];
                    dg_group.add_current(dg_thread);
                    dg_thread.reset_current();
                }

                // add particle group charge and current densities to grid.
                grid.add_current(dg_group);

                // calculate UP of particle group.
                set_grid_UP(grid, dg_group);
                set_boundary_conditions("on_set_boundary_group_UP", dg_group);

                // calculate NP of particle group.
                normalize_NP(grid, dg_group);
                set_boundary_conditions("on_set_boundary_group_NP", dg_group);

                // save group grid data.
                print_tm("Save grid data: ", group_name);
                save_grid(grid, dg_group, group_name);
            } // for (size_t g = 0; g!= group_names.size(); ++g)
        }
        else
        {
            print_tm("Call \"particles moved on half time\" handler");
            call_lua_function("on_particles_moved_half_time");

            // move all particle on half time step.
            print_tm("Move particles in half time step: ", ParticleGroups::all_particles_name);
            move_particles_half_time(grid, particles, ParticleGroups::all_particles_name);

            // accumulate all particles charge and current densities
            // scattered in particles push threads.
            for (int t = 0; t != max_threads; ++t)
            {
                DensityGrid& dg = density_grids[t];
                grid.add_current(dg);
                dg.reset_current();
            }
        }

        // set velocity of all particles.
        print_tm("Set boundary conditions for UP");
        set_grid_UP(grid, grid);
        set_boundary_conditions("on_set_boundary_UP");

        // set charge density all particles.
        print_tm("Set boundary conditions for NP");
        set_threshold(grid);
        normalize_NP(grid, grid);
        set_boundary_conditions("on_set_boundary_NP");

        if (Config::is_on_save_step())
        {
            print_tm("Save particles data");
            save_particles(grid, particles);

            print_tm("Save grid data: ", ParticleGroups::all_particles_name);
            save_grid(grid, grid, ParticleGroups::all_particles_name);
        }

        // move all particle on full time step
        print_tm("Move particles in full time step: ", ParticleGroups::all_particles_name);
        move_particles_full_time(grid, particles, ParticleGroups::all_particles_name);

        print_tm("Call \"particles moved on full time\" handler");
        call_lua_function("on_particles_moved_full_time");

        print_tm("Calculate magnetic field with full time step");
        calc_magnetic_field_half_time(grid);
        set_boundary_conditions("on_set_boundary_MF");

        print_tm("Calculate electron velocities");
        calc_electrons_velocity(grid);
        set_boundary_conditions("on_set_boundary_UE");

        print_tm("Calculate electric field");
        calc_electric_field(grid);
        set_boundary_conditions("on_set_boundary_EF");

        print_tm("Call \"on iteration end\" handler");
        call_lua_function("on_iteration_end");
    }
}

} // namespace PIC


/*
Frechet Python Package

TODO: unnötige Abhängigkeiten
- Dritter Decider ist vermischt mit Index, daher nicht sinnvoll isolierbar
- template parameter DIM, coordinate_getter, squared_distance
- template param niederlande: braucht zusätzlich punkt_typ
*/

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include<iostream>

#include<boost/python.hpp>
#include<boost/python/numpy.hpp>
#include<boost/python/numpy/ndarray.hpp>
#include <numpy/ndarrayobject.h>
#include <numpy/ndarraytypes.h>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <iomanip>      // std::setprecision
#include <limits>
#include <boost/geometry/algorithms/distance.hpp>

#include "../include/frechetrange/frechetrange.hpp"

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
namespace bp =  boost::python;

typedef bg::model::point<double, 2, bg::cs::cartesian> point;

typedef bg::model::box<point> box;
typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygo
typedef bg::model::multi_polygon<polygon> multipolygon;
typedef bg::model::linestring<point> linestring;
typedef std::pair<point, size_t> value;

typedef bgi::rtree< value, bgi::rstar<16, 4> > rtree_t;

namespace bp = boost::python;
namespace np = boost::python::numpy;

typedef point point_adapter;

struct trajectory_adapter {
   const np::ndarray &_m;
   const int s1,s2; // the strides
   const char *data;

   typedef point_adapter value_type; // @todo

   trajectory_adapter(const np::ndarray &m): _m(m), s1(m.strides(0)), s2(m.strides(1)), data(m.get_data()){
   };

   point_adapter operator[](size_t idx) const {
      assert(idx < _m.shape(0));
      double c1 = *const_cast<double *>(reinterpret_cast<const double *>( data + idx * s1));
      double c2 = *const_cast<double *>(reinterpret_cast<const double *>( data + idx * s1 + s2));
      return point(c1,c2);
   }

  size_t size() const { return _m.shape(0); }
};

struct get_adapter_coord {
  template <size_t dim> static double get(const point_adapter &c) {
    return bg::get<dim> (c);
  }
};

using std::cout;
using std::endl;

frechetrange::detail::dv::frechet_distance<2, get_adapter_coord>   fd;
frechetrange::detail::bb::frechet_distance<2, get_adapter_coord>   fd2;

class FrechetDecider
{
    public:
       FrechetDecider(){}

       bool decide(np::ndarray t1, np::ndarray t2, double dist, std::string decider)
	{
	    if (decider == "duetschvahrenhold") return decide_dv(t1,t2,dist);
	    if (decider == "baldusbringman") return decide_bb(t1,t2,dist);
	    throw(std::runtime_error("Decider "+decider +" is not known."));
	}

	bool decide_dv(np::ndarray t1, np::ndarray t2, double dist)
	{
	    trajectory_adapter c1(t1), c2(t2);
	    return fd.is_bounded_by(c1,c2,dist);
 	}

	bool decide_bb(np::ndarray t1, np::ndarray t2, double dist)
	{
	    trajectory_adapter c1(t1), c2(t2);
	    return fd2.is_bounded_by(c1,c2,dist);
 	}

}; // FrechetDecider

///////////////////// TYPES FOR THE GRID INDICES AND THE COPY API

constexpr size_t g_DIMENSIONS = 2;
template <size_t dims> using _point_t = std::array<double, dims>;

struct get_point_coord {
  template <size_t dim> static double get(const _point_t<g_DIMENSIONS> &p) {
    return std::get<dim>(p);
  }
};

template <size_t dims> using _trajectory_t = std::vector<_point_t<dims>>;

template <size_t dims>
void _copyMatrixToTrajectory(const np::ndarray &m, _trajectory_t<dims> &t) {
  char *row = m.get_data();
  const int s1 = m.strides(0), s2 = m.strides(1);
  for (size_t i = 0; i < m.shape(0); i++) {
    char *p = row;
    for (size_t d = 0; d < dims; d++)
    {
      t[i][d] = *reinterpret_cast< double *> (p);
      p += s2;
    }
    row += s1;
  }
}
template<size_t dims>
void _trajectoryToMatrix(const _trajectory_t<dims> &t, np::ndarray &m) {
  char *row = m.get_data();
  const int s1 = m.strides(0), s2 = m.strides(1);
  for (size_t i = 0; i < m.shape(0); i++) {
    char *p = row;
    for (size_t d = 0; d < dims; d++)
    {
      *reinterpret_cast< double *> (p) = t[i][d];
      p += s2;
    }
    row += s1;
  }
}

template <size_t dims> class grid_data {
public:
  typedef frechetrange::detail::dv::grid<
      dims, _trajectory_t<dims>, get_point_coord>
      grid_type;

  grid_data() : _pGrid(nullptr), _trajectories() {}
  ~grid_data() { delete _pGrid; }

  grid_type &grid() { return *_pGrid; }

  size_t add_trajectory(const np::ndarray &m) {
    _trajectories.emplace_back(m.shape(0));
    _copyMatrixToTrajectory(m, _trajectories.back());
    return (_trajectories.size() - 1);
  }

  std::string as_ssv()
  {
    // This is meant mainly for debug, it gets back the data as a SSV string
    std::stringstream ss;
    for (size_t i=0; i< _trajectories.size(); i++)
    {
	auto &t =_trajectories[i];
	for (auto &p: t)
	    ss << i << " " << std::get<0>(p) << " " << std::get<1>(p) << std::endl;
    }
     return ss.str();
  }

  size_t size() { return _trajectories.size(); }

  void clear() { _trajectories.clear(); }

  void build_index(double meshSize) {
    _pGrid = new grid_type(meshSize);
    _pGrid->reserve(size());
    for (_trajectory_t<dims> &t : _trajectories)
      _pGrid->insert(std::move(t));
    clear();
    _pGrid->build_index();
  }

  bp::object query(const np::ndarray &m, double dist)
  {
      if (_pGrid== nullptr) throw(std::runtime_error("You need to run build_index before you can query"));
      _trajectory_t<dims> t(m.shape(0));
      _copyMatrixToTrajectory(m, t);
//      _copyMatrixToTrajectory<dims>(m, t);
       std::vector<_trajectory_t<dims>> resultSet;
       auto inserter = [&](const _trajectory_t<dims> &t) {resultSet.push_back(t);};
       grid().range_query(t, dist, inserter);
       bp::list l;
       for (auto &t:resultSet)
       {
	    npy_intp  shape[2] = { static_cast<npy_intp> (t.size()),2 }; // array size
	    np::ndarray ta = np::zeros(bp::make_tuple(t.size(),2),np::dtype::get_builtin<double>());
	    _trajectoryToMatrix(t,ta);
	    l.append(ta);
       }
       return bp::object(l);
  }

private:
  std::vector<_trajectory_t<dims>> _trajectories;
  grid_type *_pGrid;
};

//////////////////////////// baldusbringmann::spatial_index
template <size_t dims>
using bb_index_type = frechetrange::detail::bb::spatial_index<
    dims, _trajectory_t<dims>, get_point_coord>;

template<size_t dims>
class BBIndex
{
    private:
	// need to be a pointer, because copy construction is deleted
	bb_index_type<dims> *idx;
    public:

    BBIndex(){
       idx = new bb_index_type<dims>();
    }
    ~BBIndex(){
	delete idx;
    }

  size_t add_trajectory(const np::ndarray &m) {
      _trajectory_t<dims> t(m.shape(0));
      _copyMatrixToTrajectory<dims>(m, t);
      idx->insert(t);
      return (idx->size() - 1);
  }

  bp::object query(const np::ndarray &m, double dist)
  {
      _trajectory_t<dims> t(m.shape(0));
      _copyMatrixToTrajectory(m, t);

       std::vector<_trajectory_t<dims>> resultSet;
       auto inserter = [&](const _trajectory_t<dims> &t) {resultSet.push_back(t);};
       idx->range_query(t, dist, inserter);
       bp::list l;
       for (auto &t:resultSet)
       {
	    npy_intp  shape[2] = { static_cast<npy_intp> (t.size()),2 }; // array size
	    np::ndarray ta = np::zeros(bp::make_tuple(t.size(),2),np::dtype::get_builtin<double>());
	    _trajectoryToMatrix(t,ta);
	    l.append(ta);
       }
       return bp::object(l);
  }

};

//////////////////////// TUE::spatial_hash

template <size_t dims>
using tue_index_type =
    frechetrange::detail::bddm::spatial_hash<_point_t<dims>, dims, _trajectory_t<dims>,
                                            get_point_coord>;

template<size_t dims>
class TUEIndex
{
    private:
	// need to be a pointer, because copy construction is deleted
	tue_index_type<dims> *idx;
    public:

    TUEIndex(){idx = new tue_index_type<dims>();}
    ~TUEIndex(){delete idx;}

  size_t add_trajectory(const np::ndarray &m) {
      _trajectory_t<dims> t(m.shape(0));
      _copyMatrixToTrajectory<dims>(m, t);
      idx->insert(std::move(t));
      return (idx->size() - 1);
  }

  void build_index(){
	idx->build_index();
  }

  bp::object query(const np::ndarray &m, double dist)
  {
      _trajectory_t<dims> t(m.shape(0));
      _copyMatrixToTrajectory(m, t);

       std::vector<_trajectory_t<dims>> resultSet;
       auto inserter = [&](const _trajectory_t<dims> &t) {resultSet.push_back(t);};
       idx->range_query(t, dist, inserter);
       bp::list l;
       for (auto &t:resultSet)
       {
	    npy_intp  shape[2] = { static_cast<npy_intp> (t.size()),2 }; // array size
	    np::ndarray ta = np::zeros(bp::make_tuple(t.size(),2),np::dtype::get_builtin<double>());
	    _trajectoryToMatrix(t,ta);
	    l.append(ta);
       }
       return bp::object(l);

  }

};

//////////////////// MODULE DEFINITION

typedef grid_data<2> DVGrid;
typedef BBIndex<2> BBIndex2;
typedef TUEIndex<2> TUEIndex2;

BOOST_PYTHON_MODULE(frechet) {
    import_array();
    np::initialize();

    bp::class_<DVGrid>("DVGrid")
       .def("add",+[](DVGrid &self,bp::object ot1) {
	   np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
	   self.add_trajectory(at1);
	})
     .def("as_ssv",+[](DVGrid &self)->std::string {
           return self.as_ssv();
	})
     .def("build_index",+[](DVGrid &self, double mesh_size) {
          self.build_index(mesh_size);
	  })
     .def("query",+[](DVGrid &self, bp::object ot1, double distance)->bp::object {
           np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
          return self.query(at1, distance);
	  })
     .def("clear",+[](DVGrid &self) {
          self.clear();
	  })
     .def("size",+[](DVGrid &self)->int {
          return self.size();
	  })

	;

    bp::class_<BBIndex2>("BBIndex")
       .def("add",+[](BBIndex2 &self,bp::object ot1) {
	   np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
	   self.add_trajectory(at1);
	})
     .def("query",+[](BBIndex2 &self, bp::object ot1, double distance)->bp::object {
           np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
          return self.query(at1, distance);
	  })
     .def("clear",+[](BBIndex2 &self) {

	  });

    bp::class_<TUEIndex2>("TUEIndex")
       .def("add",+[](TUEIndex2 &self,bp::object ot1) {
	   np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
	   self.add_trajectory(at1);
	})
     .def("query",+[](TUEIndex2 &self, bp::object ot1, double distance)->bp::object {
           np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
          return self.query(at1, distance);
	  })
     .def("build_index",+[](TUEIndex2 &self) {
          self.build_index();
	  })
     .def("clear",+[](TUEIndex2 &self) {

	  });

       bp::class_<FrechetDecider>("FrechetDecider")
       .def("decide",+[](FrechetDecider &self,bp::object ot1,bp::object ot2, double d, std::string decider)->bool {
	   np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
	   np::ndarray at2 = np::from_object(ot2,np::dtype::get_builtin<double>());
	    return self.decide(at1,at2,d,decider);

       })
       .def("decide_dv",+[](FrechetDecider &self, bp::object ot1,bp::object ot2, double d)->bool
       {
	   np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
	   np::ndarray at2 = np::from_object(ot2,np::dtype::get_builtin<double>());
	    if (at1.get_nd() != 2) throw(std::runtime_error("2D array expected"));
	    if (at2.get_nd() != 2) throw(std::runtime_error("2D array expected"));
	    return self.decide_dv(at1,at2,d);
       })
       .def("decide_bb",+[](FrechetDecider &self, bp::object ot1,bp::object ot2, double d)->bool
       {
	   np::ndarray at1 = np::from_object(ot1,np::dtype::get_builtin<double>());
	   np::ndarray at2 = np::from_object(ot2,np::dtype::get_builtin<double>());
	    if (at1.get_nd() != 2) throw(std::runtime_error("2D array expected"));
	    if (at2.get_nd() != 2) throw(std::runtime_error("2D array expected"));
	    return self.decide_bb(at1,at2,d);
       })

       ;
}


#include <cmath>
#include <Eigen/Geometry>
#include <comma/base/exception.h>
#include <comma/math/compare.h>
#include "great_circle.h"
// #include <iomanip>

namespace snark { namespace spherical {

// quick and dirty
static bool equal_( const Eigen::Vector3d& lhs, const Eigen::Vector3d& rhs ) { return ( lhs - rhs ).lpNorm< Eigen::Infinity >() < coordinates::epsilon; }

// quick and dirty
static bool is_between_( const great_circle::arc& a, const Eigen::Vector3d& p ) // quick and dirty, faster than great_circle::arc::has()
{
    if( equal_( a.begin(), p ) || equal_( a.end(), p ) ) { return true; } // if( comma::math::equal( a.begin(), p, epsilon ) || comma::math::equal( a.end(), p, epsilon ) ) { return true; }
    double begin_angle = great_circle::arc::angle_axis(a.begin(), p ).angle();
    double end_angle = great_circle::arc::angle_axis(p, a.end() ).angle();
    return comma::math::equal( a.angle(), begin_angle + end_angle, coordinates::epsilon );
}

//qnd
template< typename T >
static inline bool is_between( const T &t, const T &d1, const T &d2 )
{
    return d1 < d2 ? ( t >= d1 && t <= d2 ) : ( t <= d1 && t >= d2 );
}

template< typename T >
static inline bool has_overlap( const T &t1, const T &t2, const T &d1, const T &d2 )
{
    return is_between( t1, d1, d2 )
        || is_between( t2, d1, d2 )
        || is_between( d1, t1, t2 )
        || is_between( d2, t1, t2 );
}

great_circle::arc::arc()
    : begin_coordinates_( 0, 0 )
    , end_coordinates_( 0, 0 )
    , begin_( 1, 0, 0 )
    , end_( 1, 0, 0 )
    , angle_axis_( 0, Eigen::Vector3d( 0, 1, 0 ) ) // quick and dirty
{ }

great_circle::arc::arc( const Eigen::Vector3d& begin, const Eigen::Vector3d& end )
    : begin_coordinates_( begin )
    , end_coordinates_( end )
    , begin_( begin )
    , end_( end )
    , angle_axis_( Eigen::Quaternion< double >::FromTwoVectors( begin_, end_ ) )
{
    if( comma::math::less( M_PI, angle() ) ) { COMMA_THROW( comma::exception, "only arcs less than pi supported; got " << angle() << " radian for begin: " << std::string(begin_coordinates_) << " end: " << std::string(end_coordinates_)); }
    //if( equal_(begin_, end_) ) { COMMA_THROW( comma::exception, "zero length great_circle::arc is not permitted: got  begin: " << std::string(begin_coordinates_) << " end: " << std::string(end_coordinates_)); }
}

// conversion to/from cartesian is expensive all permutations should be implemented
great_circle::arc::arc( const coordinates& begin, const coordinates& end )
    : begin_coordinates_( begin )
    , end_coordinates_( end )
    , begin_( begin.to_cartesian() )
    , end_( end.to_cartesian() )
    , angle_axis_( Eigen::Quaternion< double >::FromTwoVectors( begin_, end_ ) )
{
    if( comma::math::less( M_PI, angle() ) ) { COMMA_THROW( comma::exception, "only arcs less than pi supported; got " << angle() << " radian for begin: " << std::string(begin_coordinates_) << " end: " << std::string(end_coordinates_)); }
    //if( equal_(begin_, end_) ) { COMMA_THROW( comma::exception, "zero length great_circle::arc is not permitted: got  begin: " << std::string(begin_coordinates_) << " end: " << std::string(end_coordinates_)); }
}

great_circle::arc::arc( const Eigen::Vector3d& begin, const coordinates& end )
    : begin_coordinates_( begin )
    , end_coordinates_( end )
    , begin_( begin )
    , end_( end.to_cartesian() )
    , angle_axis_( Eigen::Quaternion< double >::FromTwoVectors( begin_, end_ ) )
{
    if( comma::math::less( M_PI, angle() ) ) { COMMA_THROW( comma::exception, "only arcs less than pi supported; got " << angle() << " radian for begin: " << std::string(begin_coordinates_) << " end: " << std::string(end_coordinates_)); }
    //if( equal_(begin_, end_) ) { COMMA_THROW( comma::exception, "zero length great_circle::arc is not permitted: got  begin: " << std::string(begin_coordinates_) << " end: " << std::string(end_coordinates_)); }
}

great_circle::arc::arc( const coordinates& begin, const Eigen::Vector3d& end )
    : begin_coordinates_( begin )
    , end_coordinates_( end )
    , begin_( begin.to_cartesian() )
    , end_( end )
    , angle_axis_( Eigen::Quaternion< double >::FromTwoVectors( begin_, end_ ) )
{
    if( comma::math::less( M_PI, angle() ) ) { COMMA_THROW( comma::exception, "only arcs less than pi supported; got " << angle() << " radian for begin: " << std::string(begin_coordinates_) << " end: " << std::string(end_coordinates_)); }
    //if( equal_(begin_, end_) ) { COMMA_THROW( comma::exception, "zero length great_circle::arc is not permitted: got  begin: " << std::string(begin_coordinates_) << " end: " << std::string(end_coordinates_)); }
}

double great_circle::arc::bearing( double angle ) const
{
    Eigen::Vector2d t = tangent_in_navigation_frame_at( angle );
    double b = std::acos( t.x() ) * ( t.y() < 0 ? -1 : 1 );
    return b >= M_PI ? b - M_PI * 2 : b;
}

Eigen::Vector3d great_circle::arc::at( double angle ) const
{
    return Eigen::AngleAxis< double >( angle, angle_axis_.axis() ).toRotationMatrix() * begin_;
}

Eigen::Vector3d great_circle::arc::tangent_at( double angle ) const
{
    Eigen::Vector3d tangent = angle_axis_.axis().cross( at( angle ) );
    tangent.normalize();
    return tangent;
}

Eigen::Vector2d great_circle::arc::tangent_in_navigation_frame_at( double angle ) const
{
    const Eigen::Vector3d& v = at( angle );
    const Eigen::Vector3d& n = snark::spherical::to_navigation_frame( coordinates( v ), angle_axis_.axis().cross( v ) );
    Eigen::Vector2d projection( n.x(), n.y() ); //Eigen::Vector2d projection( v.y(), v.z() ); //Eigen::Vector2d projection( v.z(), v.y() );
    projection.normalize();
    return projection;
}

bool great_circle::arc::has_inside( const coordinates& p ) const
{ return has_inside(p.to_cartesian()); }

bool great_circle::arc::has_inside( const Eigen::Vector3d& p ) const
{
    //if( begin_coordinates_ == p || end_coordinates_ == p ) { return false; }
    if( equal_(begin_, p) || equal_(end_, p) ) { return false; }

    //great_circle::arc::angle_axis(begin_, p );
    Eigen::AngleAxis< double > a = great_circle::arc::angle_axis(begin_, p );

    return equal_( axis(), a.axis() ) && comma::math::less( a.angle(), angle() );
}

bool great_circle::arc::overlaps( const great_circle::arc& rhs ) const // quick and dirty, potentially slow
{
    return    ( equal_( axis(), rhs.axis() ) || equal_( axis(), -rhs.axis() ) )
           && (    has( rhs.begin_ )
                || has( rhs.end_ )
                || rhs.has( begin_ )
                || rhs.has( end_ ) );
//     return    ( equal_( axis(), rhs.axis() ) || equal_( axis(), -rhs.axis() ) )
//            && (    has( rhs.begin_coordinates() )
//                 || has( rhs.end_coordinates() )
//                 || rhs.has( begin_coordinates_ )
//                 || rhs.has( end_coordinates_ ) );
}

// TODO no unit tests
great_circle::arc great_circle::arc::shortest_path( const coordinates &c ) const
{
    Eigen::Vector3d cc(c.to_cartesian());
    if (angle() == 0) { return arc(cc, begin()); }
    if (has_inside(c)) { return arc(cc, cc); }

    Eigen::Vector3d n = end() - begin();
    Eigen::Vector3d p((n*(n.dot(cc - begin())))/n.dot(n) + begin());
    // TODO potentially unstable
    // because the arc from p to cc just touches the original arc, never crosses for real
    boost::optional< coordinates > t = intersection_with(arc(p, cc));
    if (! t)
    {
        arc a1(cc, begin());
        arc a2(cc, end());
        return a1.angle() < a2.angle() ? a1 : a2;
    }
    return arc(cc, *t);
}

// TODO no unit tests
double great_circle::arc::angle_towards( const coordinates &c ) const
{
    if( has(c) ) { return 0; }

    // todo: using bearing may be very inaccurate; simply turning the arc axis should be the right way, literally something like this:
    // Eigen::Quaternion< double >::FromTwoVectors( axis(), great_circle::arc( begin_coordinates_, c ).axis() ).angle()

    double result = bearing(0) - arc(begin_coordinates_, c).bearing(0);
    if (result > M_PI) { result = -2*M_PI + result; }
    return result;
}

// TODO handy but needs precision as argument
great_circle::arc::operator std::string() const
{
    return "[" + std::string(begin_coordinates()) + " -> " +std::string(end_coordinates()) + "]";
}

// struct count_t
// {
//     unsigned int accepted;
//     unsigned int rejected;
//     count_t() : accepted( 0 ), rejected( 0 ) {}
//     //~count_t() { std::cerr << "=============> great_circle::arc::may_intersect: accepted: " << accepted << " rejected: " << rejected << std::endl; }
// };
//
// static count_t count;

static const double two_pi = M_PI * 2;
bool great_circle::arc::may_intersect(const great_circle::arc& target) const
{
    double self_begin_longitude = begin_coordinates_.longitude;
    double self_end_longitude = end_coordinates_.longitude;
    double other_begin_longitude = target.begin_coordinates_.longitude;
    double other_end_longitude = target.end_coordinates_.longitude;
    // if either self or target wraps longitude over 180, unwrap
    bool wrap_self = false;
    if ( std::abs( self_end_longitude  - self_begin_longitude  ) > M_PI ) { self_end_longitude  += two_pi; wrap_self  = true; }
    bool wrap_other = false;
    if ( std::abs( other_end_longitude - other_begin_longitude ) > M_PI ) { other_end_longitude += two_pi; wrap_other = true; }
    // double conv = 180.0/M_PI;
    // std::cerr << std::setprecision(12) << "self:  [" << begin_coordinates_.longitude*conv        << "," << end_coordinates_.longitude*conv        << "], [" << self_begin_longitude*conv  << "," << self_end_longitude*conv  << "], " << wrap_self << std::endl;
    // std::cerr << std::setprecision(12) << "other: [" << target.begin_coordinates_.longitude*conv << "," << target.end_coordinates_.longitude*conv << "], [" << other_begin_longitude*conv << "," << other_end_longitude*conv << "], " << wrap_self << std::endl;
    // now both intervals are unwrapped; the may overlap directly or with a shift
    if (               has_overlap( other_begin_longitude,          other_end_longitude,          self_begin_longitude,          self_end_longitude          ) ) return true;
    if ( wrap_self  && has_overlap( other_begin_longitude + two_pi, other_end_longitude + two_pi, self_begin_longitude,          self_end_longitude          ) ) return true;
    if ( wrap_other && has_overlap( other_begin_longitude,          other_end_longitude,          self_begin_longitude + two_pi, self_end_longitude + two_pi ) ) return true;
    return false;
}

bool great_circle::arc::has( const coordinates& p ) const
{ return has(p.to_cartesian()); }

bool great_circle::arc::has( const Eigen::Vector3d& p ) const
{
    //if( begin_coordinates_ == p || end_coordinates_ == p ) { return true; }
    if( equal_(begin_, p) || equal_(end_, p )) { return true; }

    //great_circle::arc a( begin_coordinates_, p );
    Eigen::AngleAxis< double > a = arc::angle_axis(begin_, p);

    return equal_( axis(), a.axis() ) && comma::math::less( a.angle(), angle() );
}

// /// @return intersection with an arc
// /// @note can be only one point, since we do not consider
// ///       arcs greater than pi
// boost::optional< coordinates > great_circle::arc::intersection_with( const arc& rhs ) const
// {
//     if (!may_intersect(rhs))  { return boost::optional< coordinates >(); }
//     if( equal_( axis(), rhs.axis() ) || equal_( axis(), -rhs.axis() ) ) { return boost::optional< coordinates >(); }
//     Eigen::Vector3d cross = axis().cross( rhs.axis() );
//     snark::range_bearing_elevation a( cross ); // quick and dirty, quite excessive
//     coordinates c( a.e(), a.b() );
//     if( has( c ) && rhs.has( c ) ) { return c; }
//     snark::range_bearing_elevation b( -cross ); // quick and dirty, quite excessive
//     coordinates d( b.e(), b.b() );
//     if( has( d ) && rhs.has( d ) ) { return d; }
//     return boost::optional< coordinates >();
// }

boost::optional< coordinates > great_circle::arc::intersection_with( const arc& rhs ) const
{
    if ( !may_intersect(rhs) )  {
        // std::cerr << "cannot intersect" << std::endl;
        return boost::optional< coordinates >();
    }
    if( equal_( axis(), rhs.axis() ) || equal_( axis(), -rhs.axis() ) ) {
        // std::cerr << "are equal" << std::endl;
        return boost::optional< coordinates >();
    }
    Eigen::Vector3d cross = axis().cross( rhs.axis() );
    if( is_between_( *this, cross ) && is_between_( rhs, cross ) )
    {
        snark::range_bearing_elevation a( cross ); // quick and dirty, quite excessive
        return coordinates( a.e(), a.b() );
    }
    if( is_between_( *this, -cross ) && is_between_( rhs, -cross ) )
    {
        snark::range_bearing_elevation a( -cross ); // quick and dirty, quite excessive
        return coordinates( a.e(), a.b() );
    }
    return boost::optional< coordinates >();
}

std::vector< coordinates > great_circle::arc::discretize( const boost::optional< double >& resolution, const boost::optional< unsigned int >& circle_size ) const
{
    boost::optional< double > r = resolution;
    boost::optional< double > s;
    if( circle_size ) { s = M_PI * 2 / *circle_size; }
    if( ( !r && s ) || ( s && *r > *s ) ) { r = s; }
    if( !r ) { COMMA_THROW( comma::exception, "expected either resolution or circle size, got none" ); }
    std::vector< coordinates > v;
    v.reserve( angle() / *r + 1 );
    for( double a = 0; a < angle(); v.push_back( coordinates( at( a ) ) ), a += *r );
    if( v.back() != end_coordinates_ ) { v.push_back( end_coordinates_ ); }
    return v;
}

} } // namespace snark { namespace spherical {


#include <algorithm>
#include <Eigen/Dense>
#include "nv_core.h"
#include "nv_num.h"
#include "nv_ip.h"
#include "nv_face_detect.h"
#include "nv_face_analyze.h"

#define NV_SKIN_COLOR_SAMPLES 1024
#define NV_SKIN_COLOR_CLASS 8

#define NV_HAIR_COLOR_SAMPLES 1024
#define NV_HAIR_COLOR_CLASS 8

#define NV_EYE_COLOR_SAMPLES 256
#define NV_EYE_COLOR_CLASS 5

static void
nv_get_skin_color(nv_color_t *skin_color,
				  nv_cov_t *skin_cov,
				  const nv_face_position_t *face,
				  const nv_matrix_t *img)
{
	nv_matrix_t *sample = nv_matrix_alloc(3, NV_SKIN_COLOR_SAMPLES);
	nv_matrix_t *skin_sample = nv_matrix_alloc(3, NV_SKIN_COLOR_SAMPLES);
	nv_matrix_t *labels = nv_matrix_alloc(1, NV_SKIN_COLOR_SAMPLES);
	nv_matrix_t *means = nv_matrix_alloc(3, NV_SKIN_COLOR_CLASS);
	nv_matrix_t *count = nv_matrix_alloc(1, NV_SKIN_COLOR_CLASS);
	int m, max_label, k, skin_m;
	cv::Rect skin_rect;

	// 目と口の間
	skin_rect.x = face->right_eye.x;
	skin_rect.y = std::max(
		face->left_eye.y + face->left_eye.height,
		face->right_eye.y + face->left_eye.height);
	skin_rect.width = face->left_eye.x + face->left_eye.width - face->right_eye.x;
	skin_rect.height = face->mouth.y - skin_rect.y;

	for (m = 0; m < NV_SKIN_COLOR_SAMPLES / 2; ++m) {
		int x = skin_rect.x + (int)(skin_rect.width * nv_rand());
		int y = skin_rect.y + (int)(skin_rect.height * nv_rand());

		nv_color_bgr2euclidean_scalar(sample, m, img, NV_MAT_M(img, y, x));
	}
	// 目と目の間
	skin_rect.x = face->right_eye.x + face->right_eye.width;
	skin_rect.y = std::min(face->left_eye.y, face->right_eye.y);
	skin_rect.width = face->left_eye.x - skin_rect.x;
	skin_rect.height = face->nose.y - skin_rect.y;

	for (m = NV_SKIN_COLOR_SAMPLES / 2; m < NV_SKIN_COLOR_SAMPLES; ++m) {
		int x = skin_rect.x + (int)(skin_rect.width * nv_rand());
		int y = skin_rect.y + (int)(skin_rect.height * nv_rand());

		nv_color_bgr2euclidean_scalar(sample, m, img, NV_MAT_M(img, y, x));
	}
	// クラスタリング
	k = nv_kmeans(means, count, labels, sample, NV_SKIN_COLOR_CLASS, 0);

	// 肌選択
	Eigen::Map<Eigen::VectorXf>(count->v, count->m).maxCoeff(&max_label);
	skin_color->v[0] = NV_MAT_V(means, max_label, 0);
	skin_color->v[1] = NV_MAT_V(means, max_label, 1);
	skin_color->v[2] = NV_MAT_V(means, max_label, 2);

	// 肌の分散共分散行列,固有値を計算
	skin_m = 0;
	for (m = 0; m < NV_SKIN_COLOR_SAMPLES; ++m) {
		if (NV_MAT_V(labels, m, 0) == (float)max_label) {
			nv_vector_copy(skin_sample, skin_m, sample, m);
			++skin_m;
		}
	}
	nv_matrix_m(skin_sample, skin_m);
	nv_cov_eigen(skin_cov, skin_sample);

	nv_matrix_free(&sample);
	nv_matrix_free(&skin_sample);
	nv_matrix_free(&means);
	nv_matrix_free(&count);
	nv_matrix_free(&labels);
}

static void nv_get_hair_color(nv_color_t *hair_color,
							  nv_cov_t *hair_cov,
							  const nv_cov_t *skin_cov,
							  const nv_color_t *skin_color,
							  const nv_face_position_t *face,
							  const nv_matrix_t *img)
{
	nv_matrix_t *sample = nv_matrix_alloc(3, NV_HAIR_COLOR_SAMPLES);
	nv_matrix_t *hair_sample = nv_matrix_alloc(3, NV_HAIR_COLOR_SAMPLES);
	nv_matrix_t *labels = nv_matrix_alloc(1, NV_HAIR_COLOR_SAMPLES);
	nv_matrix_t *means = nv_matrix_alloc(3, NV_HAIR_COLOR_CLASS);
	nv_matrix_t *count = nv_matrix_alloc(1, NV_HAIR_COLOR_CLASS);
	nv_matrix_t *skin_likelihood = nv_matrix_alloc(1, NV_HAIR_COLOR_SAMPLES);
	nv_matrix_t *likelihood_means = nv_matrix_alloc(1, NV_HAIR_COLOR_CLASS);
	int m, max_label, skin_label, hair_m;
	cv::Rect hair_rect;
	int k;

	// 目の上から顔の1/3
	hair_rect.x = face->right_eye.x;
	hair_rect.y = std::max(0, std::min(face->left_eye.y, face->right_eye.y) - face->face.height / 3);
	hair_rect.width = face->left_eye.x + face->left_eye.width - hair_rect.x;
	hair_rect.height = std::min(face->left_eye.y, face->right_eye.y) - hair_rect.y;

	if (hair_rect.y + hair_rect.height >= img->rows) {
		hair_rect.height = img->rows - hair_rect.y;
	}

	for (m = 0; m < sample->m; ++m) {
		int x = hair_rect.x + (int)(hair_rect.width * nv_rand());
		int y = hair_rect.y + (int)(hair_rect.height * nv_rand());

		nv_color_bgr2euclidean_scalar(sample, m, img, NV_MAT_M(img, y, x));
	}

	// 肌除去
	for (m = 0; m < sample->m; ++m) {
		NV_MAT_V(skin_likelihood, m, 0) = nv_gaussian_log_predict(skin_cov, sample, m);
	}
	k = nv_kmeans(likelihood_means, count, labels, skin_likelihood, likelihood_means->m, 0);
	Eigen::Map<Eigen::VectorXf>(likelihood_means->v, likelihood_means->m).maxCoeff(&skin_label);
	hair_m = 0;
	for (m = 0; m < sample->m; ++m) {
		if (NV_MAT_V(labels, m, 0) != (float)skin_label) {
			nv_vector_copy(sample, hair_m, sample, m);
			++hair_m;
		}
	}
	nv_matrix_m(sample, hair_m);

	// クラスタリング
	k = nv_kmeans(means, count, labels, sample, NV_HAIR_COLOR_CLASS, 0);
	// 最大メンバクラス選択

	Eigen::Map<Eigen::VectorXf>(count->v, count->m).maxCoeff(&max_label);
	hair_color->v[0] = NV_MAT_V(means, max_label, 0);
	hair_color->v[1] = NV_MAT_V(means, max_label, 1);
	hair_color->v[2] = NV_MAT_V(means, max_label, 2);

	// 髪の分散共分散行列,固有値を計算
	hair_m = 0;
	for (m = 0; m < sample->m; ++m) {
		if (NV_MAT_V(labels, m, 0) == (float)max_label) {
			nv_vector_copy(hair_sample, hair_m, sample, m);
			++hair_m;
		}
	}
	nv_matrix_m(hair_sample, hair_m);
	nv_cov_eigen(hair_cov, hair_sample);

	nv_matrix_free(&sample);
	nv_matrix_free(&hair_sample);
	nv_matrix_free(&means);
	nv_matrix_free(&count);
	nv_matrix_free(&labels);
	nv_matrix_free(&skin_likelihood);
	nv_matrix_free(&likelihood_means);
}

// [3]countで降順ソート用
static int nv_cmp_means(const void *p1, const void *p2)
{
	float *lhs = (float *)p1;
	float *rhs = (float *)p2;

	if (lhs[3] < rhs[3]) {
		return 1;
	} else if (lhs[3] > rhs[3]) {
		return -1;
	}
	return 0;
}

static void nv_get_eye_color(nv_color_t *eye_colors,
							 nv_matrix_t *sample,
							 const nv_color_t *skin_color,
							 const nv_cov_t *skin_cov,
							 const nv_color_t *hair_color,
							 const nv_cov_t *hair_cov,
							 const CvRect *eye_rect,
							 const nv_matrix_t *img)
{
	int m, c;
	int skin_label, hair_label;
	int color_samples = sample->m;

	nv_cov_t *eye_cov = nv_cov_alloc(skin_cov->n);
	nv_matrix_t *skin_likelihood = nv_matrix_alloc(1, color_samples);
	nv_matrix_t *hair_likelihood = nv_matrix_alloc(1, color_samples);
	nv_matrix_t *labels = nv_matrix_alloc(1, color_samples);
	nv_matrix_t *means = nv_matrix_alloc(3, NV_EYE_COLOR_CLASS);
	nv_matrix_t *likelihood_means = nv_matrix_alloc(1, NV_EYE_COLOR_CLASS);
	nv_matrix_t *count = nv_matrix_alloc(1, NV_EYE_COLOR_CLASS);
	nv_matrix_t *sorted_means = nv_matrix_alloc(3 + 1, NV_EYE_COLOR_CLASS);
	int k;
	int eye_m;
	int x, y;

	// 目の色サンプリング
	m = 0;
	for (y = eye_rect->y; y < eye_rect->y + eye_rect->height; ++y) {
		for (x = eye_rect->x; x < eye_rect->x + eye_rect->width; ++x) {
			nv_color_bgr2euclidean_scalar(sample, m, img, NV_MAT_M(img, y, x));
			++m;
		}
	}

	// 肌と髪を除去

	// 肌除去
	for (m = 0; m < sample->m; ++m) {
		NV_MAT_V(skin_likelihood, m, 0) = nv_gaussian_log_predict(skin_cov, sample, m);
	}
	k = nv_kmeans(likelihood_means, count, labels, skin_likelihood, likelihood_means->m, 0);
	Eigen::Map<Eigen::VectorXf>(likelihood_means->v, likelihood_means->m).maxCoeff(&skin_label);
	eye_m = 0;
	for (m = 0; m < sample->m; ++m) {
		if (NV_MAT_V(labels, m, 0) != (float)skin_label) {
			nv_vector_copy(sample, eye_m, sample, m);
			++eye_m;
		}
	}
	nv_matrix_m(sample, eye_m);

	// 髪除去
	for (m = 0; m < sample->m; ++m) {
		NV_MAT_V(hair_likelihood, m, 0) = nv_gaussian_log_predict(hair_cov, sample, m);
	}
	nv_matrix_m(hair_likelihood, sample->m);

	k = nv_kmeans(likelihood_means, count, labels, hair_likelihood, likelihood_means->m, 0);
	Eigen::Map<Eigen::VectorXf>(likelihood_means->v, likelihood_means->m).maxCoeff(&skin_label);
	eye_m = 0;
	for (m = 0; m < sample->m; ++m) {
		if (NV_MAT_V(labels, m, 0) != (float)hair_label) {
			nv_vector_copy(sample, eye_m, sample, m);
			++eye_m;
		}
	}
	nv_matrix_m(sample, eye_m);

	// 目のクラスタリング
	k = nv_kmeans(means, count, labels, sample, NV_EYE_COLOR_CLASS, 0);

	// カウントでソート
	for (m = 0; m < k; ++m) {
		NV_MAT_V(sorted_means, m, 0) = NV_MAT_V(means, m, 0);
		NV_MAT_V(sorted_means, m, 1) = NV_MAT_V(means, m, 1);
		NV_MAT_V(sorted_means, m, 2) = NV_MAT_V(means, m, 2);
		NV_MAT_V(sorted_means, m, 3) = NV_MAT_V(count, m, 0);
	}
	// ソート (label破壊)
	qsort(sorted_means->v, k, sizeof(float) * sorted_means->n, nv_cmp_means);

	// 色
	c = 0;
	for (m = 0; m < k; ++m) {
		eye_colors[c].v[0] = NV_MAT_V(sorted_means, m, 0);
		eye_colors[c].v[1] = NV_MAT_V(sorted_means, m, 1);
		eye_colors[c].v[2] = NV_MAT_V(sorted_means, m, 2);
		eye_colors[c].v[3] = NV_MAT_V(count, m, 0);
		++c;
		if (c == 4) {
			break;
		}
	}
	nv_matrix_free(&skin_likelihood);
	nv_matrix_free(&hair_likelihood);
	nv_matrix_free(&means);
	nv_matrix_free(&likelihood_means);
	nv_matrix_free(&count);
	nv_matrix_free(&labels);
	nv_matrix_free(&sorted_means);
	nv_cov_free(&eye_cov);
}

static void
nv_get_eye_colors(nv_color_t *eye_colors,
				  nv_color_t *left_eye_colors,
				  nv_color_t *right_eye_colors,
				  const nv_color_t *skin_color,
				  const nv_cov_t *skin_cov,
				  const nv_color_t *hair_color,
				  const nv_cov_t *hair_cov,
				  const nv_face_position_t *face,
				  const nv_matrix_t *img)
{
	nv_matrix_t *left_sample = nv_matrix_alloc(3, face->left_eye.height * face->left_eye.width);
	nv_matrix_t *right_sample = nv_matrix_alloc(3, face->right_eye.height * face->right_eye.width);
	int color_samples = left_sample->m + right_sample->m;
	nv_matrix_t *sample = nv_matrix_alloc(3, color_samples);
	nv_matrix_t *labels = nv_matrix_alloc(1, color_samples);
	nv_matrix_t *means = nv_matrix_alloc(3, NV_EYE_COLOR_CLASS);
	nv_matrix_t *count = nv_matrix_alloc(1, NV_EYE_COLOR_CLASS);
	nv_matrix_t *sorted_means = nv_matrix_alloc(3 + 1, NV_EYE_COLOR_CLASS);
	int m, c, epoch;

	// 左目
	nv_get_eye_color(
		left_eye_colors, left_sample,
		skin_color, skin_cov,
		hair_color, hair_cov,
		&face->left_eye, img);
	// 右目
	nv_get_eye_color(
		right_eye_colors, right_sample,
		skin_color, skin_cov,
		hair_color, hair_cov,
		&face->right_eye, img);

	// サンプル数更新
	color_samples = right_sample->m + left_sample->m;
	nv_matrix_m(sample, color_samples);

	// 両目
	// 目の色サンプリング
	for (m = 0; m < left_sample->m; ++m) {
		NV_MAT_V(sample, m, 0) = NV_MAT_V(left_sample, m, 0);
		NV_MAT_V(sample, m, 1) = NV_MAT_V(left_sample, m, 1);
		NV_MAT_V(sample, m, 2) = NV_MAT_V(left_sample, m, 2);
	}
	for (m = 0; m < right_sample->m; ++m) {
		NV_MAT_V(sample, m + left_sample->m, 0) = NV_MAT_V(right_sample, m, 0);
		NV_MAT_V(sample, m + left_sample->m, 1) = NV_MAT_V(right_sample, m, 1);
		NV_MAT_V(sample, m + left_sample->m, 2) = NV_MAT_V(right_sample, m, 2);
	}
	epoch = nv_kmeans(means, count, labels, sample, NV_EYE_COLOR_CLASS, 0);

	// カウントでソート
	for (m = 0; m < NV_EYE_COLOR_CLASS; ++m) {
		NV_MAT_V(sorted_means, m, 0) = NV_MAT_V(means, m, 0);
		NV_MAT_V(sorted_means, m, 1) = NV_MAT_V(means, m, 1);
		NV_MAT_V(sorted_means, m, 2) = NV_MAT_V(means, m, 2);
		NV_MAT_V(sorted_means, m, 3) = NV_MAT_V(count, m, 0);
	}
	// カウントでソート, !labels破壊!
	qsort(sorted_means->v, NV_EYE_COLOR_CLASS, sizeof(float) * sorted_means->n, nv_cmp_means);

	// 色
	c = 0;
	for (m = 0; m < NV_EYE_COLOR_CLASS; ++m) {
		eye_colors[c].v[0] = NV_MAT_V(sorted_means, m, 0);
		eye_colors[c].v[1] = NV_MAT_V(sorted_means, m, 1);
		eye_colors[c].v[2] = NV_MAT_V(sorted_means, m, 2);
		++c;
		if (c == 4) {
			break;
		}
	}

	nv_matrix_free(&sample);
	nv_matrix_free(&left_sample);
	nv_matrix_free(&right_sample);
	nv_matrix_free(&means);
	nv_matrix_free(&sorted_means);
	nv_matrix_free(&count);
	nv_matrix_free(&labels);
}

float nv_eye_ratio(const nv_face_position_t *face)
{
	float left = (float)face->left_eye.width / face->left_eye.height;
	float right = (float)face->right_eye.width / face->right_eye.height;

	return std::max(left, right);
}

float nv_face_ratio(const nv_face_position_t *face)
{
	float left = (float)face->left_eye.width / face->left_eye.height;
	float right = (float)face->right_eye.width / face->right_eye.height;
	float eye_height = (float)((left > right) ? face->left_eye.height: face->right_eye.height);
	float face_height = (float)std::max(face->left_eye.y, face->right_eye.y) - face->chin.y;

	return face_height / eye_height;
}

static void nv_ec2bgr(const nv_color_t *ec, nv_color_t *bgr)
{
	nv_matrix_t *tmp = nv_matrix_alloc(3, 2);
	NV_MAT_V(tmp, 0, 0) = ec->v[0];
	NV_MAT_V(tmp, 0, 1) = ec->v[1];
	NV_MAT_V(tmp, 0, 2) = ec->v[2];
	nv_color_euclidean2bgr_scalar(tmp, 1, tmp, 0);
	bgr->v[0] = NV_MAT_V(tmp, 1, 0);
	bgr->v[1] = NV_MAT_V(tmp, 1, 1);
	bgr->v[2] = NV_MAT_V(tmp, 1, 2);

	nv_matrix_free(&tmp);
}

void
nv_face_analyze(nv_face_feature_t *feature,
				const nv_face_position_t *face,
				const nv_matrix_t *img)
{
	int i;
	nv_cov_t *skin_cov = nv_cov_alloc(3);
	nv_cov_t *hair_cov = nv_cov_alloc(3);

	nv_get_skin_color(&feature->skin_ec, skin_cov, face, img);
	nv_get_hair_color(&feature->hair_ec, hair_cov, skin_cov, &feature->skin_ec, face, img);
	nv_get_eye_colors(
		feature->eye_ec,
		feature->left_eye_ec,
		feature->right_eye_ec,
		&feature->skin_ec, skin_cov,
		&feature->hair_ec, hair_cov,
		face, img);
	feature->eye_ratio = nv_eye_ratio(face);
	feature->face_ratio = nv_face_ratio(face);

	// 色空間変換
	nv_ec2bgr(&feature->skin_ec, &feature->skin_bgr);
	nv_ec2bgr(&feature->hair_ec, &feature->hair_bgr);
	for (i = 0; i < 4; ++i) {
		nv_ec2bgr(&feature->eye_ec[i], &feature->eye_bgr[i]);
		nv_ec2bgr(&feature->left_eye_ec[i], &feature->left_eye_bgr[i]);
		nv_ec2bgr(&feature->right_eye_ec[i], &feature->right_eye_bgr[i]);
	}

	nv_cov_free(&skin_cov);
	nv_cov_free(&hair_cov);
}


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/2dlrf/ErrorTermMotion.h"

#include <Eigen/Dense>

#include <sm/kinematics/rotations.hpp>

#include <aslam/calibration/data-structures/VectorDesignVariable.h>

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    ErrorTermMotion::ErrorTermMotion(VectorDesignVariable<3>* xkm1,
        VectorDesignVariable<3>* xk, double T, const Input& uk,
        const Covariance& Q) :
        _xkm1(xkm1),
        _xk(xk),
        _T(T),
        _uk(uk),
        _Q(Q) {
      setInvR(_Q.inverse());
      setDesignVariables(xkm1, xk);
    }

    ErrorTermMotion::ErrorTermMotion(const ErrorTermMotion& other) :
        ErrorTermFs<3>(other),
        _xkm1(other._xkm1),
        _xk(other._xk),
        _T(other._T),
        _uk(other._uk),
        _Q(other._Q) {
    }

    ErrorTermMotion& ErrorTermMotion::operator =
        (const ErrorTermMotion& other) {
      if (this != &other) {
        ErrorTermFs<3>::operator=(other);
       _xkm1 = other._xkm1;
       _xk = other._xk;
       _T = other._T;
       _uk = other._uk;
       _Q = other._Q;
      }
      return *this;
    }

    ErrorTermMotion::~ErrorTermMotion() {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    double ErrorTermMotion::getTimestep() const {
      return _T;
    }

    void ErrorTermMotion::setTimestep(double T) {
      _T = T;
    }

    const ErrorTermMotion::Input& ErrorTermMotion::getInput() const {
      return _uk;
    }

    ErrorTermMotion::Input& ErrorTermMotion::getInput() {
      return _uk;
    }

    void ErrorTermMotion::setInput(const Input& uk) {
      _uk = uk;
    }

    const ErrorTermMotion::Covariance& ErrorTermMotion::getCovariance() const {
      return _Q;
    }

    ErrorTermMotion::Covariance& ErrorTermMotion::getCovariance() {
      return _Q;
    }

    void ErrorTermMotion::setCovariance(const Covariance& Q) {
      _Q = Q;
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    double ErrorTermMotion::evaluateErrorImplementation() {
      Eigen::Matrix<double, 3, 3> B = Eigen::Matrix<double, 3, 3>::Identity();
      B(0, 0) = cos((_xkm1->getValue())(2));
      B(0, 1) = sin((_xkm1->getValue())(2));
      B(1, 0) = -sin((_xkm1->getValue())(2));
      B(1, 1) = cos((_xkm1->getValue())(2));
      error_t error = _uk -
        (1 / _T * B * (_xk->getValue() - _xkm1->getValue()));
      setError(error);
      return evaluateChiSquaredError();
    }

    void ErrorTermMotion::evaluateJacobiansImplementation(
        aslam::backend::JacobianContainer& jacobians) {
      Eigen::Matrix<double, 3, 3> Hxk = Eigen::Matrix<double, 3, 3>::Zero();
      Hxk(0, 0) = cos((_xkm1->getValue())(2));
      Hxk(0, 1) = sin((_xkm1->getValue())(2));
      Hxk(1, 0) = -sin((_xkm1->getValue())(2));
      Hxk(1, 1) = cos((_xkm1->getValue())(2));
      Hxk(2, 2) = 1;
      Eigen::Matrix<double, 3, 3> Hxkm1 =
        Eigen::Matrix<double, 3, 3>::Zero();
      Hxkm1(0, 0) = -cos((_xkm1->getValue())(2));
      Hxkm1(0, 1) = -sin((_xkm1->getValue())(2));
      Hxkm1(0, 2) = -sin((_xkm1->getValue())(2)) *
        ((_xk->getValue())(0) - (_xkm1->getValue())(0)) +
        cos((_xkm1->getValue())(2)) *
        ((_xk->getValue())(1) - (_xkm1->getValue())(1));
      Hxkm1(1, 0) = sin((_xkm1->getValue())(2));
      Hxkm1(1, 1) = -cos((_xkm1->getValue())(2));
      Hxkm1(1, 2) = -cos((_xkm1->getValue())(2)) *
        ((_xk->getValue())(0) - (_xkm1->getValue())(0)) -
        sin((_xkm1->getValue())(2)) *
        ((_xk->getValue())(1) - (_xkm1->getValue())(1));
      Hxkm1(2, 2) = -1;
      jacobians.add(_xkm1, -Hxkm1 / _T);
      jacobians.add(_xk, -Hxk / _T);
    }

  }
}


#include <boost/range/adaptor/reversed.hpp>

#include "PackedLayout.h"

#include "Scale.h"

#include "../MessageSystem.h"

namespace Kriti {
namespace GUI {

Math::Vector PackedLayout::minSize() {
    if(m_dir == Vertical) {
        double minY = 0.0;
        double maxX = 0.0;

        for(auto item : m_items) {
            maxX = std::max(item->minSize().x(), maxX);
            minY += item->minSize().y();
        }

        if(m_items.size() > 0) {
            minY += (m_items.size()-1) * Scale().padding().y() * scale().y();
        }

        return Math::Vector(maxX, minY);
    }
    else return Math::Vector();
}

void PackedLayout::updated(boost::shared_ptr<OutlineRegistry> registry,
    Math::Vector clipStart, Math::Vector clipEnd) {

    if(m_dir == Vertical) {
        double minY = 0.0;
        double stretchY = 0.0;
        double ypad = Scale().padding().y() * scale().y();

        for(auto item : m_items) {
            minY += item->minSize().y();
            stretchY += item->stretch().y();
        }

        if(m_items.size() > 1) {
            minY += (m_items.size()-1) * ypad;
        }

        if(stretchY == 0.0) stretchY = 1.0;

        double extraY = size().y() - minY;
        if(extraY < 0) {
            Message3(GUI, Debug,
                "Trying to pack GUI items into too small of a space!");
            extraY = 0.0;
        }

        double cursor = 0.0;
        for(auto item : boost::adaptors::reverse(m_items)) {
            double sfactor = item->stretch().y();
            double height = item->minSize().y();

            height += (sfactor / stretchY) * extraY;
            item->update(registry, pos() + Math::Vector(0.0, cursor),
                Math::Vector(size().x(), height), scale(), clipStart, clipEnd);

            cursor += height;
            cursor += ypad;
        }
    }
}

}  // namespace GUI
}  // namespace Kriti


// \file rk.hpp
//    - Project:     SOFTSUSY
//    - Author:      Ben Allanach
//    - Manual:      hep-ph/0104145, Comp. Phys. Comm. 143 (2002) 305
//    - Webpage:     http://allanach.home.cern.ch/allanach/softsusy.html
//    - Description: Integration of ODEs by Runge Kutta, minimum finding and
//                 derivative calculation

#ifndef RK_H
#define RK_H

#include <boost/function.hpp>
#include <Eigen/Dense>

namespace flexiblesusy {

namespace runge_kutta {

typedef boost::function<Eigen::ArrayXd(double, const Eigen::ArrayXd&)> Derivs;

/// A single step of Runge Kutta (5th order), input:
/// y and dydx (derivative of y), x is independent variable. yout is value
/// after step. derivs is a user-supplied function
void rungeKuttaStep(const Eigen::ArrayXd& y, const Eigen::ArrayXd& dydx,
	double x, double h, Eigen::ArrayXd& yout, Eigen::ArrayXd& yerr,
	Derivs derivs);

/// organises the variable step-size for Runge-Kutta evolution
void odeStepper(Eigen::ArrayXd& y, const Eigen::ArrayXd& dydx, double *x,
                double htry, double eps, const Eigen::ArrayXd& yscal,
                double *hdid, double *hnext, Derivs derivs, int& max_step_dir);

typedef boost::function<decltype(odeStepper)> RungeKuttaQuinticStepper;

/// Organises integration of 1st order system of ODEs
void integrateOdes(Eigen::ArrayXd& ystart, double x1, double x2, double eps,
                   double h1, double hmin, Derivs derivs,
                   RungeKuttaQuinticStepper rkqs);

} // namespace runge_kutta

}

#endif // RK_H


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

#ifndef SOLVERS_PETSCSOLVER_HPP
#define SOLVERS_PETSCSOLVER_HPP

#include <Eigen/Dense>

#include <petscksp.h>

#include <solvers/Solver.hpp>
#include <solvers/SparseSystem.hpp>

namespace solvers {

enum class PetscMethod {
    CG,
    FlexibleCG,
    GMRES,
    FlexibleGMRES,
    QMR,
    CR,
    MinRes,
    SymmLQ,
    Cholesky,
    LU,
    QR
};

enum class PetscPreconditioner { None, Jacobi, SOR, Eisenstat, ILU, ICC };

/**
 * \brief PetscSolver Solver based on PETSc library.
 */
class PetscSolver : public Solver {
private:
    /**
     * \brief _ksp Resolution algorithm.
     */
    KSP _ksp{};

public:
    explicit PetscSolver(
        PetscMethod method = PetscMethod::CG,
        PetscPreconditioner preconditioner = PetscPreconditioner::None);

    Eigen::VectorXd solve(const SparseSystem& system,
                          double& duration) const override;
};

}    // namespace solvers

#endif    // SOLVERS_PETSCSOLVER_HPP


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2015 Adam Wulkiewicz, Lodz, Poland.

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

//[linestring
//` Declaration and use of the Boost.Geometry model::linestring, modelling the Linestring Concept

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

namespace bg = boost::geometry;

int main()
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
    typedef bg::model::linestring<point_t> linestring_t;

    linestring_t ls1; /*< Default-construct a linestring. >*/

#if !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX) \
 && !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST)

    linestring_t ls2{{0.0, 0.0}, {1.0, 0.0}, {1.0, 2.0}}; /*< Construct a linestring containing three points, using C++11 unified initialization syntax. >*/

#endif

    bg::append(ls1, point_t(0.0, 0.0)); /*< Append point. >*/
    bg::append(ls1, point_t(1.0, 0.0));
    bg::append(ls1, point_t(1.0, 2.0));

    double l = bg::length(ls1);

    std::cout << l << std::endl;

    return 0;
}

//]

//[linestring_output
/*`
Output:
[pre
3
]
*/
//]


//
// Created by Alex Beccaro on 20/01/2019.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/121/problem121.hpp"

BOOST_AUTO_TEST_SUITE( Problem121 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem121::solve(4);
        BOOST_CHECK_EQUAL(res, 10);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem121::solve();
        BOOST_CHECK_EQUAL(res, 2269);
    }

BOOST_AUTO_TEST_SUITE_END()