#include <string>

//#include "tracker_irls.h"
//#include "tracker_pf_texture.h"
//#include "tracker_pf_textureless.h"

#include <boost/signals2/mutex.hpp>
#include <boost/thread.hpp>
#include <boost/array.hpp>
#include <boost/asio.hpp>

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <omp.h>        // openmp
#include <iostream>
//#include <direct.h> // mkdir(), creating a directory

#include <boost/program_options.hpp>
#include <boost/algorithm/string.hpp>

#include <limits.h>

#include "object_tracking_2D/ObjectModel.h"

#include "Fitline/LFLineFitter.h"

namespace po = boost::program_options;

CvPoint2D32f project3Dto2D(CvPoint3D32f pt3, CvMat* pose, CvMat* param_intrinsic)
{
  CvPoint2D32f pt2;
  CvPoint3D32f pt3_cam;
  pt3_cam.x = CV_MAT_ELEM(*pose, float, 0, 0)*pt3.x + CV_MAT_ELEM(*pose, float, 0, 1)*pt3.y + CV_MAT_ELEM(*pose, float, 0, 2)*pt3.z + CV_MAT_ELEM(*pose, float, 0, 3);
  pt3_cam.y = CV_MAT_ELEM(*pose, float, 1, 0)*pt3.x + CV_MAT_ELEM(*pose, float, 1, 1)*pt3.y + CV_MAT_ELEM(*pose, float, 1, 2)*pt3.z + CV_MAT_ELEM(*pose, float, 1, 3);
  pt3_cam.z = CV_MAT_ELEM(*pose, float, 2, 0)*pt3.x + CV_MAT_ELEM(*pose, float, 2, 1)*pt3.y + CV_MAT_ELEM(*pose, float, 2, 2)*pt3.z + CV_MAT_ELEM(*pose, float, 2, 3);

  float fx = CV_MAT_ELEM(*param_intrinsic, float, 0, 0);
  float fy = CV_MAT_ELEM(*param_intrinsic, float, 1, 1);
  float ux = CV_MAT_ELEM(*param_intrinsic, float, 0, 2);
  float uy = CV_MAT_ELEM(*param_intrinsic, float, 1, 2);

  pt2.x = fx*pt3_cam.x/pt3_cam.z + ux;
  pt2.y = fy*pt3_cam.y/pt3_cam.z + uy;

  float th_d = 10000.0;
  if(pt2.x < -th_d || pt2.x > th_d || pt2.y < -th_d || pt2.y > th_d)
  {
    pt2.x = -1;
    pt2.y = -1;
  }

  return pt2;
}

CvRect drawModel(IplImage* img, std::vector<CvPoint3D32f> ep1, std::vector<CvPoint3D32f> ep2, CvMat* pose, CvMat* param_intrinsic, CvScalar color)
{
  float widthf = static_cast<float>(img->width), heightf = static_cast<float>(img->height);
  CvPoint2D32f pt21, pt22;
  float l = numeric_limits<float>::max(), r = 0.0f, t = numeric_limits<float>::max(), b = 0.0f;
  for(int i=0; i<ep1.size(); i++)
  {
    pt21 = project3Dto2D(ep1[i], pose, param_intrinsic);
    pt22 = project3Dto2D(ep2[i], pose, param_intrinsic);
    cvLine(img, cvPointFrom32f(pt21), cvPointFrom32f(pt22), color, 1, 8);
    if(pt21.x < l) l = pt21.x;
    if(pt21.x > r) r = pt21.x;
    if(pt21.y < t) t = pt21.y;
    if(pt21.y > b) b = pt21.y;

    if(pt22.x < l) l = pt22.x;
    if(pt22.x > r) r = pt22.x;
    if(pt22.y < t) t = pt22.y;
    if(pt22.y > b) b = pt22.y;
  }

  l = max(0.f, l);
  r = min(widthf - 1.f, r);
  t = max(0.f, t);
  b = min(heightf - 1.f, b);
  return cvRect(static_cast<int>(l), static_cast<int>(t), static_cast<int>(r-l+1.f), static_cast<int>(b-t+1.f));
}

int main(int argc, char **argv)
{
  std::string obj_name;
  std::string intrinsic;
  int width;
  int height;
  float sample_step;
  bool dull_edge;
  std::string str_result_path;
  std::string str_param_linfit;
  float depth;

  po::options_description desc("\nTemplate generator keys:\nk: save current template\nw: translate further\ns: translate closer\nq,e,a,d,z,d: rotates in 3 axes\n\nTemplate generator options");
  desc.add_options()
    ("help,h", "produce help message")
    ("obj-name,o", po::value<std::string>(&obj_name), "name of traget object")
    ("sample-step,s", po::value<float>(&sample_step)->default_value(0.005f), "sample step")
    ("depth,d", po::value<float>(&depth)->default_value(0.3f), "distance between object and camera")
    ("param-linefit,l", po::value<std::string>(&str_param_linfit)->default_value(std::string("para_template_line_fitter.txt")), "set parameters for line fitting")
    ("save-path,p", po::value<std::string>(&str_result_path)->default_value(std::string("data_newobject")), "set result path")

    ("width", po::value<int>(&width)->default_value(640), "width")
    ("height", po::value<int>(&height)->default_value(480), "height")
    ("intrinsic", po::value<std::string>(&intrinsic)->default_value("Intrinsics_normal.xml"), "intrinsic parameters")
    ("dull_edge", po::value<bool>(&dull_edge)->default_value(true), "consider dull edges")
  ;
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if(argc < 2 || vm.count("help"))
  {
    std::cout << desc << std::endl;
    return 1;
  }

  if(obj_name.empty())
  {
    std::cerr << "obj-name should be specified." << std::endl;
    return 1;
  }

  if(vm.count("obj-name"))
  {
    std::cout << "obj-name: " << vm["obj-name"].as<std::string>() << std::endl;
  }

  if(vm.count("save-path"))
  {
    if(mkdir(str_result_path.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1 && errno != EEXIST)
    {
      std::cerr << "Cannot create " << str_result_path << " directory for saving results." << std::endl;
      return false;
    }
  }

  IplImage* img_result = cvCreateImage(cvSize(width, height), 8, 1);
  CvMat* pose = cvCreateMat(4, 4, CV_32F);
  char key = 0;
  float modelPosition[3];
  float modelAngle[3];
  float matrixModel[16];
  modelPosition[0] = 0.0;
  modelPosition[1] = 0.0;
  modelPosition[2] = depth;
  modelAngle[0] = 0.0;
  modelAngle[1] = 0.0;
  modelAngle[2] = 0.0;
  int int_not = 0; // number of templates

  CvMat* param_intrinsic = (CvMat*)cvLoad(intrinsic.c_str());

  // Create object model instance
  int maxd = 16;
  CObjectModel cObjModel(obj_name, width, height, param_intrinsic, sample_step, maxd, dull_edge, NULL);

  cvSetIdentity(pose);
  CV_MAT_ELEM(*pose, float, 2, 3) = depth;

  // init line fitter
  LFLineFitter lf;
  lf.Init();
  lf.Configure("para_template_line_fitter.txt");

  cvNamedWindow("Edge");

  while(key != 27) // until 'esc'
  {
    cvSet(img_result, cvScalar(0)); // reset image

    glPushMatrix();
    glMatrixMode(GL_MODELVIEW);
    // In order to get the modeling matrix only, reset GL_MODELVIEW matrix
    glLoadIdentity();
    // transform the object
    // From now, all transform will be for modeling matrix only. (transform from object space to world space)
    glTranslatef(modelPosition[0], modelPosition[1], modelPosition[2]);
    glRotatef(modelAngle[0], 1, 0, 0);
    glRotatef(modelAngle[1], 0, 1, 0);
    glRotatef(modelAngle[2], 0, 0, 1);
    // save modeling matrix
    glGetFloatv(GL_MODELVIEW_MATRIX, matrixModel);

    CvMat* poset = cvCreateMat(4, 4, CV_32F);
    memcpy(poset->data.fl, matrixModel, sizeof(float)*16);
    cvTranspose(poset, pose);
    cvReleaseMat(&poset);
    glPopMatrix();

    // Draw object model with visibility test
    cObjModel.setModelviewMatrix(pose); // update the initial pose to object model for displaying
    cObjModel.findVisibleSamplePoints(); // draw object model with visibility test

    // Find visible edges
    std::vector<CvPoint3D32f> ep1, ep2;

    std::vector<CObjectModel::SamplePoint>& vsp = cObjModel.getVisibleSamplePoints();

    // determine two end points in each common edge_mem sample points
    int edge_mem = vsp[0].edge_mem;
    ep1.push_back(vsp[0].coord3);
    int i;
    for(i=0; i<int(vsp.size()); i++)
    {
      if(edge_mem == vsp[i].edge_mem)
      {
        // skip over
      }
      else
      {
        // new point, so add end/starting edge point
        ep2.push_back(vsp[i-1].coord3);
        ep1.push_back(vsp[i].coord3);
        // update new edge_mem value
        edge_mem = vsp[i].edge_mem;
      }
    }
    ep2.push_back(vsp[i-1].coord3);

    CvRect bound = drawModel(img_result, ep1, ep2, pose, param_intrinsic, CV_RGB(255, 255, 255));

    cvShowImage("Edge", img_result);
    cout << "position: " << modelPosition[0] << ", " << modelPosition[1] << ", " << modelPosition[2] << " angle: " << modelAngle[0] << ", " << modelAngle[1] << ", " << modelAngle[2] << endl;
    key = cvWaitKey(0);
    switch(key)
    {
    case 'w': // further
      modelPosition[2] += 0.02f;
      break;
    case 's': // closer
      modelPosition[2] -= 0.02f;
      break;
    case 'a':
      modelAngle[2] += 5.f;
      break;
    case 'd':
      modelAngle[2] -= 5.f;
      break;
    case 'q':
      modelAngle[0] += 10.f;
      break;
    case 'e':
      modelAngle[0] -= 10.f;
      break;
    case 'z':
      modelAngle[1] += 30.f;
      break;
    case 'c':
      modelAngle[1] -= 30.f;
      break;
    case 'k':
      // save into image and xml
      char buf[50];

      // save edge template
      cvSetImageROI(img_result, bound);
      sprintf(buf, "/edge_template%03d.png", int_not);
      cvSaveImage((str_result_path + buf).c_str(), img_result);

      // fit lines
      IplImage* img = cvLoadImage((str_result_path + buf).c_str(), 0);
      lf.FitLine(img);
      sprintf(buf, "/%s_edge_template_line%03d.png", obj_name.c_str(), int_not);
      lf.DisplayEdgeMap(img, (str_result_path + buf).c_str());
      cvReleaseImage(&img);

      sprintf(buf, "/%s_edge_template%03d.txt", obj_name.c_str(), int_not);
      lf.SaveEdgeMap((str_result_path + buf).c_str());

      cvResetImageROI(img_result);

      // save template pose
      sprintf(buf, "/edge_template_pose%03d.xml", int_not);
      cvSave((str_result_path + buf).c_str(), pose);

      int_not++;
      break;
    }
    cvWaitKey(10);
  }

  cvDestroyWindow("Edge");
  cvReleaseMat(&pose);
  cvReleaseImage(&img_result);

  return (0);
}


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

#ifndef WANGTETER_HPP
#define	WANGTETER_HPP

#include <armadillo>
#include <memory>
#include "KEDF.hpp"
#include "FourierGrid.hpp"
#include "HelperFunctions.hpp"
using namespace std;
using namespace arma;

template<class GridType>
class WangTeter: public KEDF<GridType> {

public:

    WangTeter(GridType* example, const double alpha, const double beta, cube* keKernel)
    : _alpha(alpha), _beta(beta), _keKernel(keKernel){
    }

    ~WangTeter() {
    }

    string getMethodDescription() const {
        return "Wang-Teter KEDF";
    }

    vector<string> getCitations() const {

        vector<string> citations(0);
        citations.push_back("L.-W. Wang and M. P. Teter, Phys. Rev. B 45, 13196 (1992).");

        return citations;
    }

    vector<string> getWorkingEquations() const {

        vector<string> equations(0);
        equations.push_back("");

        //   WTEnergy = cTF * SUM(rhoR_SI**alpha * FFT(FFT(rhoR_SI**beta) * keKernel(:,:,:,1)))*/

        return equations;
    }

    double calcEnergy(const GridType& grid) const {

        unique_ptr<GridType> workGridB = grid.duplicate();

        workGridB->powGrid(_beta);

        cx_cube* recB = workGridB->getReciprocalGrid();

        const uword recElems = recB->n_elem;

        #pragma omp parallel for default(none) shared(recB)
        for (uword x = 0; x < recElems; ++x) {
            recB->at(x) *= _keKernel->at(x);
        }
        workGridB->completeReciprocal(recB);

        cube* realB = workGridB->getRealGrid();

        const uword elems = realB->n_elem;

        const cube* dens = grid.tryReadRealGrid();
        if (!dens) {
            // unfortunately, we need to get a copy of the grid
            unique_ptr<FourierGrid> workGrid = grid.createFourierDuplicate();
            dens = workGrid->readRealGrid();
        }

        #pragma omp parallel for default(none) shared(realB,dens)
        for (uword x = 0; x < elems; ++x) {
            realB->at(x) *= pow(dens->at(x), _alpha);
        }
        workGridB->complete(realB);

        const double eWT = _CTF * workGridB->integrate();

        return eWT;
    }

    double calcPotential(const GridType& grid, GridType& potential) const {

        const cube* dens = grid.tryReadRealGrid();
        if (!dens) {
            // unfortunately, we need to get a copy of the grid
            unique_ptr<GridType> workGrid = grid.duplicate();
            dens = workGrid->readRealGrid();
        }

        unique_ptr<GridType> workGridA = grid.duplicate();
        cube* densityA = workGridA->getRealGrid();

        const uword elems = densityA->n_elem;
        const size_t nSlices = densityA->n_slices;
        const size_t nRows = densityA->n_rows;
        const size_t nCols = densityA->n_cols;

        auto densityB = MemoryFunctions::allocateScratch(nRows, nCols, nSlices);
        cube* poten = potential.getRealGrid();

        #pragma omp parallel for default(none) shared(densityA,densityB,poten)
        for (uword x = 0; x < elems; ++x) {
            const double rho = densityA->at(x);
            const double rhoPBM = pow(rho, (_beta - 1));
            densityB->at(x) = rhoPBM; // density B contains rho**(beta-1)
            poten->at(x) = rhoPBM*rho; // poten contains rho**(beta)
            densityA->at(x) = pow(rho, _alpha); // density A contains rho**(alpha)
        }
        potential.complete(poten);
        workGridA->complete(densityA);

        cx_cube* recPot = potential.getReciprocalGrid();

        const uword recElems = recPot->n_elem;

        #pragma omp parallel for default(none) shared(recPot)
        for (uword x = 0; x < recElems; ++x) {
            recPot->at(x) *= _keKernel->at(x);
        }
        potential.completeReciprocal(recPot);

        potential.multiplyElementwise(workGridA.get());

        // get the energy
        const double eWT = _CTF * potential.integrate();

        // transform what is currently in potential into the first part of the actual potential
        cube* realPot = potential.getRealGrid();

        const double preAl = _CTF*_alpha;
        #pragma omp parallel for default(none) shared(realPot,dens)
        for (uword x = 0; x < elems; ++x) {
            realPot->at(x) *= preAl / dens->at(x);
        }

        // now take what is in densityA and FFT it
        cx_cube* recDensA = workGridA->getReciprocalGrid();

        #pragma omp parallel for default(none) shared(recDensA)
        for (size_t x = 0; x < recElems; ++x) {
            recDensA->at(x) *= _keKernel->at(x);
        }
        workGridA->completeReciprocal(recDensA);

        densityA = workGridA->getRealGrid();

        const double preBe = _CTF*_beta;
        #pragma omp parallel for default(none) shared(realPot,densityA,densityB)
        for (size_t x = 0; x < elems; ++x) {
            const double pot = densityA->at(x) * preBe * densityB->at(x);
            realPot->at(x) += pot;
        }
        potential.complete(realPot);

        return eWT;
    }

    unique_ptr<StressTensor> calcStress(const GridType& grid) const {
        throw runtime_error("not yet implemented");
    }

private:

    const double _CTF = 2.87123400018819;
    const double _alpha;
    const double _beta;
    const cube * _keKernel;
};

class WangTeterKernel {
public:
    WangTeterKernel(const double alpha, const double beta, const double rho0, const double lambdaTF, const double muVW, const double ft = (5.0/3.0));
    ~WangTeterKernel();

    void fillWTKernelReciprocal(cube* kernel, const cube* gNorms);

private:
    double _alpha;
    double _beta;
    double _rho0;
    double _ft;
    double _lambda;
    double _mu;
    double _coeff;
    double _tkF;
};

#ifdef LIBKEDF_OCL
#include "WangTeterOCL.hpp"
#endif

#endif	/* WANGTETER_HPP */


#ifndef USE_CUDA

#include <Spirit_Defines.h>
#include <engine/Hamiltonian_Heisenberg_Neighbours.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Neighbours.hpp>
#include <data/Spin_System.hpp>
#include <utility/Constants.hpp>

#include <Eigen/Dense>

using namespace Data;
using namespace Utility;
using Utility::Constants::mu_B;
using Utility::Constants::mu_0;
using Utility::Constants::Pi;
using Engine::Vectormath::check_atom_type;
using Engine::Vectormath::idx_from_pair;

namespace Engine
{
    Hamiltonian_Heisenberg_Neighbours::Hamiltonian_Heisenberg_Neighbours(
        scalarfield mu_s,
        scalar external_field_magnitude, Vector3 external_field_normal,
        intfield anisotropy_indices, scalarfield anisotropy_magnitudes, vectorfield anisotropy_normals,
        scalarfield exchange_magnitudes,
        scalarfield dmi_magnitudes, int dm_chirality,
        scalar ddi_radius,
        std::shared_ptr<Data::Geometry> geometry,
        intfield boundary_conditions
    ) :
        Hamiltonian(boundary_conditions),
        geometry(geometry),
        mu_s(mu_s),
        external_field_magnitude(external_field_magnitude * mu_B), external_field_normal(external_field_normal),
        anisotropy_indices(anisotropy_indices), anisotropy_magnitudes(anisotropy_magnitudes), anisotropy_normals(anisotropy_normals),
        exchange_magnitudes(exchange_magnitudes),
        dmi_magnitudes(dmi_magnitudes),
        ddi_radius(ddi_radius)
    {
        // Generate Exchange neighbours
        exchange_neighbours = Neighbours::Get_Neighbours_in_Shells(*geometry, exchange_magnitudes.size());

        // Generate DMI neighbours and normals
        dmi_neighbours = Neighbours::Get_Neighbours_in_Shells(*geometry, dmi_magnitudes.size());
        for (unsigned int ineigh = 0; ineigh < dmi_neighbours.size(); ++ineigh)
        {
            dmi_normals.push_back(Neighbours::DMI_Normal_from_Pair(*geometry, dmi_neighbours[ineigh], dm_chirality));
        }

        // Generate DDI neighbours, magnitudes and normals
        this->ddi_neighbours = Engine::Neighbours::Get_Neighbours_in_Radius(*this->geometry, ddi_radius);
        scalar magnitude;
        Vector3 normal;
        for (unsigned int i=0; i<ddi_neighbours.size(); ++i)
        {
            Engine::Neighbours::DDI_from_Pair(*this->geometry, ddi_neighbours[i], magnitude, normal);
            this->ddi_magnitudes.push_back(magnitude);
            this->ddi_normals.push_back(normal);
        }

        this->Update_Energy_Contributions();
    }

    void Hamiltonian_Heisenberg_Neighbours::Update_N_Neighbour_Shells(int n_shells_exchange, int n_shells_dmi)
    {
        if (this->exchange_magnitudes.size() != n_shells_exchange)
        {
            this->exchange_magnitudes = scalarfield(n_shells_exchange);
            // Re-calculate exchange neighbour list
        }
        if (this->dmi_magnitudes.size() != n_shells_dmi)
        {
            this->dmi_magnitudes = scalarfield(n_shells_dmi);
            // Re-calculate dmi neighbour list
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::Update_Energy_Contributions()
    {
        this->energy_contributions_per_spin = std::vector<std::pair<std::string, scalarfield>>(0);

        // External field
        if (this->external_field_magnitude > 0)
        {
            this->energy_contributions_per_spin.push_back({"Zeeman", scalarfield(0)});
            this->idx_zeeman = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_zeeman = -1;
        // Anisotropy
        if (this->anisotropy_indices.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"Anisotropy", scalarfield(0) });
            this->idx_anisotropy = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_anisotropy = -1;
        // Exchange
        if (this->exchange_neighbours.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"Exchange", scalarfield(0) });
            this->idx_exchange = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_exchange = -1;
        // DMI
        if (this->dmi_neighbours.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"DMI", scalarfield(0) });
            this->idx_dmi = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_dmi = -1;
        // Dipole-Dipole
        if (this->ddi_neighbours.size() > 0)
        {
            this->energy_contributions_per_spin.push_back({"DD", scalarfield(0) });
            this->idx_ddi = this->energy_contributions_per_spin.size()-1;
        }
        else this->idx_ddi = -1;
    }

    void Hamiltonian_Heisenberg_Neighbours::Energy_Contributions_per_Spin(const vectorfield & spins, std::vector<std::pair<std::string, scalarfield>> & contributions)
    {
        if (contributions.size() != this->energy_contributions_per_spin.size())
        {
            contributions = this->energy_contributions_per_spin;
        }

        int nos = spins.size();
        for (auto& pair : energy_contributions_per_spin)
        {
            // Allocate if not already allocated
            if (pair.second.size() != nos) pair.second = scalarfield(nos, 0);
            // Otherwise set to zero
            else for (auto& pair : energy_contributions_per_spin) Vectormath::fill(pair.second, 0);
        }

        // External field
        if (this->idx_zeeman >=0 )     E_Zeeman(spins, energy_contributions_per_spin[idx_zeeman].second);
        // Anisotropy
        if (this->idx_anisotropy >=0 ) E_Anisotropy(spins, energy_contributions_per_spin[idx_anisotropy].second);

        // Exchange
        if (this->idx_exchange >=0 )   E_Exchange(spins,energy_contributions_per_spin[idx_exchange].second);
        // DMI
        if (this->idx_dmi >=0 )        E_DMI(spins, energy_contributions_per_spin[idx_dmi].second);
        // DDI
        if (this->idx_ddi >=0 )        E_DDI(spins, energy_contributions_per_spin[idx_ddi].second);
    }

    void Hamiltonian_Heisenberg_Neighbours::E_Zeeman(const vectorfield & spins, scalarfield & Energy)
    {
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int ibasis = 0; ibasis < N; ++ibasis)
            {
                int ispin = icell*N + ibasis;
                if (check_atom_type(this->geometry->atom_types[ispin]))
                    Energy[ispin] -= this->mu_s[ibasis] * this->external_field_magnitude * this->external_field_normal.dot(spins[ispin]);
            }
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::E_Anisotropy(const vectorfield & spins, scalarfield & Energy)
    {
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int iani = 0; iani < anisotropy_indices.size(); ++iani)
            {
                int ispin = icell*N + anisotropy_indices[iani];
                if (check_atom_type(this->geometry->atom_types[ispin]))
                    Energy[ispin] -= this->anisotropy_magnitudes[iani] * std::pow(anisotropy_normals[iani].dot(spins[ispin]), 2.0);
            }
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::E_Exchange(const vectorfield & spins, scalarfield & Energy)
    {
        #pragma omp parallel for
        for (unsigned int ispin = 0; ispin < spins.size(); ++ispin)
        {
            // auto translations = Vectormath::translations_from_idx(geometry->n_cells, geometry->n_cell_atoms, ispin);
            for (unsigned int ineigh = 0; ineigh < exchange_neighbours.size(); ++ineigh)
            {
                int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, exchange_neighbours[ineigh]);
                if ( jspin >= 0 )
                {
                    auto& ishell = exchange_neighbours[ineigh].idx_shell;
                    Energy[ispin] -= 0.5 * exchange_magnitudes[ishell] * spins[ispin].dot(spins[jspin]);
                }
            }
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::E_DMI(const vectorfield & spins, scalarfield & Energy)
    {
        #pragma omp parallel for
        for (unsigned int ispin = 0; ispin < spins.size(); ++ispin)
        {
            for (unsigned int ineigh = 0; ineigh < dmi_neighbours.size(); ++ineigh)
            {
                int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, dmi_neighbours[ineigh]);
                if ( jspin >= 0 )
                {
                    auto& ishell = dmi_neighbours[ineigh].idx_shell;
                    Energy[ispin] -= 0.5 * dmi_magnitudes[ishell] * dmi_normals[ineigh].dot(spins[ispin].cross(spins[jspin]));
                }
            }
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::E_DDI(const vectorfield & spins, scalarfield & Energy)
    {
        // The translations are in angstr�m, so the |r|[m] becomes |r|[m]*10^-10
        const scalar mult = mu_0 * std::pow(mu_B, 2) / ( 4*Pi * 1e-30 );

        scalar result = 0.0;
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int ibasis = 0; ibasis < N; ++ibasis)
            {
                int ispin = icell * N + ibasis;
                for (unsigned int ineigh = 0; ineigh < ddi_neighbours.size(); ++ineigh)
                {
                    if (ddi_magnitudes[ineigh] > 0.0)
                    {
                        int jbasis = ddi_neighbours[ineigh].j;
                        int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, ddi_neighbours[ineigh]);
                        if (jspin >= 0)
                        {
                            Energy[ispin] -= 0.5 * this->mu_s[ibasis] * this->mu_s[jbasis] * mult / std::pow(ddi_magnitudes[ineigh], 3.0) *
                                (3 * spins[jspin].dot(ddi_normals[ineigh]) * spins[ispin].dot(ddi_normals[ineigh]) - spins[ispin].dot(spins[jspin]));
                            Energy[jspin] -= 0.5 * this->mu_s[ibasis] * this->mu_s[jbasis] * mult / std::pow(ddi_magnitudes[ineigh], 3.0) *
                                (3 * spins[jspin].dot(ddi_normals[ineigh]) * spins[ispin].dot(ddi_normals[ineigh]) - spins[ispin].dot(spins[jspin]));
                        }
                    }
                }
            }
        }
    }// end DipoleDipole

    void Hamiltonian_Heisenberg_Neighbours::Gradient(const vectorfield & spins, vectorfield & gradient)
    {
        // Set to zero
        Vectormath::fill(gradient, {0,0,0});

        // External field
        Gradient_Zeeman(gradient);

        // Anisotropy
        Gradient_Anisotropy(spins, gradient);

        // Exchange
        this->Gradient_Exchange(spins, gradient);
        // DMI
        this->Gradient_DMI(spins, gradient);
        // DD
        this->Gradient_DDI(spins, gradient);
    }

    void Hamiltonian_Heisenberg_Neighbours::Gradient_Zeeman(vectorfield & gradient)
    {
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int ibasis = 0; ibasis < N; ++ibasis)
            {
                int ispin = icell*N + ibasis;
                if (check_atom_type(this->geometry->atom_types[ispin]))
                    gradient[ispin] -= this->mu_s[ibasis] * this->external_field_magnitude * this->external_field_normal;
            }
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::Gradient_Anisotropy(const vectorfield & spins, vectorfield & gradient)
    {
        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int iani = 0; iani < anisotropy_indices.size(); ++iani)
            {
                int ispin = icell*N + anisotropy_indices[iani];
                if (check_atom_type(this->geometry->atom_types[ispin]))
                    gradient[ispin] -= 2.0 * this->anisotropy_magnitudes[iani] * this->anisotropy_normals[iani] * anisotropy_normals[iani].dot(spins[ispin]);
            }
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::Gradient_Exchange(const vectorfield & spins, vectorfield & gradient)
    {
        #pragma omp parallel for
        for (unsigned int ispin = 0; ispin < spins.size(); ++ispin)
        {
            for (unsigned int ineigh = 0; ineigh < exchange_neighbours.size(); ++ineigh)
            {
                int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, exchange_neighbours[ineigh]);
                if ( jspin >= 0 )
                {
                    auto& ishell = exchange_neighbours[ineigh].idx_shell;
                    gradient[ispin] -= exchange_magnitudes[ishell] * spins[jspin];
                }
            }
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::Gradient_DMI(const vectorfield & spins, vectorfield & gradient)
    {
        #pragma omp parallel for
        for (unsigned int ispin = 0; ispin < spins.size(); ++ispin)
        {
            auto translations = Vectormath::translations_from_idx(geometry->n_cells, geometry->n_cell_atoms, ispin);
            for (unsigned int ineigh = 0; ineigh < dmi_neighbours.size(); ++ineigh)
            {
                int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, dmi_neighbours[ineigh]);
                if ( jspin >= 0 )
                {
                    auto& ishell = dmi_neighbours[ineigh].idx_shell;
                    gradient[ispin] -= dmi_magnitudes[ishell] * spins[jspin].cross(dmi_normals[ineigh]);
                }
            }
        }
    }

    void Hamiltonian_Heisenberg_Neighbours::Gradient_DDI(const vectorfield & spins, vectorfield & gradient)
    {
        // The translations are in angstr�m, so the |r|[m] becomes |r|[m]*10^-10
        const scalar mult = mu_0 * std::pow(mu_B, 2) / ( 4*Pi * 1e-30 );

        const int N = geometry->n_cell_atoms;

        #pragma omp parallel for
        for (int icell = 0; icell < geometry->n_cells_total; ++icell)
        {
            for (int ibasis = 0; ibasis < N; ++ibasis)
            {
                int ispin = icell * N + ibasis;
                for (unsigned int ineigh = 0; ineigh < ddi_neighbours.size(); ++ineigh)
                {
                    if (ddi_magnitudes[ineigh] > 0.0)
                    {
                        int jbasis = ddi_neighbours[ineigh].j;
                        int jspin = idx_from_pair(ispin, boundary_conditions, geometry->n_cells, geometry->n_cell_atoms, geometry->atom_types, ddi_neighbours[ineigh]);
                        if (jspin >= 0)
                        {
                            scalar skalar_contrib = mult / std::pow(ddi_magnitudes[ineigh], 3.0);
                            gradient[ispin] -= this->mu_s[jbasis] * skalar_contrib * (3 * ddi_normals[ineigh] * spins[jspin].dot(ddi_normals[ineigh]) - spins[jspin]);
                            gradient[jspin] -= this->mu_s[ibasis] * skalar_contrib * (3 * ddi_normals[ineigh] * spins[ispin].dot(ddi_normals[ineigh]) - spins[ispin]);
                        }
                    }
                }
            }
        }
    }//end Field_DipoleDipole

    void Hamiltonian_Heisenberg_Neighbours::Hessian(const vectorfield & spins, MatrixX & hessian)
    {
        int nos = spins.size();

        // Set to zero
        // for (auto& h : hessian) h = 0;
        hessian.setZero();

        // Single Spin elements
        for (int alpha = 0; alpha < 3; ++alpha)
        {
            for (unsigned int i = 0; i < anisotropy_indices.size(); ++i)
            {
                int idx = anisotropy_indices[i];
                // scalar x = -2.0*this->anisotropy_magnitudes[i] * std::pow(this->anisotropy_normals[i][alpha], 2);
                hessian(3*idx + alpha, 3*idx + alpha) += -2.0*this->anisotropy_magnitudes[i]*std::pow(this->anisotropy_normals[i][alpha],2);
            }
        }

        // std::cerr << "calculated hessian" << std::endl;

        // Spin Pair elements
        // Exchange
        for (unsigned int ispin = 0; ispin < spins.size(); ++ispin)
        {
            auto translations = Vectormath::translations_from_idx(geometry->n_cells, geometry->n_cell_atoms, ispin);
            for (unsigned int ineigh = 0; ineigh < this->exchange_neighbours.size(); ++ineigh)
            {
                for (int alpha = 0; alpha < 3; ++alpha)
                {
                    //int idx_i = 3 * exchange_neighbours[i_pair][0] + alpha;
                    //int idx_j = 3 * exchange_neighbours[i_pair][1] + alpha;
                    int jspin = Vectormath::idx_from_translations(geometry->n_cells, geometry->n_cell_atoms, translations, exchange_neighbours[ineigh].translations);
                    int ishell = exchange_neighbours[ineigh].idx_shell;
                    hessian(ispin, jspin) += -exchange_magnitudes[ineigh];
                    hessian(jspin, ispin) += -exchange_magnitudes[ineigh];
                }
            }
        }
        // DMI
        for (unsigned int ispin = 0; ispin < spins.size(); ++ispin)
        {
            auto translations = Vectormath::translations_from_idx(geometry->n_cells, geometry->n_cell_atoms, ispin);
            for (unsigned int ineigh = 0; ineigh < this->dmi_neighbours.size(); ++ineigh)
            {
                for (int alpha = 0; alpha < 3; ++alpha)
                {
                    for (int beta = 0; beta < 3; ++beta)
                    {
                        int idx_i = 3 * dmi_neighbours[ineigh].i + alpha;
                        int idx_j = 3 * dmi_neighbours[ineigh].j + beta;
                        if ((alpha == 0 && beta == 1))
                        {
                            hessian(idx_i, idx_j) +=
                                -dmi_magnitudes[ineigh] * dmi_normals[ineigh][2];
                            hessian(idx_j, idx_i) +=
                                -dmi_magnitudes[ineigh] * dmi_normals[ineigh][2];
                        }
                        else if ((alpha == 1 && beta == 0))
                        {
                            hessian(idx_i, idx_j) +=
                                dmi_magnitudes[ineigh] * dmi_normals[ineigh][2];
                            hessian(idx_j, idx_i) +=
                                dmi_magnitudes[ineigh] * dmi_normals[ineigh][2];
                        }
                        else if ((alpha == 0 && beta == 2))
                        {
                            hessian(idx_i, idx_j) +=
                                dmi_magnitudes[ineigh] * dmi_normals[ineigh][1];
                            hessian(idx_j, idx_i) +=
                                dmi_magnitudes[ineigh] * dmi_normals[ineigh][1];
                        }
                        else if ((alpha == 2 && beta == 0))
                        {
                            hessian(idx_i, idx_j) +=
                                -dmi_magnitudes[ineigh] * dmi_normals[ineigh][1];
                            hessian(idx_j, idx_i) +=
                                -dmi_magnitudes[ineigh] * dmi_normals[ineigh][1];
                        }
                        else if ((alpha == 1 && beta == 2))
                        {
                            hessian(idx_i, idx_j) +=
                                -dmi_magnitudes[ineigh] * dmi_normals[ineigh][0];
                            hessian(idx_j, idx_i) +=
                                -dmi_magnitudes[ineigh] * dmi_normals[ineigh][0];
                        }
                        else if ((alpha == 2 && beta == 1))
                        {
                            hessian(idx_i, idx_j) +=
                                dmi_magnitudes[ineigh] * dmi_normals[ineigh][0];
                            hessian(idx_j, idx_i) +=
                                dmi_magnitudes[ineigh] * dmi_normals[ineigh][0];
                        }
                    }
                }
            }
        }
        //// Dipole-Dipole
        //for (unsigned int i_pair = 0; i_pair < this->ddi_neighbours[i_periodicity].size(); ++i_pair)
        //{
        //	// indices
        //	int idx_1 = ddi_neighbours[i_periodicity][i_pair][0];
        //	int idx_2 = ddi_neighbours[i_periodicity][i_pair][1];
        //	// prefactor
        //	scalar prefactor = 0.0536814951168
        //		* this->mu_s[idx_1] * this->mu_s[idx_2]
        //		/ std::pow(ddi_magnitude[i_periodicity][i_pair], 3);
        //	// components
        //	for (int alpha = 0; alpha < 3; ++alpha)
        //	{
        //		for (int beta = 0; beta < 3; ++beta)
        //		{
        //			int idx_h = idx_1 + alpha*nos + 3 * nos*(idx_2 + beta*nos);
        //			if (alpha == beta)
        //				hessian[idx_h] += prefactor;
        //			hessian[idx_h] += -3.0*prefactor*DD_normal[i_periodicity][i_pair][alpha] * DD_normal[i_periodicity][i_pair][beta];
        //		}
        //	}
        //}
    }

    // Hamiltonian name as string
    static const std::string name = "Heisenberg (Neighbours)";
    const std::string& Hamiltonian_Heisenberg_Neighbours::Name() { return name; }
}

#endif


#include "resource.hpp"

#include "instance.hpp"
#include "traits.hpp"

#include <algorithm>
#include <boost/container/small_vector.hpp>
#include <cassert> // for assert
#include <cmath>   // for pow
#include <iterator>
#include <memory>
#include <pstl/glue_algorithm_defs.h>
#include <stdexcept>

// TODO factor polynomial stuff out into util
double
apply_polynomial(const polynomial & poly, double x)
{
	double sum = 0;
	for (auto term : poly) {
		sum += std::get<0>(term) * std::pow(x, std::get<1>(term));
	}
	return sum;
}

polynomial
add_poly(const polynomial & lhs, const polynomial & rhs)
{
	polynomial result(lhs.begin(), lhs.end());
	result.insert(result.end(), rhs.begin(), rhs.end());
	std::sort(result.begin(), result.end(),
	          [](const poly_term & sort_lhs, const poly_term & sort_rhs) {
		          return sort_lhs.second < sort_rhs.second;
	          });

	size_t len = result.size();
	for (size_t i = 0; (len > 0) && (i < (len - 1)); ++i) {
		// TODO float-equal comparison.
		if (double_eq(result[i].second, result[i + 1].second)) {
			// Equal exponents. Combine, mark for swap-and-delete
			result[i].first += result[i + 1].first;
			result[i + 1].first = 0;
			i++;
		}
	}

	auto new_end =
	    std::remove_if(result.begin(), result.end(), [](const poly_term & t) {
		    return double_eq(t.first, 0.0);
	    });
	result.erase(new_end, result.end());

	return result;
}
// TODO test add_poly

Resource::Resource(unsigned int id)
    : rid(id), availability(0), overshoot_costs({})
{}

Resource
Resource::clone() const
{
	Resource cloned(this->rid);
	cloned.set_availability(Availability(this->availability));
	cloned.set_investment_costs(this->investment_costs);
	cloned.set_overshoot_costs(FlexCost(this->overshoot_costs));

	return cloned;
}

void
Resource::set_availability(Availability && availability_in)
{
	this->availability = std::move(availability_in);
}

const Availability &
Resource::get_availability() const
{
	return this->availability;
}

unsigned int
Resource::get_rid()
{
	return this->rid;
}

void
Resource::set_id(unsigned int id)
{
	this->rid = id;
}

void
Resource::set_investment_costs(polynomial costs)
{
	this->investment_costs = costs;
}

void
Resource::set_overshoot_costs(FlexCost && costs)
{
	this->overshoot_costs = std::move(costs);
}

const polynomial &
Resource::get_investment_costs() const
{
	return this->investment_costs;
}

const polynomial &
Resource::get_overshoot_costs() const
{
	return this->overshoot_costs.get_base();
}

const FlexCost &
Resource::get_flex_overshoot() const
{
	return this->overshoot_costs;
}

const polynomial &
Resource::get_overshoot_costs(unsigned int pos) const
{
	return this->overshoot_costs.get_at(pos);
}

bool
Resource::is_overshoot_flat() const
{
	return this->overshoot_costs.is_flat();
}

FlexCost::FlexCost(polynomial base_in) : base(std::move(base_in)) {}

bool
FlexCost::is_flat() const
{
	return this->points.empty();
}

const polynomial &
FlexCost::get_base() const noexcept
{
	return this->base;
}

void
FlexCost::set_flexible(
    std::vector<std::pair<unsigned int, polynomial>> && new_points)
{
	if (this->base.empty()) {
		this->points = std::move(new_points);
	} else {
		this->points.reserve(new_points.size());
		this->points.clear();

		std::transform(new_points.begin(), new_points.end(),
		               std::back_inserter(this->points),
		               [&](const std::pair<unsigned int, polynomial> point) {
			               return std::pair<unsigned int, polynomial>(
			                   point.first, add_poly(this->base, point.second));
		               });
	}
}

const polynomial &
FlexCost::get_at(unsigned int pos) const noexcept
{
	if (this->points.empty()) {
		return this->base;
	} else {

		/* This comparator inverses comparison s.t. we can just use
		 * lower_bound on the reversed points to find the correct point. */
		struct Comp
		{
			bool
			operator()(const std::pair<unsigned int, polynomial> & lhs,
			           unsigned int rhs) const noexcept
			{
				return lhs.first > rhs;
			}
			bool
			operator()(unsigned int lhs,
			           const std::pair<unsigned int, polynomial> & rhs) const noexcept
			{
				return lhs > rhs.first;
			}
		};

		auto it = std::lower_bound(this->points.rbegin(), this->points.rend(), pos,
		                           Comp{});
		if (it == this->points.rend()) {
			return this->base;
		} else {
			return it->second;
		}
	}
}
// TODO test get_at

std::vector<std::pair<unsigned int, polynomial>>::const_iterator
FlexCost::begin() const
{
	return this->points.begin();
}
std::vector<std::pair<unsigned int, polynomial>>::const_iterator
FlexCost::end() const
{
	return this->points.end();
}

Availability::Availability(double start_amount) : points(1, {0, start_amount})
{}

double
Availability::get_at(unsigned int pos) const noexcept
{
	if (this->points.empty()) {
		return 0;
	} else {

		/* This comparator inverses comparison s.t. we can just use
		 * lower_bound on the reversed points to find the correct point. */
		struct Comp
		{
			bool
			operator()(const std::pair<unsigned int, double> & lhs,
			           unsigned int rhs) const noexcept
			{
				return lhs.first > rhs;
			}
			bool
			operator()(unsigned int lhs,
			           const std::pair<unsigned int, double> & rhs) const noexcept
			{
				return lhs > rhs.first;
			}
		};

		auto it = std::lower_bound(this->points.rbegin(), this->points.rend(), pos,
		                           Comp{});
		if (it == this->points.rend()) {
			return 0.0;
		} else {
			return it->second;
		}
	}
}

void
Availability::set(std::vector<std::pair<unsigned int, double>> && new_points)
{
	this->points = std::move(new_points);
}

std::vector<std::pair<unsigned int, double>>::const_iterator
Availability::begin() const
{
	return this->points.begin();
}

std::vector<std::pair<unsigned int, double>>::const_iterator
Availability::end() const
{
	return this->points.end();
}

double
Availability::get_flat_available() const
{
	assert(this->points.size() == 1);
	return this->points[0].second;
}

Resources::Resources() {}

Resources::Resources(double _usage) : usage{_usage} {}

Resources::Resources(const Instance * in, const ResVec & u)
    : instance(in), usage(u)
{
	if (in == NULL && u.size() > 1) {
		throw std::invalid_argument("Instance in is NULL");
	} else if (u.size() > 1 &&
	           !in->get_traits().has_flag(Traits::FLAT_AVAILABILITY)) {
		throw TraitUnfulfilledError("FLAT_AVAILABILITY required!");
	}
}

Resources::Resources(const Instance * in, const std::vector<double> & u)
    : instance(in), usage(u.begin(), u.end())
{
	if (in == NULL && u.size() > 1) {
		throw std::invalid_argument("Instance in is NULL");
	} else if (u.size() > 1 &&
	           !in->get_traits().has_flag(Traits::FLAT_AVAILABILITY)) {
		throw TraitUnfulfilledError("FLAT_AVAILABILITY required!");
	}
}

Resources::Resources(const Instance * in, ResVec && u)
    : instance(in), usage(std::move(u))
{
	if (in == NULL && u.size() > 1) {
		throw std::invalid_argument("Instance in is NULL");
	} else if (u.size() > 1 &&
	           !in->get_traits().has_flag(Traits::FLAT_AVAILABILITY)) {
		throw TraitUnfulfilledError("FLAT_AVAILABILITY required!");
	}
}

Resources::Resources(const Instance * in)
    : instance(in), usage(in->resource_count(), 0.0)
{}

const ResVec &
Resources::getUsage() const
{
	return usage;
}

ResVec &
Resources::getUsage()
{
	return usage;
}

Resources
Resources::operator+(const Resources & other) const
{
	Resources res(*this);
	res += other;
	return res;
}

Resources
Resources::operator-(const Resources & other) const
{
	Resources res(*this);
	res -= other;
	return res;
}

Resources
Resources::operator*(const Resources & other) const
{
	Resources res(*this);
	res *= other;
	return res;
}

Resources
Resources::operator/(const Resources & other) const
{
	Resources res(*this);
	res /= other;
	return res;
}

void
Resources::operator+=(const Resources & other)
{
	if (instance == NULL) {
		instance = other.instance;
	}
	if (usage.size() < other.usage.size()) {
		usage.resize(other.usage.size());
	}
	for (size_t i = 0; i < other.usage.size(); i++) {
		usage[i] += other.usage[i];
	}
}

void
Resources::operator*=(const Resources & other)
{
	if (instance == NULL) {
		instance = other.instance;
	}
	if (usage.size() < other.usage.size()) {
		usage.resize(other.usage.size());
	}
	for (size_t i = 0; i < other.usage.size(); i++) {
		usage[i] *= other.usage[i];
	}
}

void
Resources::operator/=(const Resources & other)
{
	if (instance == NULL) {
		instance = other.instance;
	}
	if (usage.size() < other.usage.size()) {
		usage.resize(other.usage.size());
	}
	for (size_t i = 0; i < other.usage.size(); i++) {
		usage[i] /= other.usage[i];
	}
}

void
Resources::operator-=(const Resources & other)
{
	if (instance == NULL) {
		instance = other.instance;
	}
	if (usage.size() < other.usage.size()) {
		usage.resize(other.usage.size());
	}
	for (size_t i = 0; i < other.usage.size(); i++) {
		usage[i] -= other.usage[i];
	}
}

bool
Resources::operator<(const Resources & other) const
{
	return this->getCosts() < other.getCosts();
}

bool
Resources::operator>(const Resources & other) const
{
	return this->getCosts() > other.getCosts();
}

bool
Resources::operator<=(const Resources & other) const
{
	return this->getCosts() <= other.getCosts();
}

bool
Resources::operator>=(const Resources & other) const
{
	return this->getCosts() >= other.getCosts();
}

bool
Resources::operator!=(const Resources & other) const
{
	return this->usage != other.usage;
}

bool
Resources::operator==(const Resources & other) const
{
	return this->usage == other.usage;
}

double
Resources::getCosts() const
{
	if (usage.size() == 0) {
		return 0;
	} else if (usage.size() == 1) {
		return usage[0];
	} else if (!cached) {
		cache = instance->calculate_costs(usage);
		cached = true;
	}
	return cache;
}


#include <iostream>
#include <numeric>
#include <chrono>

#include <docopt/docopt.h>
#include <SFML/Graphics.hpp>

#include <boost/range.hpp>
#include <boost/iterator/distance.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/min_element.hpp>

#include "utils.hpp"

class Compressor {
public:
    Compressor(const std::string_view path) :
        m_source    (load<sf::Image>(path)),
        m_size_x    (m_source.getSize().x),
        m_size_y    (m_source.getSize().y),
        m_output    (m_source)
    { }

    auto compress(int level) -> const sf::Image &
    {
        using namespace std::chrono;
        std::cout << "Starting compression with " << level << " colors" << "\n";
        const auto start{ system_clock::now() };

        last_level = level;
        Cluster::app = this;

        // create as much cluster as number of colors
        std::vector<Cluster> clusters(level);
        std::generate(std::begin(clusters), std::end(clusters), Cluster::init);

        // do the k-mean iteration until we done
        while (!kMeanIteration(clusters));

        // update the pixels in the ouput image
        for (auto &[ centroid, pixels, _ ] : clusters) for (const auto &i : pixels)
            m_output.setPixel(i % m_size_x, i / m_size_x, change(centroid));

        const auto delay{ duration_cast<milliseconds>(system_clock::now() - start).count() };
        std::cout << "Done in " << static_cast<float>(delay) / 1000.0f << " seconds" << "\n";

        return m_output;
    }

    auto save(const std::string &path) const noexcept -> bool
    {
        return m_output.saveToFile(path + ".compressed_" + std::to_string(last_level) + ".png");
    }

private:

    static constexpr auto CONVERGENCE_LIMIT = 1.0f;

    struct Cluster {
        px_color centroid;
        std::vector<px_index> indices;
        bool done = false;

        auto update(const sf::Image &img) -> void
        {
            const auto newColor = normalize(
                std::accumulate(std::begin(indices), std::end(indices), px_color{}, transform_accumulator_t{ img }),
                static_cast<float>(indices.size() ?: 1));
            done = distance(centroid, newColor) < CONVERGENCE_LIMIT;
            centroid = newColor;
        }

        static Compressor *app;

        static
        auto init() noexcept -> Cluster { return {
            change(app->m_source.getPixel(std::rand() % app->m_size_x, std::rand() % app->m_size_y)),
            std::vector<px_index>( app->m_size_x * app->m_size_y / static_cast<float>(app->last_level) )
        }; }
    };

    const sf::Image m_source;

    const int m_size_x;
    const int m_size_y;

    int last_level;
    sf::Image m_output;

    // return the index of the clusters with the closest color to @color
    auto getClosestCluster(const std::vector<Cluster> &clusters, const px_color &color) noexcept -> int
    {
        return boost::distance(boost::begin(clusters), boost::range::min_element(clusters |
            boost::adaptors::transformed([&color](const auto &i) -> float {
                return distance(i.centroid, color); })).base());
    }

    auto kMeanIteration(std::vector<Cluster> &clusters) noexcept -> bool
    {
        // remove the previously assigned pixels of the cluster
        for (auto &[ _, index, __ ] : clusters) index.clear();

        // assign each pixels to it closest cluster
        for (int index = 0; index != m_size_x * m_size_y; index++)
            clusters[getClosestCluster(clusters,
                change(m_source.getPixelsPtr() + 4 * index))].indices.emplace_back(index);

        // update the centroid of the cluster
        for (auto &i : clusters) i.update(m_source);

        return std::any_of(std::begin(clusters), std::end(clusters),
            [](auto &i) { return !i.done; });
    }

};

Compressor *Compressor::Cluster::app = nullptr;

constexpr auto USAGE =
R"(Usage: app [--file=<path>] [--level=<level>] [-i]

Options:
  file      path of the source target
  level     number of color in the ouput image
  i         launch in interactive mode

Interactive Mode:
  mouse left button     increment the colors count
  mouse right button    decrement the colors count
  any key               save the picture
)";

constexpr auto VERSION = "ImgCompressor x.y.z";

#define LEVEL_MAX 10000

template<typename T>
auto normalize_level(T level) ->
    std::enable_if_t<std::is_arithmetic_v<std::decay_t<T>>, T>
{
    if (level <= 0) level = 1;
    else if (level >= LEVEL_MAX) level = LEVEL_MAX - 1;
    return level;
}

#define WINDOW_SIZE_W 1080
#define WINDOW_SIZE_H 720

auto main(int ac, char **av) -> int try
{
    std::srand(std::time(nullptr));

    const auto args = docopt::docopt(USAGE, { av + 1, av + ac }, true, VERSION);

    const auto path = args.at("--file") ? args.at("--file").asString() : "default.png";
    auto level = args.at("--level") ? normalize_level(args.at("--level").asLong()) : 10;

    Compressor compressor(path);
    auto &img = compressor.compress(level);

    if (!args.at("-i").asBool())
        return !compressor.save(path);

    sf::RenderWindow w{ { WINDOW_SIZE_W, WINDOW_SIZE_H }, VERSION };

    sf::Texture texture; texture.loadFromImage(img);
    const auto shape = [](const sf::Texture &tx) {
        sf::RectangleShape shape({ WINDOW_SIZE_W, WINDOW_SIZE_H }); shape.setTexture(&tx); return shape;
    } (texture);

    while (w.isOpen()) {
        sf::Event e;
        while (w.pollEvent(e)) {
            if (e.type == sf::Event::Closed) w.close();
            else if (e.type == sf::Event::MouseButtonPressed) {
                if (e.mouseButton.button == sf::Mouse::Button::Left) level++;
                else if (e.mouseButton.button == sf::Mouse::Button::Right) level--;
                texture.loadFromImage(compressor.compress(normalize_level<long &>(level)));
            }
            else if (e.type == sf::Event::KeyPressed) compressor.save(path);
        }

        w.clear();
        w.draw(shape);
        w.display();

    }
    return 0;
}
catch (const std::exception &e)
{
    std::cerr << e.what() << '\n';
    return 1;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_MULS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_MULS_HPP_INCLUDED

//#include <boost/simd/arithmetic/functions/muls.hpp>
//#include <boost/simd/include/functions/simd/is_eqz.hpp>
//#include <boost/simd/include/functions/simd/is_ltz.hpp>
//#include <boost/simd/include/functions/simd/abs.hpp>
//#include <boost/simd/include/functions/simd/if_else.hpp>
//#include <boost/simd/include/functions/simd/divs.hpp>
//#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
//TODO...a common version or delete the file

// /////////////////////////////////////////////////////////////////////////////
// // Implementation when type A0 is int 32 8 64
// /////////////////////////////////////////////////////////////////////////////
// BOOST_SIMD_REGISTER_DISPATCH(boost::simd::tag::muls_, tag::cpu_,
//                        (A0)(X),
//                        ((simd_<integer_<A0>,X>))((simd_<integer_<A0>,X>))
//                       );
// namespace boost { namespace simd { namespace ext
// {
//   template<class X, class Dummy>
//   struct call<boost::simd::tag::muls_(tag::simd_<tag::integer_, X> ,
//                       boost::simd::tag::simd_<tag::integer_, X>),
//               boost::simd::tag::cpu_, Dummy> : callable
//   {
//     template<class Sig> struct result;
//     template<class This,class A0>
//     struct result<This(A0, A0)> : meta::strip<A0>{};
//     BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
//     {
//       typedef typename dispatch::meta::as_integer<A0, unsigned>::type untype;
//       A0 sign = b_xor(is_ltz(a0), is_ltz(a1));
//       untype aa0 = simd::bitwise_cast<untype>(boost::simd::abs(a0));
//       untype aa1 = simd::bitwise_cast<untype>(boost::simd::abs(a1));
//       untype ga =  boost::simd::max(aa0, aa1);
//       untype la =  boost::simd::min(aa0, aa1);
//       return select( lt(rdivide(simd::bitwise_cast<untype>(Valmax<A0>()), ga), la),
//                 select(sign, Valmin<A0>(), Valmax<A0>()),
//                 a0*a1
//                );
//     }
//   };
// } } }
// /////////////////////////////////////////////////////////////////////////////
// // Implementation when type A0 is uint 32 8 64
// /////////////////////////////////////////////////////////////////////////////
// BOOST_SIMD_REGISTER_DISPATCH(boost::simd::tag::muls_, tag::cpu_,
//                        (A0)(X),
//                        ((simd_<unsigned_<A0>,X>))((simd_<unsigned_<A0>,X>))
//                       );
// namespace boost { namespace simd { namespace ext
// {
//   template<class X, class Dummy>
//   struct call<boost::simd::tag::muls_(tag::simd_<tag::unsigned_, X> ,
//                       boost::simd::tag::simd_<tag::unsigned_, X>),
//               boost::simd::tag::cpu_, Dummy> : callable
//   {
//     template<class Sig> struct result;
//     template<class This,class A0>
//     struct result<This(A0, A0)> : meta::strip<A0>{};
//     BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
//     {
//       return select( lt(rdivide(Valmax<A0>(), a0), a1),
//                 Valmax<A0>(),
//                 a0*a1
//                );
//     }
//   };
// } } }
#endif


//
// Add --log_level=message to see the messages!
//
#define BOOST_TEST_MODULE "ConvolutionalNetwork Tests"
#include <boost/test/unit_test.hpp>

#include <sstream>

#include "core/types.h"
#include "core/functions.h"
#include "core/utils.h"
#include "core/random.h"
#include "core/updaters.h"
#include "core/training.h"
#include "layers/smaxlayer.h"

#include "networks/cnn.h"

#include "test_utils.h"
#include "timer.h"
#include "mnist_test.h"

using namespace std;
using namespace boost;
using namespace boost::unit_test;
using namespace yann;
using namespace yann::test;

#define MNIST_TEST_FOLDER   "../data/mnist/"

struct CnnTestFixture
{
  MnistTest _mnist_test;

  CnnTestFixture()
  {
    BOOST_TEST_MESSAGE("*** Reading MNIST dataset ...");
    _mnist_test.read(MNIST_TEST_FOLDER);
    BOOST_TEST_MESSAGE(_mnist_test);
  }
  ~CnnTestFixture()
  {
  }

  // Creates CNN with one Conv+Poll layer, FC layer and Softmax layer
  unique_ptr<Network> create_cnn_for_mnist(
      const MatrixSize & conv_filter_size,
      const MatrixSize & polling_size,
      const MatrixSize & frames_num,
      const unique_ptr<CostFunction> & cost)
  {
    // create params
    ConvolutionalNetwork::ConvPollParams params;
    params._output_frames_num = frames_num;
    params._conv_filter_size = conv_filter_size;
    params._conv_activation_funtion = make_unique<SigmoidFunction>();
    params._polling_mode = PollingLayer::PollMode_Max;
    params._polling_filter_size = polling_size;

    // create conv layers
    auto layers = ConvolutionalNetwork::create(
        _mnist_test.get_image_rows(),
        _mnist_test.get_image_cols(),
        params,
        make_unique<IdentityFunction>(),
        _mnist_test.get_label_size());
    YANN_CHECK(layers);

    // add softmaxLayer
    auto smax_layer = make_unique<SoftmaxLayer>(
        layers->get_output_size()
    );
    YANN_CHECK(smax_layer);
    layers->append_layer(std::move(smax_layer));

    // networks
    auto cnn = make_unique<Network>(std::move(layers));
    YANN_CHECK(cnn);
    cnn->set_cost_function(cost);

    // done
    return cnn;
  }
}; // struct CnnTestFixture

BOOST_FIXTURE_TEST_SUITE(CnnTest, CnnTestFixture);

BOOST_AUTO_TEST_CASE(IO_Test)
{
  BOOST_TEST_MESSAGE("*** ConvolutionalNetwork IO test ...");

  const MatrixSize image_size = 32;
  const MatrixSize output_size = 10;

  // create params
  ConvolutionalNetwork::ConvPollParams params;
  params._output_frames_num = 3;
  params._conv_filter_size = 2;
  params._conv_activation_funtion = make_unique<SigmoidFunction>();
  params._polling_mode = PollingLayer::PollMode_Max;
  params._polling_filter_size = 2;

  // create, init and write the network
  auto one_layers = ConvolutionalNetwork::create(
      image_size, // input rows,
      image_size, // input cols
      params,
      make_unique<SigmoidFunction>(), // FC layer activation func
      output_size      // output size
  );
  auto one = make_unique<Network>(std::move(one_layers));
  YANN_CHECK(one);
  one->init(Layer::InitMode_Random);

  // BOOST_TEST_MESSAGE("ConvolutionalNetwork before writing to file: " << (*one));
  ostringstream oss;
  oss << (*one);
  BOOST_CHECK(!oss.fail());

  // read network
  auto two_layers = ConvolutionalNetwork::create(
      image_size, // input rows,
      image_size, // input cols
      params,
      make_unique<SigmoidFunction>(), // FC layer activation func
      output_size      // output size
  );
  auto two = make_unique<Network>(std::move(two_layers));
  YANN_CHECK(two);
  std::istringstream iss(oss.str());
  iss >> (*two);
  BOOST_CHECK(!iss.fail());
  // BOOST_TEST_MESSAGE("ConvolutionalNetwork after loading from file: " << (*two));

  // compare
  BOOST_CHECK(one->is_equal(*two, TEST_TOLERANCE));
}

BOOST_AUTO_TEST_CASE(Training_GradientDescent_Test)
{
  BOOST_TEST_MESSAGE("*** ConvolutionalNetwork training test...");

  const size_t epochs = 300;
  const MatrixSize image_size = 2;
  const MatrixSize filter_size = 2;
  const MatrixSize polling_size = 1;
  const MatrixSize input_size = image_size * image_size;
  const MatrixSize output_size = 2;
  const MatrixSize training_batch_size = 4;
  const MatrixSize testing_batch_size = 2;
  const MatrixSize tests_per_batch = 1;
  VectorBatch training_inputs, training_outputs;
  VectorBatch testing_inputs, testing_outputs;

  resize_batch(training_inputs,  training_batch_size, input_size);
  resize_batch(training_outputs, training_batch_size, output_size);
  resize_batch(testing_inputs,   testing_batch_size,  input_size);
  resize_batch(testing_outputs,  testing_batch_size,  output_size);

  training_inputs  << 0, 0, 0, 1,
                      0, 0, 1, 0,
                      0, 1, 0, 0,
                      1, 0, 0, 0;
  training_outputs << 0, 1,
                      0, 1,
                      1, 0,
                      1, 0;

  testing_inputs   << 0, 0, 1, 1,
                      1, 1, 0, 0;
  testing_outputs  << 0, 1,
                      1, 0;

  ConvolutionalNetwork::ConvPollParams params;
  params._output_frames_num = 1;
  params._conv_filter_size = filter_size;
  params._conv_activation_funtion = make_unique<SigmoidFunction>();
  params._polling_mode = PollingLayer::PollMode_Max;
  params._polling_filter_size = polling_size;
  auto layers = ConvolutionalNetwork::create(
      image_size, // input rows
      image_size, // input cols
      params,
      make_unique<SigmoidFunction>(), // FC layer activation
      output_size
  );
  YANN_CHECK(layers);

  auto cnn = make_unique<Network>(std::move(layers));
  YANN_CHECK(cnn);
  cnn->set_cost_function(make_unique<QuadraticCost>());
  {
    Timer timer("Initializing ConvolutionalNetwork");
    cnn->init(Layer::InitMode_Random, Layer::InitContext(12345));
    BOOST_TEST_MESSAGE(timer);
  }

  // training
  Trainer trainer(make_unique<Updater_GradientDescent>(1.0, 0.0));
  DataSource_Stochastic data_source(
      training_inputs,
      training_outputs,
      DataSource_Stochastic::Sequential,
      tests_per_batch);
  BOOST_TEST_MESSAGE("trainer: " << trainer.get_info());
  {
    Timer timer("Training");
    trainer.train(*cnn, data_source, tests_per_batch, epochs);

    BOOST_TEST_MESSAGE(timer);
  }
  // BOOST_TEST_MESSAGE("After training: " << (*cnn));

  // testing
  VectorBatch res;
  for (MatrixSize ii = 0; ii < testing_batch_size; ii++) {
    VectorBatch in = get_batch(testing_inputs, ii); // copy
    VectorBatch out = get_batch(testing_outputs, ii); // copy
    res.resizeLike(out);

    cnn->calculate(in, res);

    Value cost = cnn->cost(res, out);
    BOOST_TEST_MESSAGE("test " << ii << " input: " << in);
    BOOST_TEST_MESSAGE("test " << ii << " expected: " << out);
    BOOST_TEST_MESSAGE("test " << ii << " actual: " << res);
    BOOST_TEST_MESSAGE("test " << ii << " cost/loss: " << cost);

    BOOST_CHECK_LE(cost, 0.06);
  }
}

BOOST_AUTO_TEST_CASE(Mnist_OneLayer_Two_Labels_Test)
{
  const size_t epochs = 3;

  // reduce the test size to two labels to make it faster
  BOOST_TEST_MESSAGE("*** Filtering test set...");
  _mnist_test.filter(1, 1000, 1000); // only allow 0,1 images; 1000 count
  BOOST_TEST_MESSAGE("*** Filtered test set: " << "\n" << _mnist_test);

  // setup
  auto cnn = create_cnn_for_mnist(
        10, // filter_size
        4, // polling_size,
        2, // frames_num,
        make_unique<QuadraticCost>());
  BOOST_CHECK(cnn);
  cnn->init(Layer::InitMode_Random, Layer::InitContext(12345));

  // create trainer
  auto trainer = make_unique<Trainer>(
      make_unique<Updater_GradientDescent>(1.0, 0.0));
  BOOST_CHECK(trainer);
  trainer->set_batch_progress_callback(batch_progress_callback);

  // print info
  BOOST_TEST_MESSAGE("*** Training against MNIST dataset with ");
  BOOST_TEST_MESSAGE(" ConvolutionalNetwork: " << cnn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // test
  pair<double, Value> res = _mnist_test.train_and_test(
      *cnn,
      *trainer,
      DataSource_Stochastic::Sequential, // want consistency for this test
      10,                  // training_batch_size
      epochs,
      100                  // testing_batch_size
  );
  BOOST_TEST_MESSAGE("*** Success rate: " << (res.first * 100) << "% Loss: " << res.second << " after " << epochs << " epochs");
  BOOST_TEST_MESSAGE(" ConvolutionalNetwork: " << cnn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // check
  BOOST_CHECK_GE(res.first, 0.995); // > 995%%
  BOOST_CHECK_LE(res.second, 0.006); // < 0.006 per test
}

BOOST_AUTO_TEST_CASE(Mnist_OneLayer_Full_Test, * disabled())
{
  const size_t epochs = 30;

  // setup
  auto cnn = create_cnn_for_mnist(
        5, // filter_size
        2, // polling_size,
        3, // frames_num,
        make_unique<QuadraticCost>());
  BOOST_CHECK(cnn);
  cnn->init(Layer::InitMode_Random);

  // trainer
  auto trainer = make_unique<Trainer>(
      make_unique<Updater_GradientDescent>(5.0, 0.0));
  BOOST_CHECK(trainer);
  trainer->set_batch_progress_callback(batch_progress_callback);

  // print info
  BOOST_TEST_MESSAGE("*** Training against MNIST dataset with ");
  BOOST_TEST_MESSAGE(" ConvolutionalNetwork: " << cnn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // test
  pair<double, Value> res = _mnist_test.train_and_test(
      *cnn,
      *trainer,
      DataSource_Stochastic::Random,
      20, // training_batch_size
      epochs,
      100 // testing_batch_size
  );
  BOOST_TEST_MESSAGE("*** Success rate: " << (res.first * 100) << "% Loss: " << res.second << " after " << epochs << " epochs");
  BOOST_TEST_MESSAGE(" ConvolutionalNetwork: " << cnn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);
}

BOOST_AUTO_TEST_CASE(LeNet1_Two_Labels_Test)
{
  // reduce the test size to two labels to make it faster
  BOOST_TEST_MESSAGE("*** Filtering test set...");
  _mnist_test.filter(1, 1000, 1000); // only allow 0,1 images; 1000 count
  BOOST_TEST_MESSAGE("*** Filtered test set: " << "\n" << _mnist_test);

  const double alpha = 0.5;
  const double beta = 0.9;
  const MatrixSize training_batch_size = 10;
  const size_t epochs = 3;

  auto layers = ConvolutionalNetwork::create_lenet1(
      _mnist_test.get_image_rows(), // input_rows
      _mnist_test.get_image_cols(), // input_cols
      PollingLayer::PollMode_Avg,
      20, // fc_size
      _mnist_test.get_label_size(),   // output_size
      make_unique<SigmoidFunction>(), // conv activation
      make_unique<SigmoidFunction>(), // poll activation
      make_unique<SigmoidFunction>()  // fc activation
  );
  YANN_CHECK(layers);
  auto nn = make_unique<Network>(std::move(layers));
  nn->init(Layer::InitMode_Random, Layer::InitContext(12345));
  nn->set_cost_function(make_unique<QuadraticCost>());

  // Trainer
  auto trainer = make_unique<Trainer>(
      make_unique<Updater_GradientDescentWithMomentum>(alpha, beta));
  BOOST_CHECK(trainer);
  trainer->set_batch_progress_callback(batch_progress_callback);

  // print info
  BOOST_TEST_MESSAGE("*** Training against MNIST dataset with ");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // train and test
  pair<double, Value> res = _mnist_test.train_and_test(
      *nn,
      *trainer,
      DataSource_Stochastic::Sequential, // want consistency for this test
      training_batch_size,
      epochs,
      100 // testing_batch_size
  );
  BOOST_TEST_MESSAGE("*** Success rate: " << (res.first * 100)
                     << "% Loss: " << res.second
                     << " after " << epochs << " epochs");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // save_to_file(*nn, "nn");

  // check
  BOOST_CHECK_GE(res.first, 0.99);  // > 99%%
  BOOST_CHECK_LE(res.second, 0.05); // < 0.05 per test
}

// The best result so far:
//
// Success rate for epoch 100: against training dataset: 98.3633% against test dataset: 98.38%
// Cost/loss per test for epoch 100: against training dataset: 0.122933 against test dataset: 0.122639
// Training time 17385 milliseconds
// Testing against training dataset time 7825.93 milliseconds
// Testing against test dataset time 1340.66 milliseconds
//
//  auto polling_mode = PollingLayer::PollMode_Avg;
//  unique_ptr<ActivationFunction> activation_func = make_unique<SigmoidFunction>();
//  unique_ptr<CostFunction> cost_func = make_unique<CrossEntropyCost>();
//  const MatrixSize fc_size = 100;
//  auto init_mode = Layer::InitMode_Random;
//  const double learning_rate = 0.01;
//  const double regularization = 0.0001;
//  const MatrixSize training_batch_size = 10;
//  const size_t epochs = 100;
//
BOOST_AUTO_TEST_CASE(LeNet1_Full_Test, * disabled())
{
  // reduce the test size to two labels to make it faster
  // BOOST_TEST_MESSAGE("*** Filtering test set...");
  // _mnist_test.filter(9, 10000, 1000); // only allow 0,1 images; 1000 count
  // BOOST_TEST_MESSAGE("*** Filtered test set: " << "\n" << _mnist_test);

  auto polling_mode = PollingLayer::PollMode_Max;
  unique_ptr<ActivationFunction> conv_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<ActivationFunction> poll_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<ActivationFunction> fc_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<CostFunction> cost_func = make_unique<CrossEntropyCost>();
  const MatrixSize fc_size = 120;
  auto init_mode = Layer::InitMode_Random;
  const double learning_rate = 0.05;
  const double regularization = 0.0001;
  const MatrixSize training_batch_size = 10;
  const size_t epochs = 100;

  auto layers = ConvolutionalNetwork::create_lenet1(
      _mnist_test.get_image_rows(), // input_rows
      _mnist_test.get_image_cols(), // input_cols
      polling_mode,
      fc_size,
      _mnist_test.get_label_size(), // output_size
      conv_activation_func,
      poll_activation_func,
      fc_activation_func
  );
  YANN_CHECK(layers);

  auto nn = make_unique<Network>(std::move(layers));
  YANN_CHECK(nn);
  nn->set_cost_function(cost_func);
  nn->init(init_mode);

  // Trainer
  auto trainer = make_unique<Trainer>(
      make_unique<Updater_GradientDescent>(learning_rate, regularization));
  BOOST_CHECK(trainer);
  trainer->set_batch_progress_callback(batch_progress_callback);

  // print info
  BOOST_TEST_MESSAGE("*** Training against MNIST dataset with ");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // train and test
  pair<double, Value> res = _mnist_test.train_and_test(
      *nn,
      *trainer,
      DataSource_Stochastic::Random,
      training_batch_size,
      epochs,
      100 // testing_batch_size
  );
  BOOST_TEST_MESSAGE("*** Success rate: " << (res.first * 100)
                     << "% Loss: " << res.second
                     << " after " << epochs << " epochs");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);
}

// The best result so far:
//
// Success rate for epoch 93: against training dataset: 96.4867% against test dataset: 96.14%
// Cost/loss per test for epoch 93: against training dataset: 0.229392 against test dataset: 0.256736
//
// Training time 35861.4 milliseconds
// Testing against training dataset time 17260.3 milliseconds
// Testing against test dataset time 2885.71 milliseconds
//
BOOST_AUTO_TEST_CASE(BoostedLeNet1_Full_Test, * disabled())
{
  // reduce the test size to two labels to make it faster
  // BOOST_TEST_MESSAGE("*** Filtering test set...");
  // _mnist_test.filter(9, 10000, 1000); // only allow 0,1 images; 1000 count
  // BOOST_TEST_MESSAGE("*** Filtered test set: " << "\n" << _mnist_test);

  auto polling_mode = PollingLayer::PollMode_Max;
  unique_ptr<ActivationFunction> conv_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<ActivationFunction> poll_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<ActivationFunction> fc_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<CostFunction> cost_func = make_unique<CrossEntropyCost>();
  const MatrixSize paths_num = 2;
  const MatrixSize fc_size = 50;
  const auto init_mode = Layer::InitMode_Random;
  const double alpha = 0.5;
  const double beta = 0.9;
  const MatrixSize training_batch_size = 10;
  const size_t epochs = 100;

  auto layers = ConvolutionalNetwork::create_boosted_lenet1(
      paths_num,
      _mnist_test.get_image_rows(), // input_rows
      _mnist_test.get_image_cols(), // input_cols
      polling_mode,
      fc_size,
      _mnist_test.get_label_size(), // output_size
      conv_activation_func,
      poll_activation_func,
      fc_activation_func
  );
  YANN_CHECK(layers);

  auto nn = make_unique<Network>(std::move(layers));
  YANN_CHECK(nn);
  nn->set_cost_function(cost_func);
  nn->init(init_mode);

  // Trainer
  auto trainer = make_unique<Trainer>(
      make_unique<Updater_GradientDescentWithMomentum>(alpha, beta));
  BOOST_CHECK(trainer);
  trainer->set_batch_progress_callback(batch_progress_callback);

  // print info
  BOOST_TEST_MESSAGE("*** Training against MNIST dataset with ");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // train and test
  pair<double, Value> res = _mnist_test.train_and_test(
      *nn,
      *trainer,
      DataSource_Stochastic::Random,
      training_batch_size,
      epochs,
      100 // testing_batch_size
  );
  BOOST_TEST_MESSAGE("*** Success rate: " << (res.first * 100) << "% Loss: " << res.second << " after " << epochs << " epochs");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);
}

BOOST_AUTO_TEST_CASE(LeNet5_Two_Labels_Test)
{
  // reduce the test size to two labels to make it faster
  BOOST_TEST_MESSAGE("*** Filtering test set...");
  _mnist_test.filter(1, 2000, 1000); // only allow 0,1 images; 1000 count
  BOOST_TEST_MESSAGE("*** Filtered test set: " << "\n" << _mnist_test);

  const double learning_rate = 0.1;
  const double regularization = 0.0;
  const MatrixSize training_batch_size = 10;
  const size_t epochs = 3;

  auto layers = ConvolutionalNetwork::create_lenet5(
      _mnist_test.get_image_rows(), // input_rows
      _mnist_test.get_image_cols(), // input_cols
      PollingLayer::PollMode_Avg,
      100, // fc1 size
      30, // fc2 size
      _mnist_test.get_label_size(), // output_size
      make_unique<SigmoidFunction>(), // conv activation
      make_unique<SigmoidFunction>(), // poll activation
      make_unique<SigmoidFunction>()  // fc activation
  );
  YANN_CHECK(layers);

  auto nn = make_unique<Network>(std::move(layers));
  YANN_CHECK(nn);
  nn->init(Layer::InitMode_Random, Layer::InitContext(12345));
  nn->set_cost_function(make_unique<CrossEntropyCost>());
  // nn->set_cost_function(make_unique<ExponentialCost>(2.0));

  // Trainer
  auto trainer = make_unique<Trainer>(
      make_unique<Updater_GradientDescent>(learning_rate, regularization));
  BOOST_CHECK(trainer);
  trainer->set_batch_progress_callback(batch_progress_callback);

  // print info
  BOOST_TEST_MESSAGE("*** Training against MNIST dataset with ");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // train and test
  pair<double, Value> res = _mnist_test.train_and_test(
      *nn,
      *trainer,
      DataSource_Stochastic::Sequential,
      training_batch_size,
      epochs,
      100  // testing_batch_size
  );
  BOOST_TEST_MESSAGE("*** Success rate: " << (res.first * 100)
                     << "% Loss: " << res.second
                     << " after " << epochs << " epochs");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // check
  BOOST_CHECK_GE(res.first, 0.95); // > 95%%
  BOOST_CHECK_LE(res.second, 0.5); // < 0.5 per test
}

// The best result so far:
//
// Success rate for epoch 50: against training dataset: 97.87% against test dataset: 97.61%
// Cost/loss per test for epoch 50: against training dataset: 0.144058 against test dataset: 0.154121
//
// Training time 39157.3 milliseconds
// Testing against training dataset time 17102.2 milliseconds
// Testing against test dataset time 2835.35 milliseconds
//
// unique_ptr<ActivationFunction> activation_func = make_unique<SigmoidFunction>();
// unique_ptr<CostFunction> cost_func = make_unique<CrossEntropyCost>();
// auto polling_mode = PollingLayer::PollMode_Max;
// const MatrixSize fc1_size = 120;
// const MatrixSize fc2_size = 84;
// auto init_mode = Layer::InitMode_Random;
// const double learning_rate = 0.01;
// const double regularization = 0.0001;
// const MatrixSize training_batch_size = 10;
//
BOOST_AUTO_TEST_CASE(LeNet5_Full_Test, * disabled())
{
  unique_ptr<ActivationFunction> conv_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<ActivationFunction> poll_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<ActivationFunction> fc_activation_func = make_unique<SigmoidFunction>();
  unique_ptr<CostFunction> cost_func = make_unique<CrossEntropyCost>();
  auto polling_mode = PollingLayer::PollMode_Max;
  const MatrixSize fc1_size = 120;
  const MatrixSize fc2_size = 84;
  auto init_mode = Layer::InitMode_Random;
  const double alpha = 0.01;
  const double beta = 0.9;
  const MatrixSize training_batch_size = 10;
  const size_t epochs = 50;

  auto layers = ConvolutionalNetwork::create_lenet5(
      _mnist_test.get_image_rows(), // input_rows
      _mnist_test.get_image_cols(), // input_cols
      polling_mode,
      fc1_size,
      fc2_size,
      _mnist_test.get_label_size(), // output_size
      conv_activation_func,
      poll_activation_func,
      fc_activation_func
  );
  YANN_CHECK(layers);

  auto nn = make_unique<Network>(std::move(layers));
  YANN_CHECK(nn);
  nn->init(init_mode);
  nn->set_cost_function(cost_func);

  // Trainer
  auto trainer = make_unique<Trainer>(
      make_unique<Updater_GradientDescentWithMomentum>(alpha, beta));
  BOOST_CHECK(trainer);
  trainer->set_batch_progress_callback(batch_progress_callback);

  // print info
  BOOST_TEST_MESSAGE("*** Training against MNIST dataset with ");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);

  // train and test
  pair<double, Value> res = _mnist_test.train_and_test(
      *nn,
      *trainer,
      DataSource_Stochastic::Random,
      training_batch_size,
      epochs,
      100 // testing_batch_size
  );
  BOOST_TEST_MESSAGE("*** Success rate: " << (res.first * 100)
                     << "% Loss: " << res.second
                     << " after " << epochs << " epochs");
  BOOST_TEST_MESSAGE(" CustomlNetwork: " << nn->get_info());
  BOOST_TEST_MESSAGE(" trainer: " << trainer->get_info());
  BOOST_TEST_MESSAGE(" epochs: " << epochs);
}

BOOST_AUTO_TEST_SUITE_END()


#include "Sidewalk.h"
#include "carla/geom/Math.h"
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/geometries.hpp>

namespace carla {
namespace sidewalk {

Sidewalk::Sidewalk(const std::vector<std::vector<geom::Vector2D>>& polygons)
  : _polygons(polygons) {

  std::vector<rt_value_t> index_entries;

  for (size_t i = 0; i < _polygons.size(); i++) {
    const std::vector<geom::Vector2D>& polygon = _polygons[i];
    for (size_t j = 0; j < polygon.size(); j++) {
      const geom::Vector2D& v_start = polygon[j];
      const geom::Vector2D& v_end = polygon[(j + 1) % polygon.size()];

      index_entries.emplace_back(
          rt_segment_t(
            rt_point_t(v_start.x, v_start.y),
            rt_point_t(v_end.x, v_end.y)),
          std::pair<size_t, size_t>(i, j));
    }
  }

  _segments_index = rt_tree_t(index_entries);
}

occupancy::OccupancyMap Sidewalk::CreateOccupancyMap(float width) const {
  occupancy::OccupancyMap occupancy_map;
  for (const std::vector<geom::Vector2D>& polygon : _polygons) {
    occupancy::OccupancyMap polygon_occupancy_map(polygon);
    occupancy::OccupancyMap outer_buffer = polygon_occupancy_map.Buffer(width / 2);
    occupancy::OccupancyMap inner_buffer = polygon_occupancy_map.Buffer(-width / 2);
    occupancy::OccupancyMap buffer_difference = outer_buffer.Difference(inner_buffer);
    occupancy_map = occupancy_map.Union(buffer_difference);
  }
  return occupancy_map;
}

segments::SegmentMap Sidewalk::CreateSegmentMap() const {
  std::vector<geom::Segment2D> segments;

  for (const auto& polygon : _polygons) {
    for (size_t i = 0; i < polygon.size(); i++) {
      segments.emplace_back(
          polygon[i],
          polygon[(i + 1) % polygon.size()]);
    }
  }

  return segments::SegmentMap(std::move(segments));
}

geom::Vector2D Sidewalk::GetRoutePointPosition(const SidewalkRoutePoint& route_point) const {
  const geom::Vector2D segment_start = _polygons[route_point.polygon_id][route_point.segment_id];
  const geom::Vector2D& segment_end = _polygons[route_point.polygon_id][(route_point.segment_id + 1) % _polygons[route_point.polygon_id].size()];
  return segment_start + (segment_end - segment_start).MakeUnitVector() * route_point.offset;
}

SidewalkRoutePoint Sidewalk::GetNearestRoutePoint(const geom::Vector2D& position) const {
  std::vector<rt_value_t> results;
  _segments_index.query(boost::geometry::index::nearest(rt_point_t(position.x, position.y), 1), std::back_inserter(results));
  rt_value_t& result = results[0];

  geom::Vector2D segment_start(
      boost::geometry::get<0, 0>(result.first),
      boost::geometry::get<0, 1>(result.first));
  geom::Vector2D segment_end(
      boost::geometry::get<1, 0>(result.first),
      boost::geometry::get<1, 1>(result.first));
  geom::Vector2D direction = (segment_end - segment_start).MakeUnitVector();

  float offset = geom::Vector2D::DotProduct(
      position - segment_start,
      direction);
  offset = std::max(0.0f, std::min((segment_end - segment_start).Length(), offset));

  return SidewalkRoutePoint{
      result.second.first,
      result.second.second,
      offset};
}

SidewalkRoutePoint Sidewalk::GetNextRoutePoint(const SidewalkRoutePoint& route_point, float lookahead_distance) const {
  const geom::Vector2D& segment_start = _polygons[route_point.polygon_id][route_point.segment_id];
  const geom::Vector2D& segment_end = _polygons[route_point.polygon_id][(route_point.segment_id + 1) % _polygons[route_point.polygon_id].size()];
  float segment_length = (segment_end - segment_start).Length();

  if (route_point.offset + lookahead_distance <= segment_length) {
    return SidewalkRoutePoint{
        route_point.polygon_id,
        route_point.segment_id,
        route_point.offset + lookahead_distance};
  } else {
    return GetNextRoutePoint(
        SidewalkRoutePoint{
          route_point.polygon_id,
          (route_point.segment_id + 1) % _polygons[route_point.polygon_id].size(),
          0},
        lookahead_distance - (segment_length - route_point.offset));
  }
}

SidewalkRoutePoint Sidewalk::GetPreviousRoutePoint(const SidewalkRoutePoint& route_point, float lookahead_distance) const {
  if (route_point.offset - lookahead_distance >= 0) {
    return SidewalkRoutePoint{
        route_point.polygon_id,
        route_point.segment_id,
        route_point.offset - lookahead_distance};
  } else {
    const geom::Vector2D& segment_start = _polygons[route_point.polygon_id][route_point.segment_id];
    size_t previous_segment_id = (route_point.segment_id == 0 ? _polygons[route_point.polygon_id].size() : route_point.segment_id) - 1;
    const geom::Vector2D& previous_segment_start = _polygons[route_point.polygon_id][previous_segment_id];

    return GetPreviousRoutePoint(
        SidewalkRoutePoint{
          route_point.polygon_id,
          previous_segment_id,
          (segment_start - previous_segment_start).Length()},
        lookahead_distance - route_point.offset);
  }
}

boost::optional<SidewalkRoutePoint> Sidewalk::GetAdjacentRoutePoint(const SidewalkRoutePoint& route_point, float max_cross_distance) const {
  const geom::Vector2D& segment_start = _polygons[route_point.polygon_id][route_point.segment_id];
  const geom::Vector2D& segment_end = _polygons[route_point.polygon_id][(route_point.segment_id + 1) % _polygons[route_point.polygon_id].size()];
  geom::Vector2D direction = (segment_end - segment_start).MakeUnitVector();
  geom::Vector2D normal = direction.Rotate(geom::Math::Pi<float>() / 2);

  geom::Vector2D ray_start = segment_start + route_point.offset * direction;
  geom::Vector2D ray_end = ray_start + max_cross_distance * normal;

  rt_segment_t ray(
      rt_point_t(ray_start.x, ray_start.y),
      rt_point_t(ray_end.x, ray_end.y));

  std::vector<rt_value_t> results;
  _segments_index.query(
      boost::geometry::index::intersects(ray),
      std::back_inserter(results));

  boost::optional<float> best_distance;
  boost::optional<rt_value_t> best_result;
  boost::optional<rt_point_t> best_intersection;

  for (const rt_value_t& result : results) {
    if (result.second.first == route_point.polygon_id && result.second.second == route_point.segment_id) continue;

    std::vector<rt_point_t> intersection;
    boost::geometry::intersection(result.first, ray, intersection);
    float distance = static_cast<float>(boost::geometry::distance(ray.first, intersection[0]));
    if (!best_distance || distance < best_distance) {
      best_distance = boost::optional<float>(distance);
      best_result = boost::optional<rt_value_t>(result);
      best_intersection = boost::optional<rt_point_t>(intersection[0]);
    }
  }

  if (best_distance) {
    size_t best_polygon_id = best_result->second.first;
    size_t best_segment_id = best_result->second.second;
    float best_offset = (geom::Vector2D(best_intersection->get<0>(), best_intersection->get<1>()) - _polygons[best_polygon_id][best_segment_id]).Length();

    return boost::optional<SidewalkRoutePoint>({
        best_polygon_id, best_segment_id, best_offset});
  } else {
    return boost::optional<SidewalkRoutePoint>();
  }

}

bool Sidewalk::Intersects(const geom::Segment2D& segment) const {
  rt_segment_t segment_(
      rt_point_t(segment.start.x, segment.start.y),
      rt_point_t(segment.end.x, segment.end.y));

  std::vector<rt_value_t> results;
  _segments_index.query(
      boost::geometry::index::intersects(segment_),
      std::back_inserter(results));

  return results.size() > 0;
}

}
}


//
//  cal_fe_global_plastic.hpp
//  hybrid_fem_bie
//
//  Created by Max on 3/21/18.
//
//

#ifndef cal_fe_global_plastic_hpp
#define cal_fe_global_plastic_hpp

#include <stdio.h>
#include <Eigen/Eigen>
#include "mapglobal.hpp"
#include "maplocal.hpp"
#include "cal_fe_int_el.hpp"
using namespace Eigen;

void cal_fe_global_plastic(int n_el, MatrixXi &index_store,MatrixXd &ke, std::vector<MatrixXd> &B_mat, double detJ, double E, double nu, double q, VectorXd &u_n, VectorXd &v_n, int Ndofn, VectorXd &fe_global, std::vector<MatrixXd> &strain_n_store,std::vector<MatrixXd> &stress_n_store,
                           double sxx_initial,double syy_initial, double sxy_initial, double cohes, double blkfric);

#endif /* cal_fe_global_plastic_hpp */


#ifndef STEREO_LIDAR_CALIBRATION_UTILS_HPP
#define STEREO_LIDAR_CALIBRATION_UTILS_HPP
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/geometry.h>
#include <pcl/registration/icp.h>

#include <Eigen/Dense>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Geometry>
#include <unsupported/Eigen/MatrixFunctions>
#include <set>
#include <string>
#include <cmath>
#include <unistd.h>
#include <vector>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <sophus/so3.hpp>

using namespace Eigen;
using namespace std;

class ImageResults{
public:
    vector<int> valid_index; // 有效的图像index
    vector<vector<Vector3d>> corners_3d; // 图像3d角点
    vector<vector<Vector2d>> chessboard_points_2d;
    vector<vector<Vector3d>> chessboard_points_3d;
    vector<VectorXd> planes_3d; // 图像的平面方程
    vector<vector<VectorXd>> lines_3d; // 图像边缘直线方程
};

class CloudResults{
public:
    vector<int> valid_index; // 有效的点云index
    vector<vector<Vector3d>> corners_3d; // 点云3d角点
    vector<pcl::PointCloud<pcl::PointXYZ>> plane_points_3d;
    //vector<vector<Vector3d>> plane_points_3d;
    vector<vector<VectorXd>> lines_3d; // 点云边缘直线方程
    vector<vector<pcl::PointCloud<pcl::PointXYZ>>> lines_points_3d ;//直线包含的点云
};

void display_colored_by_depth(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
void display_four_corners(vector<pcl::PointXYZ>& corners);
void display_multi_clouds(vector<pcl::PointCloud<pcl::PointXYZ>>& clouds);

// void show_cloud(pcl::PointCloud<pcl::PointXYZ>& cloud, std::string viewer_name="debug"){
//     pcl::visualization::CloudViewer viewer(viewer_name);
//     viewer.showCloud(cloud);
//     while (!viewer.wasStopped ())
//     {
//         sleep(0.1);
//     }
//     return;
// }
bool computeTransICP(const pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud,
                     const pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, Eigen::Matrix4d &trans);

void calculateInitialRt(vector<vector<Vector3d>>& cloud_3d_corners, vector<vector<Vector3d>>& image_3d_corners, VectorXd& R_t);

pcl::visualization::PCLVisualizer::Ptr show_multi_clouds(std::vector<pcl::PointCloud<pcl::PointXYZ>>& clouds);

pcl::visualization::PCLVisualizer::Ptr show_multi_clouds_with_specified_colors(std::vector<pcl::PointCloud<pcl::PointXYZ>>& clouds);

cv::Point2d project(cv::Point3d p, cv::Mat CameraMat);

// 获取指定路径下指定后缀名的文件完整路径
void get_data_by_path(std::vector<std::string>& data, std::string& path, std::string ext);

// 检查图像和激光点云的文件是否对应
bool check_data_by_index(std::vector<std::string>& data1, std::vector<std::string>& data2);

bool loadPointXYZ(string& path, pcl::PointCloud<pcl::PointXYZ>::Ptr& pcd);
// 文件路径操作
bool isDirExist(const std::string& path_name);

void matrix2Vector(Matrix4d& m, VectorXd& v);

void vector2Matrix(VectorXd& v, Matrix4d& m);

void  vector2Affine(Eigen::VectorXd& Rt, Eigen::Affine3d& affine_mat);

void  affine2Vector( Eigen::Affine3d& affine_mat,Eigen::VectorXd& Rt);

void draw_axis(cv::Mat& img, cv::Mat& rvec, cv::Mat& tvec, cv::Mat K, cv::Mat dist);

void draw_corners_in_image(cv::Mat& img, std::vector<cv::Point3d>& corners, cv::Mat& rvec, cv::Mat& tvec, cv::Mat K, cv::Mat dist);

#endif


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MIQP_COMMON_PARAMETER_REGIONS_HPP_
#define MIQP_COMMON_PARAMETER_REGIONS_HPP_

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

#include <map>
#include <set>
#include "linearization_parameters.hpp"

namespace miqp {
namespace common {
namespace parameter {

typedef int RegionIdx;
typedef std::multimap<bool, RegionIdx, std::greater<bool>> Regions;

/**
 * @brief calculates region based on vx and vy. this might yield more than one
 * region at the borders
 *
 * @param frac_params
 * @param vx
 * @param vy
 * @return Regions
 */
Regions CalculateRegionIdx(const FractionParameters& frac_params,
                           const float vx, const float vy);

/**
 * @brief Set the Initial Region
 *
 * @param regions_map
 * @param idx
 */
void SetInitialRegion(Regions& regions_map, RegionIdx idx);

/**
 * @brief Calculates all possible combinations of region vectors of N cars
 *
 * @param region_vec_cars_in
 * @param region_per_car
 * @param region_comb_out
 * @param idx
 */
void CalculateRegionCombinations(
    const std::vector<Regions>& region_vec_cars_in,
    std::vector<RegionIdx>& region_per_car,
    std::vector<std::vector<RegionIdx>>& region_comb_out, RegionIdx idx = 0);

/**
 * @brief converts region number to binary array
 *
 * @param regions
 * @param nrRegions
 * @return Eigen::MatrixXi
 */
Eigen::MatrixXi RegionNumberToBinaryArray(const std::set<RegionIdx>& regions,
                                          const int& nrRegions);

/**
 * @brief reserves neighbour regions as possible to reach for the car
 *
 * @param regions
 * @param row
 * @param expansions
 * @return true if everything worked okay
 */
bool ReserveNeighborRegions(Eigen::MatrixXi& regions, const int row,
                            const int expansions);

/**
 * @brief calculates possible regions from orientation of reference line
 *
 * @param frac_params
 * @param thetaRef
 * @return std::set<RegionIdx>
 */
std::set<RegionIdx> CalculatePossibleRegions(
    const FractionParameters& frac_params, Eigen::VectorXd& thetaRef);

}  // namespace parameter
}  // namespace common
}  // namespace miqp

#endif  // MIQP_COMMON_PARAMETER_REGIONS_HPP_


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/ulp.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/mindenormal.hpp>

STF_CASE_TPL (" ulp real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ulp;
  using r_t = decltype(ulp(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_IEEE_EQUAL(ulp(bs::Inf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(ulp(bs::Minf<T>()), bs::Nan<r_t>());
  STF_IEEE_EQUAL(ulp(bs::Nan<T>()), bs::Nan<r_t>());
#endif
  STF_EQUAL(ulp(bs::Mone<T>()), bs::Eps<r_t>()/2);
  STF_EQUAL(ulp(bs::One<T>()), bs::Eps<r_t>()/2);
  STF_EQUAL(ulp(bs::Zero<T>()), bs::Mindenormal<r_t>());
}

STF_CASE_TPL (" ulp unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ulp;
  using r_t = decltype(ulp(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(ulp(bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(ulp(bs::Zero<T>()), bs::One<r_t>());
}

STF_CASE_TPL (" ulp signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ulp;
  using r_t = decltype(ulp(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(ulp(bs::Mone<T>()), bs::One<r_t>());
  STF_EQUAL(ulp(bs::One<T>()), bs::One<r_t>());
  STF_EQUAL(ulp(bs::Zero<T>()), bs::One<r_t>());
}