/* Copyright 2020 Oinam Romesh Meitei

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
 */

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
#include "fmath.hpp"
#include "pulsec.h"
#include "agradc.h"
#include "grad_ana.h"
#include "pulsehelper.h"

#include <iostream>
#include <iomanip>
#include <chrono>

namespace py = pybind11;

gausgrad gaus_getnamp(int nqubit, int &ngaus, double duration,
						 std::vector< std::vector<double > > &amp,
						 std::vector< std::vector<double > > &sigma,
						 std::vector< std::vector<double > > &mean,
						 std::vector<double > freq,
						 std::vector<double> &tlist,
						 std::vector<std::complex<double> > &ini_vec,
						 std::vector< std::vector< Eigen::SparseMatrix
									   <double,0,ptrdiff_t> > > hdrive,
						 std::vector< std::complex<double> > dsham,
						 std::vector< int> &states,
						 Eigen::MatrixXcd &cham){

  double tmpamp;
  int tlen = tlist.size();

  std::vector< std::vector< std::vector<double > > >
    expterm(nqubit, std::vector<std::vector<double> > (ngaus, std::vector< double> (tlen)));

  std::vector<std::vector<double > > tseq;
  int nwindow = 0;
  double gamp, gsig, gmean, esigmean;
  std::vector<double > agradient;

  std::vector<std::vector<double > > tamp(nqubit, std::vector<double> (tlen));
  int i,j,k;
  for (i=0; i<nqubit; i++){
    for (j=0;j<ngaus; j++){
      for (k=0;k<tlen; k++){
	expterm[i][j][k] = fmath::expd(-sigma[i][j] * sigma[i][j] *
				       (tlist[k] - mean[i][j]) *
				       (tlist[k] - mean[i][j]));
	tamp[i][k] += amp[i][j] * expterm[i][j][k];
      }
    }
  }

  pulsec pobj(tamp, tseq, freq, duration, nqubit, nwindow);

  gradc aobj = grad_ana(tlist, ini_vec, pobj, hdrive, dsham, states, cham);

  for(int i=0; i< nqubit; i++){
    for(int j=0; j< ngaus; j++){
      gamp = 0.0;
      gsig = 0.0;
      gmean = 0.0;
      for(int k=0; k< tlen;k++){
	esigmean = aobj.gradient[i][k] * expterm[i][j][k];

	gamp += esigmean;
	gsig += esigmean * amp[i][j] * -2.0 * sigma[i][j] *
	  (tlist[k] - mean[i][j])*(tlist[k] - mean[i][j]);
	gmean += esigmean * amp[i][j] * 2.0 * sigma[i][j] * sigma[i][j] *
	  (tlist[k] - mean[i][j]);

      }
      agradient.push_back(gamp);
      agradient.push_back(gsig);
      agradient.push_back(gmean);
    }
  }
  gausgrad gausobj(aobj.energy, aobj.norm, agradient, tamp);

  return gausobj;
}

std::vector<double> gaus_gettamp(int &nqubit, int &ngaus,
				std::vector<double> &tlist,
				std::vector< std::vector<double > > &amp,
				std::vector< std::vector<double > > &sigma,
				std::vector< std::vector<double > > &mean,
				std::vector< std::vector<double > > &gradient_){

  double gamp, gsig, gmean, esigmean;
  std::vector<double > agradient;
  int tlen = tlist.size();

  for(int i=0; i< nqubit; i++){
    for(int j=0; j< ngaus; j++){
      gamp = 0.0;
      gsig = 0.0;
      gmean = 0.0;
      for(int k=0; k< tlen;k++){
	esigmean = gradient_[i][k] * fmath::expd(-sigma[i][j]*sigma[i][j] *
						(tlist[k] - mean[i][j]) *
						(tlist[k] - mean[i][j]));
	gamp += esigmean;
	gsig += esigmean * amp[i][j] * -2.0 * sigma[i][j] *
	  (tlist[k] - mean[i][j])*(tlist[k] - mean[i][j]);
	gmean += esigmean * amp[i][j] * 2.0 * sigma[i][j] * sigma[i][j] *
	  (tlist[k] - mean[i][j]);

      }
      agradient.push_back(gamp);
      agradient.push_back(gsig);
      agradient.push_back(gmean);
    }
  }
  return agradient;
}

PYBIND11_MODULE(pulse_helper,m){
  m.def("gaus_getnamp", &gaus_getnamp, "gaus_getnamp");
  m.def("gaus_gettamp", &gaus_gettamp, "gaus_gettamp");
  py::class_<gausgrad>(m,"gausgrad")
    .def(py::init<
	 double &, double &,
	 std::vector<double> &,
	 std::vector<std::vector< double > > & > ())
    .def_readonly("energy", &gausgrad::energy)
    .def_readonly("norm", &gausgrad::norm)
    .def_readonly("gradient",&gausgrad::gradient)
    .def_readonly("amp",&gausgrad::amp);
}


#include <igl/writeDMAT.h>
#include <igl/readDMAT.h>
#include <igl/readMESH.h>
#include <igl/get_seconds.h>

#include <GaussIncludes.h>
#include <ForceSpring.h>
#include <FEMIncludes.h>
#include <PhysicalSystemParticles.h>
#include <ConstraintFixedPoint.h>

#include <boost/filesystem.hpp>
#include <nlohmann/json.hpp>

#include <omp.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <memory>
#include <algorithm>

#include "GreedyCubop.h"

using json = nlohmann::json;
namespace fs = boost::filesystem;
using namespace std;
using namespace Gauss;
using namespace FEM;
using namespace ParticleSystem;

typedef PhysicalSystemFEM<double, NeohookeanTet> NeohookeanTets;

typedef World<double,
    std::tuple<PhysicalSystemParticleSingle<double> *, NeohookeanTets *>,
    std::tuple<ForceSpringFEMParticle<double> *>, std::tuple<ConstraintFixedPoint<double> *> > MyWorld;

// Globals
Eigen::MatrixXd V; // Verts
Eigen::MatrixXi T; // Tet indices
Eigen::MatrixXi F; // Face indices
fs::path model_root;
int goal_tet_count = 0;
double starting_time;

void eig_to_VEC(const Eigen::VectorXd &eig_vec, VECTOR &vec_vec) {
    vec_vec.resizeAndWipe(eig_vec.size());
    std::memcpy(&vec_vec(0), eig_vec.data(), eig_vec.size() * sizeof(double));
}

Eigen::VectorXd VEC_to_eig(VECTOR &vec) {
    Eigen::VectorXd eig(vec.size());
    std::memcpy(eig.data(), &vec(0), vec.size() * sizeof(double));
    return eig;
}

std::string ZeroPadNumber(int num)
{
    std::ostringstream ss;
    ss << std::setw( 7 ) << std::setfill( '0' ) << num;
    return ss.str();
}

template<typename T>
T get_json_value(const json &j, const std::string &key, T def) {
    try {
        return j.at(key);
    }
    catch (nlohmann::detail::out_of_range& e){
        return def;
    }
}

class MyGreedyCubop : public GreedyCubop {
public:
    MyGreedyCubop(const Eigen::MatrixXd &V, const Eigen::MatrixXi &T, const Eigen::MatrixXd &U, double YM, double poisson, double density, const std::vector<Eigen::VectorXd> &reduced_forces, const std::vector<Eigen::VectorXd> &red_displacements) : GreedyCubop(), m_reduced_forces(reduced_forces), m_red_displacements(red_displacements) {
        m_tets = make_unique<NeohookeanTets>(V,T);
        for(auto element: m_tets->getImpl().getElements()) {
            element->setDensity(density);
            element->setParameters(YM, poisson);
        }
        // m_world.addSystem(m_tets.get());
        // m_world.finalize();

        m_U = U;
    }

protected:
    /**
     * Return the total number of points we have to choose from.
     * For example, for FEM piecewise-linear tetrahedra elements, this is just the number of tets.
     */
    int numTotalPoints() {
        return m_tets->getImpl().getNumElements();
    }

    /**
     * The main method that subclasses must implement. This should evaluate the reduced force density (or just the force, since
     * the volume/area is constant anyway) at the given point.
     *
     *  pointId - an index in [0, getNumTotalPoints()). Implementations must map this index deterministically to a cubature point.
     */
    void evalPointForceDensity( int pointId, VECTOR& q, VECTOR& gOut ) {
        // THIS ISN'T THREAD SAFE WITHOUT COPYING THE WORLD
        // Make a copy of the world every time... Not the most efficient, but still faster than being single threaded
        // double start = igl::get_seconds();

        MyWorld m_world;
        m_world.addSystem(m_tets.get());
        m_world.finalize();

        // update position of only the tet pointId
        Eigen::VectorXd eig_q = VEC_to_eig(q);
        auto gauss_map_q = mapDOFEigen(m_tets->getQ(), m_world);
        const int n_verts = 4;
        std::vector<int> verts(n_verts);
        for(int i = 0; i < n_verts; i++) {
            const int tet_vert = m_tets->getImpl().getElement(pointId)->getQDOFList()[i]->getGlobalId();
            verts[i] = tet_vert;

            for(int j = 0; j < 3; j++) {
                gauss_map_q[tet_vert + j] = m_U.transpose().row(tet_vert + j) * eig_q;
            }
        }
        // gauss_map_q = m_U.transpose() * eig_q;

        // Get the element force
        Eigen::VectorXd sampled_force(12);
        m_tets->getImpl().getElement(pointId)->getInternalForce(sampled_force, m_world.getState());

        // Assemble and project the full force
        Eigen::SparseVector<double> full_force(m_U.cols());
        for(int i = 0; i < n_verts; i++) {
            int vert_index = verts[i];//m_tets->getImpl().getElement(pointId)->getQDOFList()[i]->getGlobalId();
            for(int j = 0; j < 3; j++) {
                full_force.coeffRef(vert_index + j) = sampled_force[i * 3 + j];
            }
        }

        eig_to_VEC(m_U * full_force, gOut);
        // std::cout << "evalPointForceDensity: " << igl::get_seconds() - start << "s" << std::endl;
    }

    /**
     * At the end of each iteration (_not_ a sub-train iteration), when some cubature has been optimized, this will be called.
     * Implementations should probably overwrite this and write the cubature out to file or something.
     */
    void handleCubature( std::vector<int>& selectedPoints, VECTOR& weights, Real relErr ) {
        stringstream my_out;

        my_out << "n_tets: " << selectedPoints.size() << endl;
        my_out << "relErr: " << relErr << endl;
        my_out << "Selected tets: ";
        for (std::vector<int>::iterator i = selectedPoints.begin(); i != selectedPoints.end(); ++i)
        {
            my_out << *i << ", ";
        }
        my_out << endl;

        std::vector<int> nonzero_indices;
        std::vector<double> nonzero_weights;
        int n_nonzero = 0;
        my_out << "Weights: ";
        for (int i = 0; i < weights.size(); ++i)
        {
            if(weights(i) > 0.00001) {
                n_nonzero++;
                nonzero_indices.push_back(selectedPoints[i]);
                nonzero_weights.push_back(weights(i));
            }

            my_out << weights(i) << ", ";
        }
        my_out << endl << "n_nonzero: " << n_nonzero << endl;
        my_out << endl;
        my_out << "Running time so far: " << igl::get_seconds() - starting_time << "s" << std::endl;
        my_out << endl;

        Eigen::VectorXi Is = Eigen::Map<Eigen::VectorXi>(&nonzero_indices[0], nonzero_indices.size());
        Eigen::VectorXd Ws = Eigen::Map<Eigen::VectorXd>(&nonzero_weights[0], nonzero_weights.size());

        fs::path energy_model_dir = model_root / ("energy_model/an08/pca_dim_" + std::to_string(m_U.rows()) + "/");
        fs::path this_iteration_output_dir = energy_model_dir / (ZeroPadNumber(n_nonzero) + "_samples/");
        fs::create_directories(this_iteration_output_dir);

        fs::path indices_path = this_iteration_output_dir / "indices.dmat";
        fs::path weights_path = this_iteration_output_dir / "weights.dmat";
        igl::writeDMAT(indices_path.string(), Is);
        igl::writeDMAT(weights_path.string(), Ws);

        fs::path details_path = this_iteration_output_dir / "details.txt";
        ofstream fout(details_path.string());
        fout << my_out.str();
        fout.close();

        cout << my_out.str();
        cout << "Saved weights and indices to " << this_iteration_output_dir.string() << endl << endl;

        const bool DEBUG = false;
        if(DEBUG) {
            int example_id = 500 % m_reduced_forces.size();//250;
            Eigen::VectorXd actual_g = m_reduced_forces[example_id];
            Eigen::VectorXd pred_g = get_predicted_force(example_id, nonzero_indices, nonzero_weights);

            std::cout << actual_g.transpose() << std::endl;
            std::cout << pred_g.transpose() << std::endl;

            std::cout << (actual_g - pred_g).transpose().cwiseAbs() << std::endl;

            std::cout << (actual_g - pred_g).squaredNorm() << std::endl;
        }

        // output everything every frame
        {
            fs::path indices_path = energy_model_dir / "indices.dmat";
            fs::path weights_path = energy_model_dir / "weights.dmat";
            igl::writeDMAT(indices_path.string(), Is);
            igl::writeDMAT(weights_path.string(), Ws);

            fs::path details_path = energy_model_dir / "details.txt";
            ofstream fout(details_path.string());
            fout << my_out.str();
            fout.close();
        }
        if(n_nonzero >= goal_tet_count) {
            cout << "Reached goal number of tets. Exiting." << endl;
            exit(0);
        }
    }

    Eigen::VectorXd get_predicted_force(int example_id, const std::vector<int> &nonzero_indices, const std::vector<double> &nonzero_weights) {
        Eigen::VectorXd pred_g = Eigen::VectorXd::Zero(m_U.rows());

        MyWorld world;
        world.addSystem(m_tets.get());
        world.finalize();
        auto gauss_map_q = mapDOFEigen(m_tets->getQ(), world);
        gauss_map_q =  m_U.transpose() * m_red_displacements[example_id]; // Update the mesh

        int n_sample_tets = nonzero_indices.size();
        Eigen::SparseMatrix<double> neg_energy_sample_jac(m_U.cols(), T.rows());// m_cubature_indices.size());
        neg_energy_sample_jac.reserve(Eigen::VectorXi::Constant(m_U.cols(), T.cols() * 3));

        Eigen::VectorXd cubature_weights = Eigen::Map<const Eigen::VectorXd>(&nonzero_weights[0], nonzero_weights.size());

        int n_force_per_element = T.cols() * 3;
        Eigen::MatrixXd element_forces(n_sample_tets, n_force_per_element);
        Eigen::VectorXd energy_samp(n_sample_tets);

        for(int i = 0; i < n_sample_tets; i++) { // TODO parallel
            int tet_index = nonzero_indices[i];
            energy_samp[i] = m_tets->getImpl().getElement(tet_index)->getStrainEnergy(world.getState());
            // std::cout << energy_samp[i] << std::endl;
            //Forces
            Eigen::VectorXd sampled_force(n_force_per_element);
            m_tets->getImpl().getElement(tet_index)->getInternalForce(sampled_force, world.getState());
            element_forces.row(i) = sampled_force;
        }

        // std::cout << element_forces << std::endl;

        for(int i = 0; i < n_sample_tets; i++) {
            int tet_index = nonzero_indices[i];
            for(int j = 0; j < 4; j++) {
                int vert_index = m_tets->getImpl().getElement(tet_index)->getQDOFList()[j]->getGlobalId();
                for(int k = 0; k < 3; k++) {
                    neg_energy_sample_jac.insert(vert_index + k, i) = element_forces(i, j*3 + k);
                }
            }
        }
        pred_g = m_U * (neg_energy_sample_jac * cubature_weights);

        return pred_g;
    }

private:
    // MyWorld m_world;
    unique_ptr<NeohookeanTets> m_tets;
    Eigen::MatrixXd m_U;
    const std::vector<Eigen::VectorXd> &m_reduced_forces;
    const std::vector<Eigen::VectorXd> &m_red_displacements;
};

std::vector<int> get_file_numbers_for_prefix(std::string prefix, fs::path dir) {
    std::vector<int> nums;
    for (auto i = fs::directory_iterator(dir); i != fs::directory_iterator(); i++)
    {
        fs::path cand_path = i->path();

        std::string name = cand_path.filename().string();
        if(strncmp(name.c_str(), prefix.c_str(), prefix.size()) == 0) { // Is forces
            std::string num_str = name.substr(prefix.size());
            nums.push_back(std::stoi(num_str, nullptr));
        }
    }
    std::sort(nums.begin(), nums.end());
    return nums;
}

std::vector<Eigen::VectorXd> load_forces(fs::path training_data_root, fs::path reduced_basis_path) {
    std::cout << "Loading reduced internal forces..." << std::endl;
    Eigen::MatrixXd U;
    igl::readDMAT(reduced_basis_path.string(), U);
    U.transposeInPlace();

    std::string prefix = "internalForces_";
    std::vector<int> forces_nums = get_file_numbers_for_prefix(prefix, training_data_root);
    std::vector<Eigen::VectorXd> recorded_forces(forces_nums.size());

    std::cout << "num threads: " << omp_get_num_threads() << std::endl;
    #pragma omp parallel for
    for (int i = 0; i < forces_nums.size(); i++)
    {//   std::vector<int>::iterator i = forces_nums.begin(); i != forces_nums.end(); ++i
        fs::path dis_path = training_data_root / (prefix + std::to_string(forces_nums[i]) + ".dmat");
        Eigen::VectorXd F;
        igl::readDMAT(dis_path.string(), F);
        recorded_forces[i] = U * F;
    }

    std::cout << "Done." << std::endl;

    return recorded_forces;
}

std::vector<Eigen::VectorXd> load_displacements(fs::path training_data_root, fs::path reduced_basis_path) {
    std::cout << "Loading displacements..." << std::endl;
    Eigen::MatrixXd U;
    igl::readDMAT(reduced_basis_path.string(), U);
    U.transposeInPlace();

    std::string prefix = "displacements_";
    std::vector<int> displacements_nums = get_file_numbers_for_prefix(prefix, training_data_root);
    std::vector<Eigen::VectorXd> recorded_displacements(displacements_nums.size());

    #pragma omp parallel for
    for (int i = 0; i < displacements_nums.size(); i++)
    {
        fs::path dis_path = training_data_root / (prefix + std::to_string(displacements_nums[i]) + ".dmat");
        Eigen::MatrixXd Q;
        igl::readDMAT(dis_path.string(), Q);
        Eigen::VectorXd q = Eigen::Map<Eigen::VectorXd>(Q.transpose().data(), Q.rows() * Q.cols());
        recorded_displacements[i]=(U*q);
    }
    std::cout << "Done." << std::endl;

    return recorded_displacements;
}

void progress_bar(double progress, int barWidth = 70) {
    std::cout << "[";
    int pos = barWidth * progress;
    for (int i = 0; i < barWidth; ++i) {
        if (i < pos) std::cout << "=";
        else if (i == pos) std::cout << ">";
        else std::cout << " ";
    }
    std::cout << "] " << int(progress * 100.0) << " %\r";
    std::cout.flush();
}

std::vector<Eigen::VectorXd> get_forces_from_reduced_displacements(const std::vector<Eigen::VectorXd> &red_displacements, const Eigen::MatrixXd &V, const Eigen::MatrixXi &T, const Eigen::MatrixXd &U, double YM, double poisson, double density) {
    std::cout << "Generating forces for reduced poses..." << std::endl;
    unique_ptr<NeohookeanTets> tets = make_unique<NeohookeanTets>(V,T);

    for(auto element: tets->getImpl().getElements()) {
        element->setDensity(density);//1000.0);
        element->setParameters(YM, poisson);
    }
    std::vector<Eigen::VectorXd> forces(red_displacements.size());
    int n_done = 0;
    double start = igl::get_seconds();
    #pragma omp parallel
    {
        MyWorld world;
        world.addSystem(tets.get());
        world.finalize();

        #pragma omp for
        for(int i = 0; i < red_displacements.size(); i++) {
            Eigen::Map<Eigen::VectorXd> gauss_map_q = mapDOFEigen(tets->getQ(), world);
            gauss_map_q = U.transpose() * red_displacements[i];

            AssemblerEigenVector<double> internal_force;
            getInternalForceVector(internal_force, *tets, world);
            forces[i] = (U * (*internal_force));
            // std::cout << forces.back() << std::endl;
            #pragma omp atomic
            n_done++;

            if(i % 20 == 0) progress_bar(n_done / (double) red_displacements.size());
        }
    }
    std::cout << "Generating forces: " << igl::get_seconds() - start << "s" << std::endl;
    std::cout << std::endl;
    return forces;
}

int main(int argc, char **argv) {
    starting_time = igl::get_seconds();

    if(argc < 3) {
        cout << "Need to pass in a path to model root and a goal number of tets." << endl;
        exit(1);
    }

    // ---- Set Up
    model_root = fs::path(argv[1]);
    goal_tet_count = std::stoi(argv[2]);

    fs::path reduced_basis_path = model_root / "pca_results" / "ae_pca_components.dmat";

    int pca_dim;
    if(argc == 4) {
        pca_dim = std::stoi(argv[3]);
        reduced_basis_path = model_root / "pca_results" / ("pca_components_" + std::to_string(pca_dim) + ".dmat");
    }

    fs::path model_config_path = model_root / "model_config.json";
    std::ifstream fin_model(model_config_path.string());
    json model_config;
    fin_model >> model_config;

    std::string tdr = model_config["training_dataset"];
    fs::path training_data_root = tdr;
    std::cout << training_data_root << std::endl;
    fs::path mesh_path = model_root / "tets.mesh";
    fs::path sim_config_path = model_root / "sim_config.json";

    // Load sim config
    std::ifstream fin(sim_config_path.string());
    json sim_config;
    fin >> sim_config;
    double YM = sim_config["material_config"]["youngs_modulus"];
    double poisson = sim_config["material_config"]["poissons_ratio"];
    double density = sim_config["material_config"]["density"];

    // Load data
    igl::readMESH(mesh_path.string(), V, T, F);

    Eigen::MatrixXd U;
    igl::readDMAT(reduced_basis_path.string(), U);
    U.transposeInPlace();
    std::cout << U.rows() << " " << U.cols() << std::endl;

    // std::vector<Eigen::VectorXd> reduced_forces = load_forces(training_data_root, reduced_basis_path); // TODO maybe subsample?
    std::vector<Eigen::VectorXd> red_displacements = load_displacements(training_data_root, reduced_basis_path);
    // std::random_shuffle(red_displacements.begin(), red_displacements.end());
    // std::vector<Eigen::VectorXd> less_displacements(red_displacements.begin(), red_displacements.begin() + 100);
    std::vector<Eigen::VectorXd> reduced_forces = get_forces_from_reduced_displacements(red_displacements, V, T, U, YM, poisson, density);

    // *** TODO ***
    // I should test this by summing up the reduced forces at each tet and making sure they are equal to the full reduced force
    // It's also possible that I should be generating the full reduced forces after projecting into, and out of, the reduced space
    // for each training example...

    // ---- Convert it all to the cubacode format
    int n_poses = reduced_forces.size();
    int r = reduced_forces[0].size();
    cout << "T: " << n_poses << endl;
    cout << "r: " << r << endl;
    cout << "r*T: " << (n_poses*r) << endl;
    cout << "Tets: " << T.rows() << endl;

    // I have to construct the *reduced* forces evaluated at each of the total T tets.
    VECTOR trainingForces(r * n_poses);
    for(int i = 0; i < n_poses; i++) {
        for(int j = 0; j < r; j++) {
            trainingForces(i*r + j) = reduced_forces[i][j];
        }
    }

    //What's this? -> It's all the configurations for each training pose
    std::vector<VECTOR> training_poses(red_displacements.size());
    TrainingSet trainingSet;
    for(int i = 0; i < n_poses; i++) {
        eig_to_VEC(red_displacements[i], training_poses[i]);
        trainingSet.push_back(&training_poses[i]);
    }

    // Set up the optimization
    MyGreedyCubop cubop(V, T, U, YM, poisson, density, reduced_forces, red_displacements);

    // Params
    Real relErrTol = get_json_value(model_config["learning_config"]["energy_model_config"], "rel_error_tol", 0.05);//0.05; // What's a good val?
    int maxNumPoints = goal_tet_count * 5; // some sane limit, for overnight runs
    int numCandsPerIter = 200;//100;//T.rows() / 100;  //100;// default 100;  // |C|
    int itersPerFullNNLS = r/2; // r/2 in the paper
    int numSamplesPerSubtrain = 50; //training_poses.size() / 4; // default 50;   // T_s

    std::cout << "relErrTol: " << relErrTol << std::endl;
    std::cout << "maxNumPoints: " << maxNumPoints << std::endl;
    std::cout << "numCandsPerIter: " << numCandsPerIter << std::endl;
    std::cout << "itersPerFullNNLS: " << itersPerFullNNLS << std::endl;
    std::cout << "numSamplesPerSubtrain: " << numSamplesPerSubtrain << std::endl;

    cout << "Working" << endl;

    cubop.run(
        trainingSet,
        trainingForces,
        relErrTol,
        maxNumPoints,
        numCandsPerIter,
        itersPerFullNNLS,
        numSamplesPerSubtrain
    );

    cout << "Didn't reach goal number of tets." << endl;

    return 0;
}


/**
 * Beck Pang 20180424, depreciated
 * Practice Extended Kalman Filter for eight states
 * Fusing an high precision gyroscope and a MPU6500 IMU as the process model,
 *   with an UWB and a magnetometer as the observation model.
 */
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <uwb_msgs/uwb.h>
#include <Eigen/Eigen>
#include <queue>
#include <vector>
#include <cmath>

#define INIT_Q_R_BY_MEASURE

using namespace std;
using namespace Eigen;
ros::Publisher odom_pub;

/**
 * Define states:
 *      x = [pos_x, pos_y, angle_yaw, vel_x, vel_y, bias_gyro, bias_accel_x, bias_accel_y]
 * Define inputs:
 *      u = [angular_vel, accel_x, accel_y], all measurement
 * Define noises:
 *      n = [n_gyro, n_acc_x, n_acc_y, n_bias_gyro, n_bias_acc_x, n_bias_acc_y]
 */
VectorXd x(8);                          // state
MatrixXd P = MatrixXd::Identity(8, 8);  // covariance
MatrixXd Q = MatrixXd::Identity(6, 6);  // prediction noise covariance
MatrixXd R = MatrixXd::Identity(3, 3);  // observation noise covariance

// Buffers to save imu reading and the uwb reading
// for time synchronization
queue<sensor_msgs::Imu::ConstPtr> imu_buf;
queue<nav_msgs::Odometry::ConstPtr> odom_buf;
queue<Matrix<double, 8, 1>>     x_history;
queue<Matrix<double, 8, 8>>     P_history;

double t;       //  previous propagated time

// For initialization
int imu_count = 0;
const int IMU_INIT_COUNT = 30;
Vector3d imu_mean_buf[IMU_INIT_COUNT]; // gyro, accel_x, accel_y
queue<double> odom_mean_buf_x;
queue<double> odom_mean_buf_y;
queue<double> odom_mean_buf_w;
double theta_bias = 0;
bool imu_initialized = false;
bool odom_initialized= false;

// Rotation from the IMU frame to the global frame
Eigen::Matrix3d Rimu;

/**
 * non-linear propagate for EKF
 * with linearization around the current state
 * imu propagate in the world frame
 * @param imu_msg
 */
void propagate(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double cur_t = imu_msg->header.stamp.toSec();
    double w     = imu_msg->angular_velocity.z;
    double ax_raw   = imu_msg->linear_acceleration.x;
    double ay_raw   = imu_msg->linear_acceleration.y;
    Vector3d a_rotate = Vector3d(ax_raw, ay_raw, 0);
    double ax = a_rotate(0);
    double ay = a_rotate(1);

    double dt   = cur_t - t;
//    if (dt <= 0) {  ROS_BREAK; }

    /**
     * f(x, u, n) =
     *      x4,
     *      x5,
     *      w_m - x6 - ng,
     *      cos(x3) (a_mx - x7 - n_ax) + sin(x3) (a_my - x8 - n_ay),
     *      -sin(x3) (a_mx - x7 - n_ax) + cos(x3) (a_my - x8 - n_ay),
     *      n_bg,
     *      n_bax,
     *      n_bay.
     * u_t_hat = u_t-1 + dt * f(u_t-1, u_t, 0)
     */
    x(0) += dt * x(3);
    x(1) += dt * x(4);
    x(2) += dt * (w - x(5));
    x(3) += dt * ( cos(x(2)) * (ax - x(6)) + sin(x(2)) * (ay - x(7)) );
    x(4) += dt * (-sin(x(2)) * (ax - x(6)) + cos(x(2)) * (ay - x(7)) );
//    x(5) += dt * 0;
//    x(6) += dt * 0;
//    x(7) += dt * 0;

    MatrixXd A = MatrixXd::Zero(8, 8);
    A(0, 3) =  1;
    A(1, 4) =  1;
    A(2, 5) = -1;
    // df4 / dx3
    A(3, 2) = -sin(x(2)) * (ax - x(6)) + cos(x(2)) * (ay - x(7));
    // df5 / dx3
    A(4, 2) = -cos(x(2)) * (ax - x(6)) - sin(x(2)) * (ay - x(7));
    A(3, 6) = -cos(x(2));
    A(4, 6) =  sin(x(2));
    A(3, 7) = -sin(x(2));
    A(4, 7) = -cos(x(2));

    MatrixXd U = MatrixXd::Zero(8, 6);
    U(2, 0) = -1;
    U(3, 1) = -cos(x(2));
    U(4, 1) = -sin(x(2));
    U(3, 2) =  sin(x(2));
    U(4, 2) = -cos(x(2));
    U.block<3,3>(5, 3) = MatrixXd::Identity(3, 3);

    MatrixXd F, V;
    F = dt * A + MatrixXd::Identity(8, 8);
    V = dt * U;
    /**
     * P_t_hat = F * P_t * F' + V * Q * V'
     */
    P = F * P * F.transpose() + V * Q * V.transpose();

    t = cur_t;
}

/**
 * linear update for EKF
 * @param msg
 */
void update(const uwb_msgs::uwb &msg)
{
    MatrixXd C = MatrixXd::Zero(3, 8);
    C.block<3, 3>(0, 0) = Matrix3d::Identity();

    double pos_x = msg.pos_x;
    double pos_y = msg.pos_y;
    double angle_yaw = msg.pos_theta;
    Vector3d y(pos_x, pos_y, angle_yaw);

    MatrixXd K(8, 3);
    K = P * C.transpose() * (C * P * C.transpose() + R).inverse();
    x = x + K * (y - C * x);
    P = P - K * C * P;
}

void pub_odom(std_msgs::Header header)
{
    nav_msgs::Odometry odom;
    odom.header.stamp = header.stamp;
    odom.header.frame_id = "world";
    odom.pose.pose.position.x   = x(0);
    odom.pose.pose.position.y   = x(1);
    odom.pose.pose.orientation.z= x(2);
    odom.twist.twist.linear.x   = x(3);
    odom.twist.twist.linear.y   = x(4);
    odom.pose.covariance[0]     = P(0, 0);
    odom.pose.covariance[7]     = P(1, 1);
    odom.pose.covariance[35]    = P(2, 2);
    odom.twist.covariance[0]    = P(3, 3);
    odom.twist.covariance[7]    = P(4, 4);

    odom_pub.publish(odom);
}

void imu_callback(const sensor_msgs::Imu::ConstPtr &imu_msg)
{
//    ROS_INFO("IMU callback, time: %f", imu_msg->header.stamp.toSec());

    if (!imu_initialized && imu_count < IMU_INIT_COUNT)
    {
        // calculate the imu covariance and mean, and initialize the gravity
        imu_mean_buf[imu_count](0) = imu_msg->angular_velocity.z;
        imu_mean_buf[imu_count](1) = imu_msg->linear_acceleration.x;
        imu_mean_buf[imu_count](2) = imu_msg->linear_acceleration.y;
        imu_count++;
    }
    else if (!imu_initialized && imu_count == IMU_INIT_COUNT)
    {
#ifdef INIT_Q_R_BY_MEASURE
        double imu_mean[3] = {0};
        double imu_cova[3]= {0};
        for (int i = 0; i < imu_count; ++i) {
            imu_mean[0] += imu_mean_buf[i](0);
            imu_mean[1] += imu_mean_buf[i](1);
            imu_mean[2] += imu_mean_buf[i](2);
            imu_cova[0] += pow( imu_mean_buf[i](0), 2 );
            imu_cova[1] += pow( imu_mean_buf[i](1), 2 );
            imu_cova[2] += pow( imu_mean_buf[i](2), 2 );
        }
        imu_mean[0] /= imu_count;
        imu_mean[1] /= imu_count;
        imu_mean[2] /= imu_count;
        imu_cova[0] /= imu_count;
        imu_cova[1] /= imu_count;
        imu_cova[2] /= imu_count;
        Q(0, 0) = imu_cova[0] - pow( imu_mean[0], 2 );
        Q(1, 1) = imu_cova[1] - pow( imu_mean[1], 2 );
        Q(2, 2) = imu_cova[2] - pow( imu_mean[2], 2 );
#endif
        imu_initialized = true;
    }
    else {
        imu_buf.push(imu_msg);
        propagate(imu_msg);
        x_history.push(x);
        P_history.push(P);
        pub_odom(imu_msg->header);
    }
}

/**
 * initialize, handle and save uwb messages
 * Also doing time synchronization
 * @param uwb msg
 */
void odom_callback(const uwb_msgs::uwb &msg)
{
//    ROS_INFO("UWB callback, time: %f", msg->header.stamp.toSec());

    if (!imu_initialized && !odom_initialized)
    {
        // calculate the uwb covariance and mean
        odom_mean_buf_x.push(msg.pos_x);
        odom_mean_buf_y.push(msg.pos_y);
        odom_mean_buf_w.push(msg.pos_theta);
    }
    else if (imu_initialized && !odom_initialized)
    {
#ifdef INIT_Q_R_BY_MEASURE
        double odom_mean[3] = {0};
        double odom_cova[3] = {0};
        int odom_count = (int)odom_mean_buf_x.size();
        for (int i = 0; i < odom_count; ++i) {
            double temp_x = odom_mean_buf_x.front();
            double temp_y = odom_mean_buf_y.front();
            double temp_w = odom_mean_buf_w.front();
            odom_mean[0] += temp_x;
            odom_mean[1] += temp_y;
            odom_mean[2] += temp_w;
            odom_cova[0] += pow( temp_x, 2 );
            odom_cova[1] += pow( temp_y, 2 );
            odom_cova[2] += pow( temp_w, 2 );
            odom_mean_buf_x.pop();
            odom_mean_buf_y.pop();
            odom_mean_buf_w.pop();
        }
        odom_mean[0] /= odom_count;
        odom_mean[1] /= odom_count;
        odom_mean[2] /= odom_count;
        odom_cova[0] /= odom_count;
        odom_cova[1] /= odom_count;
        odom_cova[2] /= odom_count;

		// Initialize the position and bias
		x(0) = odom_mean[0];
		x(1) = odom_mean[1];
		x(2) = 0;
		theta_bias = odom_mean[2];

        // Initialize the covariance R
		R(0, 0) = odom_cova[0] - pow( odom_mean[0], 2 );
        R(1, 1) = odom_cova[1] - pow( odom_mean[1], 2 );
        R(2, 2) = odom_cova[2] - pow( odom_mean[2], 2 );
#else
		x(0) = msg.pos_x;
		x(1) = msg.pos_y;
		x(2) = 0;
		theta_bias = msg.pos_theta;
#endif
		t = msg.header.stamp.toSec();
        odom_initialized = true;
    }
    else {
        while (!imu_buf.empty() && imu_buf.front()->header.stamp < msg.header.stamp)
        {
            ROS_INFO("throw state with time: %f", imu_buf.front()->header.stamp.toSec());
            // trace the time backwards to imu time
            t = imu_buf.front()->header.stamp.toSec();
            imu_buf.pop();
            x_history.pop();
            P_history.pop();
        }
        if (!x_history.empty())
        {
            // if x_history is empty then the odom is the same time as the imu
            x = x_history.front();
            P = P_history.front();
            // trace the time backwards to imu time
            t = imu_buf.front()->header.stamp.toSec();
            imu_buf.pop();
            x_history.pop();
            P_history.pop();
        }
        ROS_INFO("update state with time: %f", msg.header.stamp.toSec());
        update(msg);

        // clean the x and P history before new propagate
        while(!x_history.empty()) x_history.pop();
        while(!P_history.empty()) P_history.pop();

        queue<sensor_msgs::Imu::ConstPtr> temp_imu_buf;
        while (!imu_buf.empty())
        {
            ROS_INFO("propagate state with time: %f", imu_buf.front()->header.stamp.toSec());
            propagate(imu_buf.front());
            temp_imu_buf.push(imu_buf.front());
            x_history.push(x);
            P_history.push(P);
            imu_buf.pop();
        }
        std::swap(imu_buf, temp_imu_buf);
    }
}

string imu_topic, uwb_topic, publiser_topic;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ekf_8states");
    ros::NodeHandle n("~");

    ros::Duration(10).sleep(); // sleep for 10 seconds in order to launch both topics

    n.param("imu_topic", imu_topic, string("/dji_sdk/imu"));
    n.param("uwb_topic", uwb_topic, string("/uwb_info"));
    n.param("publiser_topic", publiser_topic, string("/ekf_odom"));
    ros::Subscriber s1 = n.subscribe(imu_topic, 100, imu_callback);
    ros::Subscriber s2 = n.subscribe(uwb_topic, 10, odom_callback);
    odom_pub = n.advertise<nav_msgs::Odometry>(publiser_topic, 100);
    ros::Rate r(100);

    // Rimu = Quaterniond( 0.7071, 0, 0, -0.7071 ).toRotationMatrix( );
    Rimu << 1, 0, 0,
			0, 1, 0,
			0, 0, 1;
	cout << "R_cam" << endl << Rimu << endl;

    odom_initialized = true;

    // gyroscope noise n_g
    Q(0, 0) = 0.0001 * Q(0, 0);
    // accelerometer noise n_ax, n_ay
    Q(1, 1) = 0.01 * Q(1, 1);
    Q(2, 2) = 0.01 * Q(2, 2);
    // gyroscope bias noise n_bg
    Q(3, 3) = 0.000001 * Q(3, 3);
    // accelerometer bias noise n_ba
    Q(4, 4) = 0.0001 * Q(4, 4);
    Q(5, 5) = 0.0001 * Q(5, 5);

    // uwb noise n_x, n_y
    R(0, 0) = 0.001 * R(0, 0);
    R(1, 1) = 0.001 * R(1, 1);
    // magnetometer noise n_theta
    R(2, 2) = 0.01 * R(2, 2);

    ros::spin();
}


#include <iostream>
#include <vector>
#include <list>
#include <cmath>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/for_each.hpp>

#include "fof.hpp"
#include "fof_brute.hpp"

namespace bg = boost::geometry;
namespace bmpl = boost::mpl;
namespace bgi = bg::index;

// Create a D dimensional point from an array of coordinates
template <size_t D>
struct point_setter {
    typedef bg::model::point<double, D, bg::cs::cartesian> point_t;

    point_t& point;
    double *loc;

    point_setter(point_t& point, double *loc) : point(point), loc(loc)
    {}

    template< typename U > void operator()(U i)
    {
        bg::set<i>(point, loc[i]);
    }

};

// Calculate the square of the euclidian distance between two points
template <size_t D>
struct d2_calc {
    typedef bg::model::point<double, D, bg::cs::cartesian> point_t;

    const point_t &p1;
    const point_t &p2;
    double &d2;

    d2_calc(const point_t &p1, const point_t &p2, double &d2) : p1(p1), p2(p2), d2(d2)
    {}

    template< typename U > void operator()(U i)
    {
        d2 += pow( bg::get<i>(p1) - bg::get<i>(p2), 2);
    }
};

// Add a scaler to all the coordinates of a point
template <size_t D>
struct add_scalar_to_point {
    typedef bg::model::point<double, D, bg::cs::cartesian> point_t;

    point_t &p;
    double c;

    add_scalar_to_point(point_t &p, double c) : p(p), c(c)
    {}

    template< typename U > void operator()(U i)
    {
        double new_coord = bg::get<i>(p) + c;
        bg::set<i>(p, new_coord);
    }

};

template <size_t D>
std::vector< std::vector<size_t> >
friends_of_friends_rtree(double *data, size_t npts, double linking_length)
{
    typedef bg::model::point<double, D, bg::cs::cartesian> point_t;
    typedef std::pair<point_t, size_t> value_t;
    using tree_t =  bgi::rtree< value_t, bgi::linear<16> >;
    typedef bmpl::range_c<size_t, 0, D> dim_range;

    std::vector< std::pair<point_t, size_t> > points;
    points.reserve(npts);

    for(size_t i = 0 ; i<npts ; ++i) {
        point_t point;
        bmpl::for_each< dim_range >( point_setter<D>(point, data + i*D) );
        points.push_back(std::make_pair(point, i));
    }

    tree_t tree(points.begin(), points.end());

    std::vector< std::vector< size_t > > groups;

    while( !tree.empty() ) {
        std::vector< value_t > to_add;

        // Grab a point from the tree.
        to_add.push_back( *tree.qbegin( bgi::satisfies([](value_t const &){return true;})) );
        tree.remove( to_add.begin(), to_add.end() );

        for( auto to_add_i = size_t(0) ; to_add_i < to_add.size() ; ++to_add_i ) {
            std::vector< value_t >  added;

            auto it = to_add.begin() + to_add_i;

            // Build box to query
            point_t lower = it->first;
            bmpl::for_each< dim_range >( add_scalar_to_point<D>(lower, -linking_length) );
            point_t upper = it->first;
            bmpl::for_each< dim_range >( add_scalar_to_point<D>(upper, +linking_length));

            bg::model::box< point_t > box( lower, upper );

            auto within_ball = [&it, linking_length](value_t const &v) {
                double d2 = 0.;
                bmpl::for_each< dim_range >( d2_calc<D>(it->first, v.first, d2) );
                return sqrt(d2) < linking_length;
            };

            // Find all points within a linking length of the current point.
            tree.query( bgi::within(box) && bgi::satisfies(within_ball), std::back_inserter(added) );

            // Add the found points to the list so we can find their "friends" as well
            for (auto p: added) {
                to_add.push_back(p);
            }

            // Remove any points we find from the tree as they have been assigned.
            tree.remove( added.begin(), added.end() );

            // Early exit when we have assigned all particles to a group
            if (tree.empty()) {
                break;
            }
        }

        std::vector< size_t > group;
        for( auto p : to_add ) {
            group.push_back(p.second);
        }
        groups.push_back(group);
    }

    return groups;
}

std::vector< std::vector<size_t> >
friends_of_friends(double *data, size_t npts, size_t ndim, double linking_length)
{
    switch(ndim) {
        case 1:
            return friends_of_friends_rtree<1>(data, npts, linking_length);
            break;
        case 2:
            return friends_of_friends_rtree<2>(data, npts, linking_length);
            break;
        case 3:
            return friends_of_friends_rtree<3>(data, npts, linking_length);
            break;
        case 4:
            return friends_of_friends_rtree<4>(data, npts, linking_length);
            break;
        default:
            return friends_of_friends_brute(data, npts, ndim, linking_length);
            break;
    }
}


#include "adaptive.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include "control/util.h"

namespace sailbot {
namespace control {

constexpr float AdaptiveControl::dt;

ControlPhysics::ControlPhysics() {
  constexpr double rhoair = 1.225; // Density of air, kg / m^3
  constexpr double rhowater = 1000.0; // Density of water, kg / m^3
  consts_ = {.sail = {.A = 2.0,
                      .rho = rhoair,
                      .C = 1.4,
                      .K = 1.5,
                      .drageps = 0.0,
                      .maxalpha = M_PI_2},
             .keel = {.A = 0.3,
                      .rho = rhowater,
                      .C = 1.4,
                      .K = 8.0,
                      .drageps = 0.1,
                      .maxalpha = 0.25},
             .rudder = {.A = 0.04,
                        .rho = rhowater,
                        .C = 1.7,
                        .K = 5.0,
                        .drageps = 0.05,
                        .maxalpha = 0.25},
             .hs = 1.5,
             .hk = -0.7,
             .hr = 0.0,
             .rs = 0.1,
             .rk = 0.0,
             .rr = -0.9,
             .ls = 0.35,
             .lr = 0.0,
             .hb = -0.7,
             .m = 25.0,
             .g = 9.8,
             .J = 10.0,
             .Blon = 15.0,
             .Blat = 25.0,
             .Bomega = 500.0,
             .taubias = 0.0};
  // beta = Blon, Bomega, Ar, rs, taubias, 1.0
  betamin_ << 0.0, 0.0, 0.01, -1.0, -10.0, 1.0;
  betamax_ << 1000.0, 10000.0, 1.0, 1.0, 10.0, 1.0;
}

void ControlPhysics::IncrementBeta(const MatrixBeta &diff) {
  MatrixBeta b = beta();
  b += diff;
  // For sundry reasons, Eigen only provides the max/min operators for arrays...
  b = b.array().max(betamin_).min(betamax_).matrix();
  set_beta(b);
}

double ControlPhysics::RudderForTorque(double taus, double tauk, double taugoal,
                                       double heel, double thetac,
                                       double vc) const {
  double cthetac = std::max(std::cos(thetac), 0.5);
  double denom = 0.5 * consts_.rudder.rho * consts_.rudder.A * vc * vc *
                 consts_.rudder.K * std::cos(thetac) * std::cos(heel) *
                 consts_.rr;
  return (taugoal - taus - tauk - consts_.taubias) / denom - thetac;
}

void ControlPhysics::SailAirfoil(double alpha, double v, double *force,
                                 double *ang) const {
  const double absalpha = std::abs(alpha);
  const double signalpha = util::Sign(alpha);
  if (force != nullptr) {
    double trimalpha = std::max(absalpha - 0.05, 0.0);
    const double mag =
        consts_.sail.C * (1.0 - std::exp(-consts_.sail.K * trimalpha));
    *force = 0.5 * consts_.sail.rho * consts_.sail.A * v * v * mag;
  }
  if (ang != nullptr) {
    *ang = (M_PI_2 - std::abs(alpha)) * signalpha;
  }
}

void ControlPhysics::KeelAirfoil(double alpha, double v, double *force,
                                 double *ang) const {
  const double absalpha = std::abs(alpha);
  const double signalpha = util::Sign(alpha);
  if (force != nullptr) {
    const double mag = consts_.keel.K * std::min(absalpha, consts_.keel.maxalpha);
    *force = 0.5 * consts_.keel.rho * consts_.keel.A * v * v * mag;
  }
  if (ang != nullptr) {
    *ang = (M_PI_2 - consts_.keel.drageps) * signalpha;
  }
}

void ControlPhysics::RudderAirfoil(double alpha, double v, double *force,
                                   double *ang) const {
  const double absalpha = std::abs(alpha);
  const double signalpha = util::Sign(alpha);
  if (force != nullptr) {
    const double mag =
        consts_.rudder.K * std::min(absalpha, consts_.rudder.maxalpha);
    *force = 0.5 * consts_.rudder.rho * consts_.rudder.A * v * v * mag;
  }
  if (ang != nullptr) {
    *ang = (M_PI_2 - consts_.rudder.drageps) * signalpha;
  }
}

void ControlPhysics::SailForce(double thetaw, double vw, double deltas,
                               double *Fs, double *gammas) const {
  double alpha = -util::norm_angle(thetaw + deltas + M_PI);
  double ang;
  SailAirfoil(alpha, vw, Fs, &ang);
  if (gammas != nullptr) {
    *gammas = util::norm_angle(ang - thetaw);
  }
}

void ControlPhysics::KeelForce(double thetac, double vc, double *Fk,
                               double *gammak) const {
  double alpha = -util::norm_angle(thetac);
  double ang;
  KeelAirfoil(alpha, vc, Fk, &ang);
  if (gammak != nullptr) {
    *gammak = util::norm_angle(ang - thetac + M_PI);
  }
}

void ControlPhysics::RudderForce(double thetac, double vc, double deltar,
                                 double *Fr, double *gammar) const {
  double alpha = -util::norm_angle(thetac + deltar);
  double ang;
  RudderAirfoil(alpha, vc, Fr, &ang);
  if (gammar != nullptr) {
    *gammar = util::norm_angle(ang - thetac + M_PI);
  }
}

double ControlPhysics::SailTorque(double Fs, double gammas, double deltas,
                                  double heel) const {
  return Fs * ((consts_.rs - consts_.ls * std::cos(deltas)) * std::sin(gammas) *
                   std::cos(heel) +
               consts_.hk * std::cos(gammas) * std::sin(heel));
}

double ControlPhysics::KeelTorque(double Fk, double gammak, double heel) const {
  return Fk * (consts_.rk * std::sin(gammak) * std::cos(heel) +
               consts_.hk * std::cos(gammak) * std::sin(heel));
}

double ControlPhysics::RudderTorque(double Fr, double gammar, double heel) const {
  return Fr * consts_.rr * std::sin(gammar) * std::cos(heel);
}

double ControlPhysics::CalcHeel(double Fs, double gammas, double Fk,
                                double gammak) const {
  double tanheel =
      (Fs * consts_.hs * std::sin(gammas) + Fk * consts_.hk * std::sin(gammak)) /
      (consts_.hb * consts_.m * consts_.g);
  return std::atan(tanheel);
}

void ControlPhysics::NetForce(double thetaw, double vw, double thetac,
                              double vc, double deltas, double deltar,
                              double omega, double *Flon, double *Flat,
                              double *taunet, double *newheel,
                              MatrixY *Y) const {
  double Fs, gammas, Fk, gammak, Fr, gammar;
  SailForce(thetaw, vw, deltas, &Fs, &gammas);
  KeelForce(thetac, vc, &Fk, &gammak);
  RudderForce(thetac, vc, deltar, &Fr, &gammar);
  double heel = CalcHeel(Fs, gammas, Fk, gammak);
  if (newheel != nullptr) *newheel = heel;
  double taus = SailTorque(Fs, gammas, deltas, heel);
  double tauk = KeelTorque(Fk, gammak, heel);
  double taur = RudderTorque(Fr, gammar, heel);

  double YFlonBlon = -vc * std::abs(vc) * std::cos(thetac);
  double YFlonAr = Fr * std::cos(gammar) / consts_.rudder.A;
  double YFlonconst = Fs * std::cos(gammas) + Fk * std::cos(gammak);
  Eigen::Matrix<double, 1, 6> YFlon;
  YFlon << YFlonBlon, 0.0, YFlonAr, 0.0, 0.0, YFlonconst;

  if (Flon != nullptr) *Flon = YFlon * beta();
  if (Flat != nullptr) {
    double FBlat = consts_.Blat * vc * std::sin(thetac);
    *Flat =
        (Fs * std::sin(gammas) + Fk * std::sin(gammak) + Fr * std::sin(gammar)) *
        std::cos(heel) + FBlat;
  }

  double YtauBomega = -omega * std::abs(omega);
  double YtauAr = taur / consts_.rudder.A;
  double Ytaurs = Fs * std::sin(gammas) * std::cos(heel);
  double Ytauconst = tauk + (taus - Ytaurs * consts_.rs);
  Eigen::Matrix<double, 1, 6> Ytau;
  Ytau << 0.0, YtauBomega, YtauAr, Ytaurs, 1.0, Ytauconst;

  if (taunet != nullptr) *taunet = Ytau * beta();

  if (Y != nullptr) {
    Y->row(0) = YFlon;
    Y->row(1) = Ytau;
  }
}

bool ControlPhysics::GlobalMaxForceForTorque(double thetaw, double vw,
                                             double thetac, double vc,
                                             double taug, Constraint constraint,
                                             int nsteps, double *deltas,
                                             double *deltar) const {
  CHECK_NOTNULL(deltas);
  CHECK_NOTNULL(deltar);
  double minds, maxds;
  {
    double maxsail = std::abs(util::norm_angle(M_PI - thetaw));
    double minsail = std::max(maxsail - M_PI_2, 0.0);
    maxsail = std::min(maxsail, M_PI_2);
    minds = thetaw > 0.0 ? minsail : -maxsail;
    maxds = thetaw < 0.0 ? -minsail : maxsail;
  }
  double mindr = util::Clip(-consts().rudder.maxalpha - thetac, -0.5, 0.);
  double maxdr = util::Clip(consts().rudder.maxalpha - thetac, 0., 0.5);

  double mincost = std::numeric_limits<double>::infinity();
  bool success = false;
  double deltadeltas = (maxds - minds) / std::max(1, nsteps - 1);

  double taus, tauk, taur, taue, heel;
  for (int ii = 0; ii < nsteps; ++ii) {
    double trialds = minds + (double)ii * deltadeltas;
    double Fs, gammas, Fk, gammak, Fr, gammar;
    SailForce(thetaw, vw, trialds, &Fs, &gammas);
    KeelForce(thetac, vc, &Fk, &gammak);
    heel = CalcHeel(Fs, gammas, Fk, gammak);
    taus = SailTorque(Fs, gammas, trialds, heel);
    tauk = KeelTorque(Fk, gammak, heel);
    double trialdr = RudderForTorque(taus, tauk, taug, heel, thetac, vc);
    switch (constraint) {
      case kQuadratic:
        trialdr = ClipRudder(trialdr, thetac);
        break;
      // For kStarboard and kPort, trialdr may end up outside of the allowable
      // bounds, in which case this particular iteration of the for loop has
      // produced an invalid result.
      case kStarboard:
        trialdr = std::max(trialdr, maxdr);
        break;
      case kPort:
        trialdr = std::min(trialdr, mindr);
        break;
    }

    if (ClipRudder(trialdr, thetac) != trialdr) {
      // Invalid rudder result.
      continue;
    }

    success = true;

    RudderForce(thetac, vc, trialdr, &Fr, &gammar);
    taur = RudderTorque(Fr, gammar, heel);
    double Flon =
        Fs * std::cos(gammas) + Fk * std::cos(gammak) + Fr * std::cos(gammar);

    taue = taus + tauk + taur + consts_.taubias;

    double cost = -Qf * Flon;
    switch (constraint) {
      case kQuadratic:
        cost += Qtaueq * (taue - taug) * (taue - taug);
        break;
      case kStarboard:
        cost += Qtaumax * taue;
        break;
      case kPort:
        cost += -Qtaumax * taue;
        break;
    }

    if (cost < mincost) {
      mincost = cost;
      *deltas = trialds;
      *deltar = trialdr;
    }
  }

  return success;
}

AdaptiveControl::AdaptiveControl()
    : Node(dt),
      sail_msg_(AllocateMessage<msg::SailCmd>()),
      rudder_msg_(AllocateMessage<msg::RudderCmd>()),
      boat_state_(AllocateMessage<msg::BoatState>()),
      consts_msg_(AllocateMessage<msg::ControllerConstants>()),
      heading_(2.0 * M_PI / 4.0),
      sail_cmd_("sail_cmd", true),
      rudder_cmd_("rudder_cmd", true),
      consts_queue_("control_consts", true) {

    consts_msg_->set_winch_kp(13);
    consts_msg_->set_rudder_kp(25.0);
    consts_msg_->set_qf(1.0);
    {
      std::unique_lock<std::mutex> l(consts_mutex_);
      consts_queue_.send(consts_msg_);
    }

  Kbeta.diagonal() << 0.0, 0.0, 0.0, 0.004, 0.0, 0.;
  Lambda << 1.0, 0.0,
            0.0, 1.0;
  Kref = 0.99;
  Kmax_exp_vel = 0.2;
  Kmax_exp_acc = 0.2;

  RegisterHandler<msg::BoatState>("boat_state", [this](const msg::BoatState &msg) {
    std::unique_lock<std::mutex> l(boat_state_mutex_);
    *boat_state_ = msg;
    double vx = boat_state_->vel().x();
    double vy = boat_state_->vel().y();
    yaw_ = boat_state_->euler().yaw();
    omega_ = boat_state_->omega().z();
   // double heel = boat_state_->euler().roll();
    thetac_ = -util::norm_angle(std::atan2(vy, vx) - yaw_);
    thetac_ = 0.0;
    vc_ = std::sqrt(vx * vx + vy * vy);
  });
  RegisterHandler<msg::Vector3f>("wind", [this](const msg::Vector3f &msg) {
    double wx = msg.x();
    double wy = msg.y();
    thetaw_ = util::norm_angle(std::atan2(-wy, wx));
    // TODO(james): Account for wind gradient properly, rather than by multiplying
    // by a hard-coded constant.
    vw_ = std::sqrt(wy * wy + wx * wx) * 1.6;
  });
  RegisterHandler<msg::HeadingCmd>("heading_cmd", [this](const msg::HeadingCmd &msg) {
    if (msg.has_heading()) {
      heading_ = msg.heading();
    }
  });
  RegisterHandler<msg::ControllerConstants>(
      "control_consts", [this](const msg::ControllerConstants &msg) {
    std::unique_lock<std::mutex> l(consts_mutex_);
    if (msg.has_qf()) {
      *consts_msg_ = msg;
      physics_.Qf = msg.qf();
    }
  });
  RegisterHandler<msg::SBUS>("sbus_value", [this](const msg::SBUS &sbus) {
    std::unique_lock<std::mutex> l(boat_state_mutex_);
  });
}

/**
 * Computes the betadot given a particular deltas/deltar, using a method
 * described in section 8.5.4 Adaptive Control of "Robotics: Modelling, Planning
 * and Control", in which we presume that the dynamics are linear in some
 * parameters, such that under a given set of conditions the dynamics
 * are described by u = Y * beta, where Y is a (potentially nonlinear) function
 * of the current state.
 * For our purposes, u shall notionally comprise of the forwards (longitudinal)
 * force and the net yaw torque.
 * In order to update our estimate of the current beta, we look at the
 * error in the states that most closely correspond with u (in this case,
 * current velocity and current yaw) and update beta based on a PD-like
 * system, scaling by the current values of Y (so that the parameter
 * that most affect the current error are the most updated).
 * Currently, I assume there is always zero velocity error, as I do not
 * actually *care* much about the velocity. However, ignoring it is not
 * necessarily ideal. TODO(james): Investigate
 */
ControlPhysics::MatrixBeta AdaptiveControl::Adaptor(double deltas,
                                                    double deltar) const {
  ControlPhysics::MatrixY Y;
  physics_.NetForce(thetaw_, vw_, thetac_, vc_, deltas, deltar, /*omega=*/0.0,
                    nullptr, nullptr, nullptr, nullptr, &Y);
  // See above; 0s should be accel/velocity errors.
  Eigen::Vector2d sigma =
      Eigen::Vector2d(0.0, omega_ref_ - omega_) +
      Lambda * Eigen::Vector2d(0.0, util::norm_angle(yaw_ref_ - yaw_));
  ControlPhysics::MatrixBeta betadot = -Kbeta * Y.transpose() * sigma;
  return betadot;
}

void AdaptiveControl::UpdateYawRef() {
  // Perform weighted averages
  yaw_ref_ = util::norm_angle(Kref * util::norm_angle(yaw_ref_ - yaw_) + yaw_);
  omega_ref_ = Kref * omega_ref_ + (1.0 - Kref) * omega_;

  // Calculate expected velocity
  double exp_vel = util::Clip(util::norm_angle(heading_ - yaw_ref_),
                              -Kmax_exp_vel, Kmax_exp_vel);
  // Calculate expected accel
  double exp_acc = util::Clip(util::norm_angle(exp_vel - omega_ref_),
                              -Kmax_exp_acc, Kmax_exp_acc);
  // Perform integration, nothing fancy:
  if (Kmax_exp_vel < 0.0) {
    yaw_ref_ = heading_.load();
  } else if (Kmax_exp_acc < 0.0) {
    yaw_ref_ = yaw_ref_ + exp_vel * dt;
  } else {
    yaw_ref_ = yaw_ref_ + omega_ref_ * dt + 0.5 * exp_acc * dt * dt;
    omega_ref_ = omega_ref_ + exp_acc * dt;
  }
  yaw_ref_ = util::norm_angle(yaw_ref_);
}

bool AdaptiveControl::Controller(double *deltas, double *deltar) {
  CHECK_NOTNULL(deltas);
  CHECK_NOTNULL(deltar);
  double taue = consts_msg_->rudder_kp() * util::norm_angle(heading_ - yaw_) -
                15.0 * omega_;
  ControlPhysics::Constraint constraint = ControlPhysics::kQuadratic;
//  constraint = ControlPhysics::kPort;
//  taue = -100.0;
  if (!physics_.GlobalMaxForceForTorque(thetaw_, vw_, thetac_, vc_, taue,
                                        constraint,
                                        /*nsteps=*/20, deltas, deltar)) {
    return false;
  }
  ControlPhysics::MatrixBeta betadot = Adaptor(*deltas, *deltar);
  physics_.IncrementBeta(betadot * dt);
  UpdateYawRef();
  return true;
}

void AdaptiveControl::Iterate() {
  std::unique_lock<std::mutex> l(boat_state_mutex_);
  std::unique_lock<std::mutex> lc(consts_mutex_);

  double deltas, deltar;
  if (!Controller(&deltas, &deltar)) {
    deltas = 0.0;
    deltar = 0.0;
  }
  // We can't control sign of deltas ;(
  deltas = std::abs(deltas);

  double cursail = boat_state_->internal().sail();
  double sail_err = util::norm_angle(deltas - cursail);
  sail_msg_->set_voltage(consts_msg_->winch_kp() * sail_err /
                         std::sqrt(std::abs(sail_err)));
  sail_msg_->set_pos(deltas);

  rudder_msg_->set_pos(util::Clip(deltar, -0.5, 0.5));

  sail_cmd_.send(sail_msg_);
  rudder_cmd_.send(rudder_msg_);
  consts_queue_.send(consts_msg_);
}

}  // control
}  // sailbot


// BLAS level 3
// symmetric matrices, syr2k

#include <stddef.h>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;

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

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

typedef ublas::matrix<real_t, ublas::column_major> cm_t;
typedef ublas::matrix<real_t, ublas::row_major> rm_t;
typedef ublas::symmetric_adaptor<cm_t, ublas::upper> ucsa_t;
typedef ublas::symmetric_adaptor<cm_t, ublas::lower> lcsa_t;
typedef ublas::symmetric_adaptor<rm_t, ublas::upper> ursa_t;
typedef ublas::symmetric_adaptor<rm_t, ublas::lower> lrsa_t;

typedef ublas::matrix<cmplx_t, ublas::column_major> ccm_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> crm_t;
typedef ublas::symmetric_adaptor<ccm_t, ublas::upper> cucsa_t;
typedef ublas::symmetric_adaptor<ccm_t, ublas::lower> clcsa_t;
typedef ublas::symmetric_adaptor<crm_t, ublas::upper> cursa_t;
typedef ublas::symmetric_adaptor<crm_t, ublas::lower> clrsa_t;

int main (int argc, char **argv) {
  int n = 0, k = 0;
  if (argc > 1) {
    n = atoi(argv [1]);
  }
  if (argc > 2) {
    k = atoi(argv [2]);
  }

  if (n <= 0) {
    cout << "n -> ";
    cin >> n;
  }
  if (k <= 0) {
    cout << "k -> ";
    cin >> k;
  }

  cm_t ac (n, k);
  rm_t ar (n, k);
  init_m (ac, rws1());
  init_m (ar, rws1());
  print_m (ac, "ac");
  cout << endl;
  print_m (ar, "ar");
  cout << endl << endl;

  cm_t bc (n, k);
  rm_t br (n, k);
  init_m (bc, const_val<real_t> (1));
  init_m (br, const_val<real_t> (1));
  print_m (bc, "bc");
  cout << endl;
  print_m (br, "br");
  cout << endl << endl;

  cm_t cmu (n, n);
  cm_t cml (n, n);
  rm_t rmu (n, n);
  rm_t rml (n, n);
  ucsa_t ucsa (cmu);
  lcsa_t lcsa (cml);
  ursa_t ursa (rmu);
  lrsa_t lrsa (rml);

  blas::syr2k (1.0, ac, bc, 0.0, ucsa);
  blas::syr2k (1.0, ac, bc, 0.0, lcsa);
  blas::syr2k (1.0, ar, br, 0.0, ursa);
  blas::syr2k (1.0, ar, br, 0.0, lrsa);

  print_m (ucsa, "ucsa");
  cout << endl;
  print_m (lcsa, "lcsa");
  cout << endl;
  print_m (ursa, "ursa");
  cout << endl;
  print_m (lrsa, "lrsa");
  cout << endl << endl;

  // part 2

  cm_t act (k, n);
  rm_t art (k, n);
  init_m (act, cls1());
  init_m (art, cls1());
  print_m (act, "act");
  cout << endl;
  print_m (art, "art");
  cout << endl << endl;

  cm_t bct (k, n);
  rm_t brt (k, n);
  init_m (bct, rws1());
  init_m (brt, rws1());
  print_m (bct, "bct");
  cout << endl;
  print_m (brt, "brt");
  cout << endl << endl;

  init_m (cmu, const_val<real_t> (0));
  init_m (cml, const_val<real_t> (0));
  init_m (rmu, const_val<real_t> (0));
  init_m (rml, const_val<real_t> (0));

  blas::syr2k (1.0, bindings::trans(act), bct, 0.0, bindings::upper(cmu));
  blas::syr2k (1.0, bindings::trans(act), bct, 0.0, bindings::lower(cml));
  blas::syr2k (1.0, bindings::trans(art), brt, 0.0, bindings::upper(rmu));
  blas::syr2k (1.0, bindings::trans(art), brt, 0.0, bindings::lower(rml));

  print_m (cmu, "cmu");
  cout << endl;
  print_m (cml, "cml");
  cout << endl;
  print_m (rmu, "rmu");
  cout << endl;
  print_m (rml, "rml");
  cout << endl;

  // complex

  const int n1 = 3;
  const int k1 = 2;

  ccm_t cac (n1, k1);
  crm_t car (n1, k1);
  cac(0,0) = car(0,0) = cmplx_t (1., 1.);
  cac(1,0) = car(1,0) = cmplx_t (2., 1.);
  cac(2,0) = car(2,0) = cmplx_t (3., 1.);
  cac(0,1) = car(0,1) = cmplx_t (1., 1.);
  cac(1,1) = car(1,1) = cmplx_t (2., 1.);
  cac(2,1) = car(2,1) = cmplx_t (3., 1.);
  print_m (cac, "cac");
  cout << endl;
  print_m (car, "car");
  cout << endl << endl;

  ccm_t cbc (n1, k1);
  crm_t cbr (n1, k1);
  cbc(0,0) = cbr(0,0) = cmplx_t (0., -1.);
  cbc(1,0) = cbr(1,0) = cmplx_t (0., -1.);
  cbc(2,0) = cbr(2,0) = cmplx_t (0., -1.);
  cbc(0,1) = cbr(0,1) = cmplx_t (0., -1.);
  cbc(1,1) = cbr(1,1) = cmplx_t (0., -1.);
  cbc(2,1) = cbr(2,1) = cmplx_t (0., -1.);
  print_m (cbc, "cbc");
  cout << endl;
  print_m (cbr, "cbr");
  cout << endl << endl;

  ccm_t ccmu (n1, n1);
  ccm_t ccml (n1, n1);
  crm_t crmu (n1, n1);
  crm_t crml (n1, n1);
  cucsa_t cucsa (ccmu);
  clcsa_t clcsa (ccml);
  cursa_t cursa (crmu);
  clrsa_t clrsa (crml);

  blas::syr2k ( 1.0, cac, cbc, 0.0, cucsa);
  blas::syr2k (cmplx_t(1,0), cac, cbc, cmplx_t(0,0), clcsa);
  blas::syr2k (cmplx_t(1,0), car, cbr, cmplx_t(0,0), cursa);
  blas::syr2k (1.0, car, cbr, 0.0, clrsa);

  print_m (cucsa, "cucsa");
  cout << endl;
  print_m (clcsa, "clcsa");
  cout << endl;
  print_m (cursa, "cursa");
  cout << endl;
  print_m (clrsa, "clrsa");
  cout << endl << endl;

}


#ifndef _jsc_util_interval_hpp_included_
#define _jsc_util_interval_hpp_included_

#include <boost/config.hpp>

#include <math.h>

#include <iostream>
#include <map>
#include <set>
#include <sstream>
#include <vector>

#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tokenizer.hpp>

#include <jsc/util/log.hpp>

using namespace std;
using namespace boost;
using namespace boost::lambda;

namespace jsc
{
namespace util
{

//! A function which checks whether two intervals overlap.
/*!
 * \return whether [a, b) and [c, d) overlap.
 */
template<class T, class Compare>
bool interval_overlap(T const & a, T const & b, T const & c, T const & d, Compare comp = Compare())
{
	return (!(comp(b, c) || comp(d, a)));
}

//! A function which computes the overlapping length of two intervals.
/*!
 * \return the overlapping length of [a, b) and [c, d).
 */
template<class T, class Compare>
double compute_interval_overlap(T const & a, T const & b, T const & c, T const & d, Compare comp = Compare())
{
	double length = 0;
	if (!(comp(b, c) || comp(d, a)))
	{
		if (comp(a, c))
		{
			if (comp(b, d))
			{
				length = b - c;
			}
			else
			{
				length = d - c;
			}
		}
		else
		{
			if (comp(b, d))
			{
				length = b - a;
			}
			else
			{
				length = d - a;
			}
		}
	}
	return length;
}

//! A class representing a list of intervals.
/*!
 * The intervals in the list are sorted according to their start positions and do not overlap with each other.
 */
template <class T, class Compare = less<T> >
class interval_list
{
private:
	//! The comparator.
	Compare comp;
	//! The internal representation of the list of intervals: [starts[0], end[0]), [starts[1], end[1]), ...
	vector<T> starts;
	//! The internal representation of the list of intervals: [starts[0], end[0]), [starts[1], end[1]), ...
	vector<T> ends;

public:
	//! A constructor.
	interval_list()
		: comp(Compare())
	{
	}

	//! A constructor taking an explicit comparator.
	explicit
	interval_list(Compare const & c)
		: comp(c)
	{
	}

	//! Copy constructor.
	interval_list(interval_list<T, Compare> const & il)
		: comp(il.comp), starts(il.starts), ends(il.ends)
	{
	}

	//! Gets the number of intervals.
	unsigned long get_num_intervals() const
	{
		return starts.size();
	}

	//! Gets the starts vector.
	vector<T> const & get_starts() const
	{
		return starts;
	}

	//! Gets the ends vector.
	vector<T> const & get_ends() const
	{
		return ends;
	}

	//! Counts the number of intervals in the list that overlap with the given interval.
	/*!
	 * \sa count_overlap_il().
	 */
	long count_overlap(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return 0;	// always return 0 in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		long total_count = 0;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			if (interval_overlap<T, Compare>(starts[i], ends[i], start, end, comp))
			{
				total_count++;
			}
		}

		return total_count;
	}

	//! Counts the number of intervals in the list that overlap with the given interval list.
	/*!
	 * \sa count_overlap().
	 */
	long count_overlap_il(interval_list<T, Compare> const & il) const
	{
		long total_count = 0;
		int n = starts.size();

		for (int i = 0; i < n; ++i)
		{
			if (il.check_overlap(starts[i], ends[i]))
			{
				total_count++;
			}
		}
		return total_count;
	}

	//! Checks whether the interval list overlaps with the interval [start, end).
	bool check_overlap(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return false;	// always return false in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			if (interval_overlap<T, Compare>(starts[i], ends[i], start, end, comp))
			{
				return true;
			}
		}

		return false;
	}

	static interval_list<T> get_interval_overlap(T const & a, T const & b, T const & c, T const & d, Compare comp = Compare())
	{
		interval_list<T> il;
		if (!(comp(b, c) || comp(d, a)))
		{
			if (comp(a, c))
			{
				if (comp(b, d))
				{
					il.add_interval(c, b);
				}
				else
				{
					il.add_interval(c, d);
				}
			}
			else
			{
				if (comp(b, d))
				{
					il.add_interval(a, b);
				}
				else
				{
					il.add_interval(a, d);
				}
			}
		}
		return il;
	}

	//! Gets the overlap of the interval list with the interval [start, end).
	interval_list<T> get_overlap_il(T const & start, T const & end) const
	{
		interval_list<T> il;
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return il;	// always return 0 in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			il.add_interval_list(
					get_interval_overlap(starts[i], ends[i], start, end, comp));
		}

		return il;
	}

	//! Get the overlap of the interval list with another interval list.
	interval_list<T> get_overlap_il(interval_list<T, Compare> const & il) const
	{
		interval_list<T> rst_il;
		int n = il.starts.size();
		for (int i = 0; i < n; i++)
		{
			rst_il.add_interval_list(get_overlap_il(il.starts[i], il.ends[i]));
		}
		return rst_il;
	}

	//! Computes the overlap of the interval list with the interval [start, end).
	/*!
	 * \sa compute_overlap_il().
	 */
	double compute_overlap(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return 0;	// always return 0 in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		double total_length = 0;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			total_length += compute_interval_overlap<T, Compare>(starts[i], ends[i], start, end, comp);
		}

		return total_length;
	}

	bool check_single_overlap(T const & start, T const & end, double const & threshold) const
	{
		bool found = false;
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return false;	// always return false in this case
		}

		typename vector<T>::const_iterator s_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		long s_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		long i;
		for (i = max((long)0, s_starts_idx - 1); i <= min((long)starts.size() - 1, e_ends_idx); ++i)
		{
			if (threshold <= compute_interval_overlap<T, Compare>(starts[i], ends[i], start, end, comp)) {
				found = true;
				break;
			}
		}

		return found;
	}

	//! Computes the overlap of the interval list with another interval list.
	/*!
	 * \sa compute_overlap().
	 */
	double compute_overlap_il(interval_list<T, Compare> const & il) const
	{
		double total_length = 0;
		int n = il.starts.size();
		for (int i = 0; i < n; i++)
		{
			total_length += compute_overlap(il.starts[i], il.ends[i]);
		}
		return total_length;
	}

	//! Computes the total length of the intervals in this interval list.
	double compute_total_length() const
	{
		double total_length = 0;
		int n = starts.size();
		for (int i = 0; i < n; i++)
		{
			total_length += ends[i] - starts[i];
		}
		return total_length;
	}

	//! Finds in the interval list the index of the interval which contains the given interval [start, end).
	/*!
	 * \return the index of the interval containing [start, end); otherwise starts.size().
	 * \sa contains_interval(T const & start, T const & end)
	 */
	long idx_containing_interval(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return 0;	// always return 0 in this case
		}

		typename vector<T>::const_iterator s_starts_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);

		long s_starts_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		if (s_starts_idx >= 0 && s_starts_idx < (long)starts.size() &&
				!comp(start, starts[s_starts_idx]) &&
				!comp(ends[s_starts_idx], end))
		{
			return s_starts_idx;
		}
		if (s_starts_idx - 1 >= 0 && s_starts_idx - 1 < (long)starts.size() &&
				!comp(start, starts[s_starts_idx - 1]) &&
				!comp(ends[s_starts_idx - 1], end))
		{
			return s_starts_idx - 1;
		}

		return starts.size();
	}

	//! Checks whether the interval list contains [start, end).
	/*!
	 * \sa idx_containing_interval()
	 */
	bool contains_interval(T const & start, T const & end) const
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return true;
		}

		typename vector<T>::const_iterator s_starts_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);

		long s_starts_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		if (s_starts_idx >= 0 && s_starts_idx < (long)starts.size() &&
				!comp(start, starts[s_starts_idx]) &&
				!comp(ends[s_starts_idx], end))
		{
			return true;
		}
		if (s_starts_idx - 1 >= 0 && s_starts_idx - 1 < (long)starts.size() &&
				!comp(start, starts[s_starts_idx - 1]) &&
				!comp(ends[s_starts_idx - 1], end))
		{
			return true;
		}

		return false;
	}

	//! Fill in gaps if their length is less than the given threshold
	void fill_in_gaps(double const & threshold)
	{
		int n = starts.size();
		if (n == 0)
		{
			return;
		}

		vector<T> new_starts, new_ends;
		T last_end;
		new_starts.push_back(starts[0]);
		last_end = ends[0];
		for (int i = 1; i < n; ++i)
		{
			if (starts[i] - last_end < threshold)
			{
				// fill in the gap
			}
			else
			{
				// copy the last end and current start
				new_ends.push_back(last_end);
				new_starts.push_back(starts[i]);
			}
			// update last end
			last_end = ends[i];
		}
		new_ends.push_back(last_end);
		starts = new_starts;
		ends = new_ends;
	}

	//! Add [start, end) to the interval list.
	/*!
	 * Overlapping intervals in the list will be merged.
	 * \sa add_starts_sizes() and add_interval_list()
	 */
	interval_list<T> & add_interval(T const & start, T const & end)
	{
		if (!comp(start, end))	/* [start, end) == nil */
		{
			return *this;
		}

		typename vector<T>::iterator s_starts_itr, s_ends_itr, e_starts_itr, e_ends_itr;
		s_starts_itr = lower_bound(starts.begin(), starts.end(), start);
		s_ends_itr = lower_bound(ends.begin(), ends.end(), start);
		e_starts_itr = lower_bound(starts.begin(), starts.end(), end);
		e_ends_itr = lower_bound(ends.begin(), ends.end(), end);

		int s_starts_idx, s_ends_idx, e_starts_idx, e_ends_idx;
		s_starts_idx = distance(starts.begin(), s_starts_itr);
		s_ends_idx = distance(ends.begin(), s_ends_itr);
		e_starts_idx = distance(starts.begin(), e_starts_itr);
		e_ends_idx = distance(ends.begin(), e_ends_itr);
		bool s_on_interval = (s_starts_idx - s_ends_idx == 1);
		bool e_on_interval = (e_starts_idx - e_ends_idx == 1);

		typename vector<T>::iterator starts_itr = starts.erase(s_starts_itr, e_starts_itr);
		typename vector<T>::iterator ends_itr = ends.erase(s_ends_itr, e_ends_itr);
		if (s_on_interval && e_on_interval)
		{
		}
		else if (!s_on_interval && !e_on_interval)
		{
			starts.insert(starts_itr, start);
			ends.insert(ends_itr, end);
		}
		else if (s_on_interval && !e_on_interval)
		{
			ends.insert(ends_itr, end);
		}
		else
		{
			starts.insert(starts_itr, start);
		}

		return *this;
	}

	//! Add another interval list to the interval list.
	/*!
	 * \sa add_starts_sizes() and add_interval()
	 */
	void add_interval_list(interval_list<T, Compare> const & il)
	{
		int n = il.starts.size();
		for (int i = 0; i < n; i++)
		{
			add_interval(il.starts[i], il.ends[i]);
		}
	}

	//! Add another interval list to the interval list.
	/*!
	 * \param starts the starts vector of the interval list to be added.
	 * \param sizes  the sizes vector of the interval list to be added.
	 * \sa add_interval_list() and add_interval()
	 */
	void add_starts_sizes(vector<T> const & starts, vector<T> const & sizes)
	{
		int n = starts.size();
		for (int i = 0; i < n; i++)
		{
			add_interval(starts[i], starts[i] + sizes[i]);
		}
	}

	//! Add another interval list to the interval list.
	/*!
	 * \param starts the starts vector of the interval list to be added.
	 * \param ends the ends vector of the interval list to be added.
	 * \sa add_interval_list() and add_interval()
	 */
	void add_starts_ends(vector<T> const & starts, vector<T> const & ends)
	{
		int n = starts.size();
		for (int i = 0; i < n; i++)
		{
			add_interval(starts[i], ends[i]);
		}
	}

	//! Checks whether the total coverage of the interval list overlaps with that of another interval list.
	/*!
	 * coverage = [min(starts), (max(ends))
	 */
	bool coverage_overlap(interval_list<T, Compare> const & il2)
	{
		int n1 = starts.size();
		int n2 = il2.starts.size();
		if (n1 == 0 || n2 == 0)
		{
			return false;
		}
		return interval_overlap<T, Compare>(starts[0], ends[n1 - 1], il2.starts[0], il2.ends[n2 - 1], comp);
	}

	T find_min_uncovered(T start_from) const {
		if (get_num_intervals() == 0) {
			return start_from;
		} else {
			typename vector<T>::const_iterator s_starts_itr;
			s_starts_itr = lower_bound(starts.begin(), starts.end(), start_from);

			long s_starts_idx;
			s_starts_idx = distance(starts.begin(), s_starts_itr);
			long i;
			for (i = max((long)0, (long)s_starts_idx - 1);
					i < (long)get_num_intervals(); ++i)
			{
				if (starts[i] > start_from) {
					return start_from;
				} else if (ends[i] > start_from) {
					start_from = ends[i];
				} else {
					continue;
				}
			}
			return start_from;
		}
	}
};

//! Default printing of an interval_list.
template<class T, class Compare>
ostream & operator << (ostream& os, interval_list<T, Compare> const & il)
{
	int n = il.get_starts().size();
	for (int i = 0; i < n; i++)
	{
		os << "[" << il.get_starts()[i] << "," << il.get_ends()[i] << "), ";
	}
	return os;
}

} /* end of util */

} /* end of jsc */

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>, Olga Diamanti <olga.diam@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "comb_cross_field.h"

#include <vector>
#include <deque>
#include <Eigen/Geometry>
#include "per_face_normals.h"
#include "is_border_vertex.h"
#include "rotation_matrix_from_directions.h"

#include "triangle_triangle_adjacency.h"

namespace igl {
  template <typename DerivedV, typename DerivedF>
  class Comb
  {
  public:

    const Eigen::PlainObjectBase<DerivedV> &V;
    const Eigen::PlainObjectBase<DerivedF> &F;
    const Eigen::PlainObjectBase<DerivedV> &PD1;
    const Eigen::PlainObjectBase<DerivedV> &PD2;
    Eigen::PlainObjectBase<DerivedV> N;

  private:
    // internal
    Eigen::PlainObjectBase<DerivedF> TT;
    Eigen::PlainObjectBase<DerivedF> TTi;

  private:

    static inline double Sign(double a){return (double)((a>0)?+1:-1);}

  private:

    // returns the 90 deg rotation of a (around n) most similar to target b
    /// a and b should be in the same plane orthogonal to N
    static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_new(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a,
                                                                   const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b,
                                                                   const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& n)
    {
      Eigen::Matrix<typename DerivedV::Scalar, 3, 1> c = (a.cross(n)).normalized();
      typename DerivedV::Scalar scorea = a.dot(b);
      typename DerivedV::Scalar scorec = c.dot(b);
      if (fabs(scorea)>=fabs(scorec))
        return a*Sign(scorea);
      else
        return c*Sign(scorec);
    }

  public:
    inline Comb(const Eigen::PlainObjectBase<DerivedV> &_V,
         const Eigen::PlainObjectBase<DerivedF> &_F,
         const Eigen::PlainObjectBase<DerivedV> &_PD1,
         const Eigen::PlainObjectBase<DerivedV> &_PD2
         ):
    V(_V),
    F(_F),
    PD1(_PD1),
    PD2(_PD2)
    {
      igl::per_face_normals(V,F,N);
      igl::triangle_triangle_adjacency(V,F,TT,TTi);
    }
    inline void comb(Eigen::PlainObjectBase<DerivedV> &PD1out,
              Eigen::PlainObjectBase<DerivedV> &PD2out)
    {
//      PD1out = PD1;
//      PD2out = PD2;
      PD1out.setZero(F.rows(),3);PD1out<<PD1;
      PD2out.setZero(F.rows(),3);PD2out<<PD2;

      Eigen::VectorXi mark = Eigen::VectorXi::Constant(F.rows(),false);

      std::deque<int> d;

      d.push_back(0);
      mark(0) = true;

      while (!d.empty())
      {
        int f0 = d.at(0);
        d.pop_front();
        for (int k=0; k<3; k++)
        {
          int f1 = TT(f0,k);
          if (f1==-1) continue;
          if (mark(f1)) continue;

          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> dir0    = PD1out.row(f0);
          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> dir1    = PD1out.row(f1);
          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> n0    = N.row(f0);
          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> n1    = N.row(f1);

          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> dir0Rot = igl::rotation_matrix_from_directions(n0, n1)*dir0;
          dir0Rot.normalize();
          Eigen::Matrix<typename DerivedV::Scalar, 3, 1> targD   = K_PI_new(dir1,dir0Rot,n1);

          PD1out.row(f1)  = targD;
          PD2out.row(f1)  = n1.cross(targD).normalized();

          mark(f1) = true;
          d.push_back(f1);

        }
      }

      // everything should be marked
      for (int i=0; i<F.rows(); i++)
      {
        assert(mark(i));
      }
    }

  };
}
template <typename DerivedV, typename DerivedF>
IGL_INLINE void igl::comb_cross_field(const Eigen::PlainObjectBase<DerivedV> &V,
                                      const Eigen::PlainObjectBase<DerivedF> &F,
                                      const Eigen::PlainObjectBase<DerivedV> &PD1,
                                      const Eigen::PlainObjectBase<DerivedV> &PD2,
                                      Eigen::PlainObjectBase<DerivedV> &PD1out,
                                      Eigen::PlainObjectBase<DerivedV> &PD2out)
{
  igl::Comb<DerivedV, DerivedF> cmb(V, F, PD1, PD2);
  cmb.comb(PD1out, PD2out);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template void igl::comb_cross_field<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::comb_cross_field<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/random.hpp>
#include <boost/lexical_cast.hpp>
#include <alps/accumulators.hpp>

int main(int argc, char* argv[]){
  alps::accumulators::accumulator_set measurements;
  measurements << alps::accumulators::LogBinningAccumulator<double>("log");
  measurements << alps::accumulators::LogBinningAccumulator<double>("ran");
  boost::mt19937 eng(49135);
  boost::variate_generator<boost::mt19937&, boost::uniform_real<>> random_uniform(eng, boost::uniform_real<>());
  std::vector<double> log_data, ran_data;
  int N = 500;
  if(argc == 2){
    N = boost::lexical_cast<double>(argv[1]);
  }else{
    std::cerr << "usege: ./main number, N = 500 (Default)" << std::endl;
  }
  double x = 0;
  for(int i = 0; i < N; ++i){
    double next = x + random_uniform() - 0.5;
    if(abs(x) < abs(next)){
      double probability = exp((x*x - next*next) * 0.125);
      double dice = random_uniform();
      if(probability > dice){
	x = next;
      }
    }else{
      x = next;
    }
    measurements["log"] << x;
    double ran_cash = random_uniform();
    measurements["ran"] << ran_cash;

    log_data.push_back(x);
    ran_data.push_back(ran_cash);
  }
  alps::accumulators::result_set result(measurements);
  std::cout << "log " << std::setprecision(20) << result["log"] << std::endl;
  std::cout << "ran " <<  result["ran"] << std::endl;
  auto log = result["log"];
  auto ran = result["ran"];

  double vari_log = 0.0;
  double mean_log = 0.0;
  double vari_ran = 0.0;
  double mean_ran = 0.0;
  for(int i = 0; i < ran_data.size(); ++i){
    mean_log += log_data[i];
    mean_ran += ran_data[i];
  }
  mean_log /= log_data.size();
  mean_ran /= ran_data.size();
  for(int i = 0; i < ran_data.size(); i = i+pow(2,floor(log2(N))-7)){
    double bin_log = 0.0;
    double bin_ran = 0.0;
    for(int j = 0; j < pow(2,floor(log2(N))-7); ++j){
      bin_log += log_data[i+j];
      bin_ran += ran_data[i+j];
    }
    bin_log /= pow(2,floor(log2(N))-7);
    bin_ran /= pow(2,floor(log2(N))-7);

    vari_log += (bin_log - mean_log)*(bin_log - mean_log);
    vari_ran += (bin_ran - mean_ran)*(bin_ran - mean_ran);
  }
  double vari_naive = 0.0;
  for(int i = 0; i < ran_data.size(); ++i){
    vari_naive += (log_data[i] - mean_log)*(log_data[i]-mean_log);
  }
  vari_naive /= log_data.size()*(log_data.size()-1);

  vari_log /= log_data.size()/(pow(2,floor(log2(N))-7))*(log_data.size()/(pow(2,floor(log2(N))-7)) - 1);
  vari_ran /= log_data.size()/(pow(2,floor(log2(N))-7))*(log_data.size()/(pow(2,floor(log2(N))-7)) - 1);
  std::cout << "log error: " << sqrt(vari_log) << std::setprecision(20) << ", log tau: " << (vari_log/vari_naive - 1.0)*0.5 << std::endl;
  std::cout << "ran error: " << sqrt(vari_ran) << std::endl;
  std::cout << "naive_error: " << sqrt(vari_naive) << std::endl;
  return 0;
}


// (C) 2014 Arek Olek

#pragma once

#include <algorithm>
#include <functional>
#include <utility>
#include <vector>

#include <boost/iterator/function_input_iterator.hpp>
#include <boost/function_output_iterator.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Point_set_2.h>
#include <CGAL/squared_distance_2.h>

#include "range.hpp"

template<class Graph>
unsigned num_internal(Graph const & G) {
  unsigned internal = 0;
  for (auto v : range(vertices(G)))
    internal += out_degree(v, G) > 1;
  return internal;
}

template<class Graph>
unsigned upper_bound(Graph const & G) {
  return std::min(num_internal(G), (unsigned) num_vertices(G) - 2);
}

template<class Graph>
bool is_connected(Graph const & G) {
  std::vector<int> component(num_vertices(G));
  return connected_components(G, &component[0]) == 1;
}

template<class Tree, class Graph>
bool is_subgraph(Tree const & T, Graph const & G) {
  for (auto e : range(edges(T)))
    if(!edge(source(e, T), target(e, T), G).second)
      return false;
  return true;
}

template<class G>
void add_edge_no_dup(typename boost::graph_traits<G>::vertex_descriptor v,
                     typename boost::graph_traits<G>::vertex_descriptor u,
                     G& g) {
  if (!edge(v, u, g).second) add_edge(v, u, g);
}

template<class Input, class Output>
void copy_edges(const Input& in, Output& out) {
  for (auto e : range(edges(in)))
    add_edge(source(e, in), target(e, in), out);
}

template<class Input, class Output, class Generator>
void copy_edges_shuffled(const Input& in, Output& out, Generator& generator) {
  auto p = shuffled(range_iterator(0, num_vertices(in)), generator);
  for (auto e : shuffled(edges(in), generator))
    add_edge(p[source(e, in)], p[target(e, in)], out);
}

template<class Graph>
void add_spider(Graph& G, unsigned legs) {
  unsigned n = num_vertices(G);
  unsigned cutoff = ceil((double)n / legs);
  for (unsigned i = 0; i < n - 1; ++i)
    add_edge_no_dup(i % cutoff == 0 ? 0 : i, i + 1, G);
}

template<class Graph, class Generator>
void add_edges_uniform(Graph& G, double p, Generator& generator, bool mst) {
  unsigned n = num_vertices(G);
  double connectedness = 20.0 / n;
  if (mst && p < connectedness) {
    typedef boost::property<boost::edge_weight_t, double> Weight;
    typedef boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS, boost::no_property, Weight> WeightedGraph;
    typedef boost::graph_traits<WeightedGraph>::edge_descriptor WeightedEdge;
    std::uniform_real_distribution<> distribution(0, 1);
    auto trial = std::bind(distribution, std::ref(generator));
    WeightedGraph g;
    for (unsigned i = 0; i < n; ++i) {
      for (unsigned j = i + 1; j < n; ++j) {
        auto w = trial();
        if (w < connectedness) add_edge(i, j, w, g);
        if (w < p) add_edge_no_dup(i, j, G);
      }
    }
    std::vector<WeightedEdge> t;
    kruskal_minimum_spanning_tree(g, std::back_inserter(t));
    for (auto e : t)
      add_edge_no_dup(source(e, g), target(e, g), G);
  } else {
    std::bernoulli_distribution distribution(p);
    auto trial = std::bind(distribution, std::ref(generator));
    for (unsigned i = 0; i < n; ++i) {
      for (unsigned j = i + 1; j < n; ++j) {
        if (trial()) add_edge_no_dup(i, j, G);
      }
    }
  }
}

class Geometric {
  CGAL::Exact_predicates_inexact_constructions_kernel typedef Kernel;
  CGAL::Triangulation_vertex_base_with_info_2<unsigned, Kernel> typedef Vertex_struct;
  CGAL::Triangulation_data_structure_2<Vertex_struct> typedef Triangulation_struct;
  CGAL::Point_set_2<Kernel, Triangulation_struct> typedef Delaunay;
  Delaunay::Point typedef Point;

  Delaunay D;

 public:
  template<class Generator>
  Geometric(int n, Generator& generator) {
    // Choose random points in square [0, 1) x [0, 1)
    std::uniform_real_distribution<> distribution(0, 1);
    auto r = std::bind(distribution, std::ref(generator));
    // Generate index for every point
    int counter = 0;
    std::function<std::pair<Point, int>()> f = [&]() {return std::make_pair(Point(r(), r()), counter++);};
    // Initialize triangulation with n points
    D = Delaunay(boost::make_function_input_iterator(f, 0), boost::make_function_input_iterator(f, n));
  }

  template<class Graph>
  void add_random_geometric(Graph& G, double d) {
    CGAL::Circle_2<Kernel> typedef Circle;
    Delaunay::Vertex_handle typedef Vertex_handle;
    // Query each point for other points lying in the circle
    for (auto v : range(D.finite_vertices_begin(), D.finite_vertices_end()))
      D.range_search(Circle(v.point(), d * d), boost::make_function_output_iterator([&](Vertex_handle u) {
        if (v.info() < u->info()) add_edge_no_dup(v.info(), u->info(), G);
      }));
  }

  template<class Graph>
  void add_mst(Graph& G) {
    typedef boost::property<boost::edge_weight_t, Kernel::FT> Weight;
    typedef boost::adjacency_list<boost::hash_setS, boost::vecS, boost::undirectedS, boost::no_property, Weight> WeightedGraph;
    typedef boost::graph_traits<WeightedGraph>::edge_descriptor WeightedEdge;

    Delaunay::Finite_vertices_iterator vit;
    Delaunay::Vertex_circulator vc, done;

    WeightedGraph g;

    for (vit = D.finite_vertices_begin(); vit != D.finite_vertices_end(); ++vit) {
      unsigned s = vit->info();
      done = vc = vit->incident_vertices();
      if (vc != 0) do {
        if (D.is_infinite(vc)) continue;
        unsigned d = vc->info();
        add_edge(s, d, CGAL::squared_distance(vit->point(), vc->point()), g);
      } while (++vc != done);
    }

    std::vector<WeightedEdge> mst;
    kruskal_minimum_spanning_tree(g, std::back_inserter(mst));
    for (auto e : mst)
      add_edge_no_dup(source(e, g), target(e, g), G);
  }
};

template<class G>
bool is_planar(G const & gIn) {
  return boyer_myrvold_planarity_test(gIn);
}

//a class to hold the coordinates of the straight line embedding
struct coord_t {
  std::size_t x;
  std::size_t y;
};

template<class G>
std::vector<coord_t> straight_line_drawing(G const & gIn) {
  using namespace boost;

  typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_index_t, int>, property<edge_index_t, int> > Graph;

  Graph g;
  copy_edges(gIn, g);

  //Define the storage type for the planar embedding
  typedef std::vector<std::vector<graph_traits<Graph>::edge_descriptor>> embedding_storage_t;
  typedef iterator_property_map<embedding_storage_t::iterator, property_map<Graph, vertex_index_t>::type> embedding_t;

  make_connected(g);

  // Create the planar embedding
  embedding_storage_t embedding_storage(num_vertices(g));
  embedding_t embedding(embedding_storage.begin(), get(vertex_index, g));

  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = embedding);

  //Initialize the interior edge index
  property_map<Graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<Graph>::edges_size_type edge_count = 0;
  graph_traits<Graph>::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  make_biconnected_planar(g, embedding);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity again; compute the planar embedding as a side-effect
  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = embedding);

  make_maximal_planar(g, embedding);

  //Test for planarity again; compute the planar embedding as a side-effect
  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = embedding);

  // Find a canonical ordering
  std::vector<typename graph_traits<Graph>::vertex_descriptor> ordering;
  planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

  //Set up a property map to hold the mapping from vertices to coord_t's
  typedef std::vector<coord_t> drawing_storage_t;
  typedef boost::iterator_property_map<drawing_storage_t::iterator, property_map<Graph, vertex_index_t>::type> drawing_t;

  drawing_storage_t drawing_storage(num_vertices(g));
  drawing_t drawing(drawing_storage.begin(), get(vertex_index, g));

  // Compute the straight line drawing
  chrobak_payne_straight_line_drawing(g, embedding, ordering.begin(), ordering.end(), drawing);

  return drawing_storage;
}