/*
 * Author: Patrick Schmidt
 */

#include "CohomologyBasis.hh"

#include <HomologyInference/Genus.hh>
#include <HomologyInference/Utils/BarycentricPoint.hh>
#include <HomologyInference/Utils/CotanWeights.hh>
#include <HomologyInference/Utils/Timer.hh>
#include <Eigen/SparseCholesky>
#include <queue>

namespace HomologyInference
{

std::vector<ExternalProperty<HEH, double>>
cohomology_basis(
        const PrimalLoops& _loops,
        const TriMesh& _mesh)
{
    Timer timer(__FUNCTION__);

    // Set up linear system
    const int n_fields = _loops.size();
    const int n_constr = _mesh.n_faces() + _mesh.n_vertices() + n_fields;
    const int n_edges = _mesh.n_edges();
    std::vector<Triplet> triplets;
    MatXd rhs = MatXd::Zero(n_constr, n_fields);

    // 1 if canonical direction, -1 otherwise
    auto he_sign = [&] (const HEH& heh)
    {
        const auto& eh = _mesh.edge_handle(heh);
        const auto& heh0 = _mesh.halfedge_handle(eh, 0);
        return (heh == heh0) ? 1.0 : -1.0;
    };

    // Closedness condition:
    // Integral along every contractible loop is zero.
    for (auto f : _mesh.faces())
    {
        for (auto h : f.halfedges())
            triplets.push_back(Triplet(f.idx(), h.edge().idx(), he_sign(h)));
    }

    // Harmonicity condition:
    // Laplace of field is zero at every vertex
    int offset = _mesh.n_faces();
    for (auto v : _mesh.vertices())
    {
        if (v.is_boundary())
        {
            for (auto h : v.outgoing_halfedges())
                if (h.edge().is_boundary())
                    triplets.push_back(Triplet(offset + v.idx(), h.edge().idx(), he_sign(h) * 1.0 / _mesh.calc_edge_length(h.edge())));
        }
        else
        {
            for (auto h : v.outgoing_halfedges())
                triplets.push_back(Triplet(offset + v.idx(), h.edge().idx(), he_sign(h) * cotan_weight(_mesh, h)));
        }
    }

    // Duality condition:
    // Integral of field i along loop i is 1. All others are 0.
    offset += _mesh.n_vertices();
    for (int i = 0; i < _loops.size(); ++i)
    {
        for (auto heh : _loops[i].hehs)
        {
            const auto& eh = _mesh.edge_handle(heh);
            triplets.push_back(Triplet(offset + i, eh.idx(), he_sign(heh)));
        }
    }
    rhs.block(offset, 0, n_fields, n_fields) = MatXd::Identity(n_fields, n_fields);

    SparseMatrix A(n_constr, n_edges);
    A.setFromTriplets(triplets.begin(), triplets.end());

    // Solve A^T*A * x = A^T*b via sparse Cholesky
    const SparseMatrix AtA = A.transpose() * A;
    const MatXd Atrhs = A.transpose() * rhs;
    Eigen::SimplicialLDLT<SparseMatrix> solver;
    solver.compute(AtA);
    ISM_ASSERT(solver.info() == Eigen::Success);
    MatXd x = solver.solve(Atrhs);
    ISM_ASSERT(solver.info() == Eigen::Success);
    ISM_ASSERT_EQ(x.rows(), n_edges);
    ISM_ASSERT_EQ(x.cols(), n_fields);

    double residual = (A * x - rhs).norm();
    ISM_DEBUG_VAR(residual);

    // Convert to halfedge properties
    auto gradients = std::vector<ExternalProperty<HEH, double>>(n_fields, ExternalProperty<HEH, double>(_mesh));
    for (int i = 0; i < n_fields; ++i)
        for (auto h : _mesh.halfedges())
            gradients[i][h] = he_sign(h) * x(h.edge().idx(), i);

    return gradients;
}

ExternalProperty<VH, double>
integrate_field_real(
        const ExternalProperty<HEH, double>& _gradient,
        const TriMesh& _mesh,
        const VH _seed_vh,
        const double _seed_value)
{
    // Flood fill starting from first vertex
    ExternalProperty<VH, double> u(_mesh, NAN_DOUBLE); // integrated function
    ExternalProperty<VH, bool> visited(_mesh, false);

    std::queue<SHEH> queue;
    for (auto h : _mesh.voh_range(_seed_vh))
        queue.push(h);
    u[_seed_vh] = _seed_value;
    visited[_seed_vh] = true;

    while (!queue.empty())
    {
        const auto h = queue.front();
        queue.pop();

        if (visited[h.to()])
            continue;

        u[h.to()] = u[h.from()] + _gradient[h];
        visited[h.to()] = true;

        for (auto h_enq : h.to().outgoing_halfedges())
        {
            if (!visited[h_enq.to()])
                queue.push(h_enq);
        }
    }

    return u;
}

std::vector<ExternalProperty<VH, double>>
integrated_fields_real(
        const std::vector<ExternalProperty<HEH, double>>& _gradients,
        const TriMesh& _mesh,
        const VH _seed_vh,
        const double _seed_value)
{
    std::vector<ExternalProperty<VH, double>> result;
    for (const auto& g : _gradients)
        result.push_back(integrate_field_real(g, _mesh, _seed_vh, _seed_value));
    return result;
}

ExternalProperty<VH, Complex>
integrate_field_complex(
        const ExternalProperty<HEH, double>& _gradient,
        const TriMesh& _mesh,
        const VH _seed_vh,
        const Complex _seed_value)
{
    ExternalProperty<VH, double> u_real = integrate_field_real(_gradient, _mesh, _seed_vh, 0.0);
    ExternalProperty<VH, Complex> u_complex(_mesh, NAN_DOUBLE);
    for (const auto& v : _mesh.vertices())
    {
        const double angle = u_real[v] * 2 * M_PI;
        const Complex rot = std::polar(1.0, angle);
        u_complex[v] = rot * _seed_value;
    }
    return u_complex;
}

std::vector<ExternalProperty<VH, Complex>>
integrated_fields_complex(
        const std::vector<ExternalProperty<HEH, double>>& _gradients,
        const TriMesh& _mesh,
        const VH _seed_vh,
        const Complex _seed_value)
{
    Timer timer(__FUNCTION__);

    std::vector<ExternalProperty<VH, Complex>> result;
    for (const auto& g : _gradients)
        result.push_back(integrate_field_complex(g, _mesh, _seed_vh, _seed_value));
    return result;
}

ExternalProperty<HEH, double>
differentiate_field(
        const ExternalProperty<VH, Complex>& _u,
        const TriMesh& _mesh)
{
    ExternalProperty<HEH, double> gradient(_mesh);
    for (const auto& heh : _mesh.halfedges())
    {
        const VH vh0 = _mesh.from_vertex_handle(heh);
        const VH vh1 = _mesh.to_vertex_handle(heh);
        const Complex u0 = _u[vh0];
        const Complex u1 = _u[vh1];
        const Complex rot = u1 / u0;
        const double angle_diff = std::arg(rot) / (2 * M_PI);
        gradient[heh] = angle_diff;
    }
    return gradient;
}

std::vector<ExternalProperty<HEH, double>>
differentiate_fields(
        const std::vector<ExternalProperty<VH, Complex>>& _us,
        const TriMesh& _mesh)
{
    Timer timer(__FUNCTION__);

    std::vector<ExternalProperty<HEH, double>> result;
    for (const auto& u : _us)
        result.push_back(differentiate_field(u, _mesh));
    return result;
}

double
integrate_loop(
        const ExternalProperty<HEH, double>& _gradient,
        const PrimalLoop& _loop)
{
    double res = 0.0;
    for (auto h : _loop.hehs)
        res += _gradient[h];
    return res;
}

MatXd
integrate_loops(
        const std::vector<ExternalProperty<HEH, double>>& _fields,
        const PrimalLoops& _loops)
{
    ISM_ASSERT(_loops.size() == _fields.size());
    const int n = _loops.size();
    MatXd M_integrated = MatXd::Zero(n, n);
    for (int row = 0; row < n; ++row)
        for (int col = 0; col < n; ++col)
            M_integrated(row, col) = integrate_loop(_fields[col], _loops[row]);
    return M_integrated;
}

ExternalProperty<HEH, double>
combine_fields(
        const TriMesh& _mesh,
        const VecXi& _coeffs,
        const std::vector<ExternalProperty<HEH, double>>& _fields)
{
    const VecXd coeffs_d = _coeffs.cast<double>();
    return combine_fields(_mesh, coeffs_d, _fields);
}

ExternalProperty<HEH, double>
combine_fields(
        const TriMesh& _mesh,
        const VecXd& _coeffs,
        const std::vector<ExternalProperty<HEH, double>>& _fields)
{
    ISM_ASSERT_EQ(_coeffs.size(), _fields.size());
    ExternalProperty<HEH, double> field(_mesh, 0.0);
    for (int i = 0; i < _coeffs.size(); ++i)
    {
        const auto& input_field = _fields[i];
        ISM_ASSERT(input_field.size_okay(_mesh));
        for (const auto& heh : _mesh.halfedges())
            field[heh] += _coeffs[i] * input_field[heh];
    }
    return field;
}

std::vector<ExternalProperty<HEH, double>>
transform_fields(
        const TriMesh& _mesh,
        const MatXi& _M,
        const std::vector<ExternalProperty<HEH, double>>& _fields)
{
    ISM_ASSERT_EQ(_M.cols(), _fields.size());
    ISM_ASSERT(!_fields.empty());
    std::vector<ExternalProperty<HEH, double>> result;
    for (int row = 0; row < _M.rows(); ++row)
    {
        const VecXi M_row = _M.row(row);
        ExternalProperty<HEH, double> field = combine_fields(_mesh, M_row, _fields);
        result.push_back(field);
    }
    return result;
}

}


#include <iostream>
#include <map>
#include <vector>

#include <boost/dynamic_bitset.hpp>

#include "CliqueList.h"

using namespace std;
using namespace boost;

/** Constructor. Note that this isn't particularly fast (but
    doesn't have to be). */
CliqueList::CliqueList(int n, int r, int maxCliqueSize) {
    n_ = n;
    r_ = r;
    max_clique_size_ = maxCliqueSize;
    // number the (hyper)edges
    initEdgeNumbering();
    // initialize the cliques
    clique_.resize(maxCliqueSize+1);
    for(int k=r; k<=maxCliqueSize; k++)
        addCliques(k);
}

/** Adds counts of which cliques are present in a graph.
    Should be multithreaded.
    Possibly not used, as the bookkeeping seems easier if I
    just track the largest clique. (Clearly, if there's a
    9-vertex clique, there are many 8-cliques, and even
    more 7-vertex cliques, etc.) */
void CliqueList::addToCounts(vector<int> count, const bits & g) {
    // loop through the possible sizes of clique
    for(int k = 0; k < int(clique_.size()); k++)
        // loop through the cliques
        for(int i = 0; i < int(clique_[k].size()); i++)
            if (clique_[k][i].is_subset_of(g))
                count[k]++;
}

/** Gets size of the largest clique in a graph.
    Should be multithreaded.
    g: vertices of a hypergraph
    Returns: number of vertices in the largest clique */
int CliqueList::getMaxCliqueSize(const bits & g) {
    // loop through possible clique sizes (starting with largest)
    for(int k = n_; k >= r_; k--)
        // loop through the cliques
        for(int i = 0; i < int(clique_[k].size()); i++) {
            if (clique_[k][i].is_subset_of(g))
                return k;
        }
    return 0;
}

/** Gets vertices covered by hypergraphs.
    g: vertices of a hypergraph
    k: size of the hyperclique to check for
    coveredEdges: ref. to bits, which will be set with
        the edges which are covered
    Side effects: fills in coveredEdges
    Returns: number of k-vertex cliques found */
int CliqueList::getCoveredEdges(const bits & g,
        int k, bits & coveredEdges) {
    // set coveredEdges to all 0 (In theory, if we enforced
    // that callers looked for larger cliques first, we wouldn't
    // need this. But this seems simpler).
    coveredEdges.reset();
    int cliqueCount = 0;
    // loop through the cliques
    for(int i = 0; i < int(clique_[k].size()); i++) {
        if (clique_[k][i].is_subset_of(g)) {
            // record the edges covered by this clique
            coveredEdges |= clique_[k][i];
            ++cliqueCount;
        }
    }
    return cliqueCount;
}

/** Initialize the edge numbering. */
void CliqueList::initEdgeNumbering() {
    int e = 0;
    // this loops through all r-element subsets of n vertices
    SubsetIterator edge(n_, r_);
    do {
        edgeIndex_[ edge.getSet() ] = e;
        ++e;
    } while (edge.next());
}

/** Initialize the cliques with k vertices. */
void CliqueList::addCliques(int k) {
    // this will contain one bitvector per clique
    vector<bits> bitvectors;
    // this loops through the cliques
    SubsetIterator clique(n_, k);
    do {
        // this is the k indices of vertices
        vector<int> &v = clique.getSet();
        // this loops through the edges in a clique
        SubsetIterator edge(k, r_);
        // this tracks the bits corresponding to those edges
        bits edgeBits(edgeIndex_.size());
        do {
            // this will be the r indices of an edge
            // in the clique, among all n vertices
            vector<int> r1;
            vector<int> &e = edge.getSet();
            for(int i=0; i<int(e.size()); i++)
                r1.push_back(v[e[i]]);
            // add this edge into the clique
            edgeBits |= getBitmask(r1);
        } while (edge.next());
        bitvectors.push_back(edgeBits);
    } while (clique.next());
    clique_[k] = bitvectors;
}

/** Gets the bitmask corresponding to a particular clique. */
bits CliqueList::getBitmask(vector<int> & edge) {
    bits x(edgeIndex_.size());
    // set bits corresponding to each edge
    for(int i=0; i<int(edge.size()); i++)
        x[ edgeIndex_[edge] ] = 1;
    return x;
}

/** Dumps list of cliques to standard output. */
void CliqueList::printCliques() {
    cout << "dumping cliques" << endl;
    for(int cliqueSize = n_; cliqueSize >= r_; --cliqueSize) {
        cout << "n = " << cliqueSize << endl;
        vector<bits> & clique1 = clique_[cliqueSize];
        // loop through cliques of this size
        for(int c = 0; c < int(clique1.size()); ++c) {
            cout << clique1[c] << " ";
        }
        cout << endl;
    }
}

/** Prints the edges in one graph. */
void CliqueList::printGraph(bits &e) {
    // FIXME print the vertices
    cout << e << " ";
}


#pragma once

#include <algorithm>
#include <dlib/clustering.h>
#include <sstream>
#include <stdexcept>
#include <string>
#include <tuple>
#include <vector>

#include "flat.hpp"

namespace otus {
  class Clusterer {
  public:
    class IOError: public std::runtime_error {
    public:
      IOError(std::string const & message):
      std::runtime_error(message) { }
    };

    Clusterer(
        std::vector<Flat> const & data,
        int numberOfClusters,
        float gamma=0.1,
        float accuracy=0.01);

    Clusterer(std::string const & filename);

    void save(std::string const & filename);

    std::string operator()(Flat const & flat) const;

  private:
    using KernelType = dlib::radial_basis_kernel<DataType>;
    using DataSizeType = decltype(DataType().size());
    using ClusterNumType = unsigned long int;

    std::string const fileSuffix { ".dat" };
    std::vector<DataType> data;
    std::vector<std::pair<Flat, ClusterNumType>> metaData;
    dlib::kkmeans<KernelType> kMeans;
    /// For normalization function y = kx - a.
    std::vector<float> k, a; // TODO Make serializable functor.

    void normalizeData();

    void normalize(DataType & item) const;

    void train(int numberOfClusters);
  };
}


/**
 * Decode binary space partitioning airline seating assignments
 */
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <vector>
#include <sstream>

#include <boost/algorithm/string.hpp>

void decode(const std::string &spec, int &row, int &col)
{
    if(spec.length() != 10) { return; }

    const std::string rowspec = spec.substr(0, 7);
    const std::string colspec = spec.substr(7, 3);

    row = 0;
    for(int i=0; i<7; ++i)
    {
        row <<= 1;

        //std::cout << rowspec.at(i) << std::endl;
        if(rowspec.at(i) == 'F')
        {
            row |= 0;
        }
        else
        {
            row |= 1;
        }
    }

    col = 0;
    for(int i=0; i<3; ++i)
    {
        col <<= 1;

        //std::cout << colspec.at(i) << std::endl;
        if(colspec.at(i) == 'R')
        {
            col |= 1;
        }
        else
        {
            col |= 0;
        }
    }
}

void usage()
{
    std::cout << "day5 <input.txt>" << std::endl;
}

int main(int argc, char *argv[])
{
    if(argc != 2)
    {
        usage();
        return EXIT_FAILURE;
    }

    int row, col;

    //  test data
    /*
    decode("FBFBBFFRLR", row, col);
    std::cout << "row: " << row << std::endl;
    std::cout << "col: " << col << std::endl;

    decode("BFFFBBFRRR", row, col);
    std::cout << "row: " << row << std::endl;
    std::cout << "col: " << col << std::endl;

    decode("FFFBBBFRRR", row, col);
    std::cout << "row: " << row << std::endl;
    std::cout << "col: " << col << std::endl;

    decode("BBFFBBFRLL", row, col);
    std::cout << "row: " << row << std::endl;
    std::cout << "col: " << col << std::endl;
    */

    int maxSeatId = 0;

    std::ifstream ifs(argv[1], std::ifstream::in);
    std::string line;
    while(std::getline(ifs, line))
    {
        decode(line, row, col);
        int seatId = row * 8 + col;
        if(seatId > maxSeatId)
        {
            maxSeatId = seatId;
        }
    }

    std::cout << "max seat id: " << maxSeatId << std::endl;

    ifs.close();
}


#include <fstream>
#include <armadillo>
#include <thread>

using namespace std;
using namespace arma;

// Armadillo documentation is available at:
// http://arma.sourceforge.net/docs.html

bool intersect(const vec& origin, const vec& direction, const vec& center, double radius, vec& normal, vec& hit) {
	vec oc = origin - center;
	double a = dot(direction, direction);
	double b = 2.0*dot(direction, oc);
	double c = dot(oc, oc) - radius*radius;

	double delta = b*b - 4.0*a*c;
	if (delta < 0.0) {
		return false;
	}
	double t1 = (-b + sqrt(delta)) / (2.0*a);
	double t2 = (-b - sqrt(delta)) / (2.0*a);
	double t = (t1 < t2) ? t1 : t2;

	hit = origin + direction*t;
	normal = hit - center;
	return true;
}

int main() {
	vec centro_esfera = { 0.0, 0.0, 10.0 };
	vec origin = { 0.0, 0.0, 0.0 };
	double intensidade = 0.9;
	vec posicao_luz = { 0.0, 4.0, 4.0 };
	vec radiancia_luz = { intensidade, intensidade, intensidade };
	mat k;
	k << 1000.0 << 0.0 << 400.0 << endr
		<< 0.0 << -1000.0 << 300.0 << endr
		<< 0.0 << 0.0 << 1.0;
	mat invk = k.i();

	ofstream output;
	output.open("imagem.pgm");
	output << "P3" << endl;
	output << "800 600" << endl;
	output << "255" << endl;

	for (int linha = 0; linha < 600; linha++) {
		for (int coluna = 0; coluna < 800; coluna++) {
			vec r = { double(coluna), double(linha), 1.0 };
			vec normal, hit;

			r = invk * r;
			r *= 5.0 / r(2);
			if (intersect(origin, r, centro_esfera, 2.0, normal, hit)) {
				normal /= norm(normal);
				vec l = posicao_luz - hit;
				l /= norm(l);

				vec cor = { 255.0, 0.0, 0.0 };
				cor = (cor % radiancia_luz)*std::max(0.0, dot(normal, l));
				output << std::min(255, int(cor(0))) << " "
					   << std::min(255, int(cor(1))) << " "
					   << std::min(255, int(cor(2))) << " ";
			} else {
				output << "255 255 255 ";
			}
		}
	}
	output.close();
	return 0;
}


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

#include "y2017/vision/target_finder.h"

#include "aos/vision/blob/move_scale.h"
#include "aos/vision/blob/stream_view.h"
#include "aos/vision/blob/transpose.h"
#include "aos/vision/debug/debug_framework.h"
#include "aos/vision/math/vector.h"

using aos::vision::ImageRange;
using aos::vision::ImageFormat;
using aos::vision::RangeImage;
using aos::vision::BlobList;

namespace y2017 {
namespace vision {

BlobList RenderTargetListShifted(const std::vector<TargetComponent> &list) {
  BlobList out;
  for (const auto &entity : list) {
    out.emplace_back(entity.RenderShifted());
  }
  return out;
}

RangeImage TargetComponent::RenderShifted() const {
  std::vector<std::vector<ImageRange>> out_range_list;
  int y = 0;
  double max_y = -b / (2 * a);
  double parab_off = max_y * max_y * a + max_y * b;
  RangeImage t_img = Transpose(*img);
  for (const auto &row : t_img) {
    int off = -(y * y * a + y * b - parab_off);
    // int off = 0;
    // fprintf(stderr, "off: %d %d\n", off, y);
    std::vector<ImageRange> row_out;
    for (const ImageRange &range : row) {
      row_out.emplace_back(ImageRange{off + range.st, off + range.ed});
    }
    ++y;
    out_range_list.emplace_back(std::move(row_out));
  }
  return RangeImage(t_img.min_y(), std::move(out_range_list));
}

class FilterHarnessExample : public aos::vision::FilterHarness {
 public:
  aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
    return finder_.Threshold(image);
  }

  void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
    viewer_ = viewer;
    viewer_->SetScale(0.5);
    overlays_.push_back(&overlay_);
    overlays_.push_back(finder_.GetOverlay());
    viewer_->view()->SetOverlays(&overlays_);
  }

  bool HandleBlobs(BlobList imgs, ImageFormat /*fmt*/) override {
    // reset for next drawing cycle
    for (auto &overlay : overlays_) {
      overlay->Reset();
    }

    // Remove bad blobs.
    finder_.PreFilter(imgs);

    // calculate each component/
    std::vector<TargetComponent> target_component_list =
        finder_.FillTargetComponentList(imgs);

    DrawComponents(target_component_list);

    // Put the compenents together into targets and pick the best.
    Target final_target;
    bool found_target =
        finder_.FindTargetFromComponents(target_component_list, &final_target);

    // BlobList newImg = RenderTargetListShifted(target_component_list);
    if (viewer_) {
      viewer_->DrawBlobList(imgs, {0, 0, 255});
    }

    if (found_target) {
      BlobList list;
      list.emplace_back(*(final_target.comp1.img));
      list.emplace_back(*(final_target.comp2.img));
      viewer_->DrawBlobList(list, {0, 255, 0});
      overlay_.DrawCross(final_target.screen_coord, 25, {255, 255, 255});
    }

    // No targets.
    return found_target;
  }

  void DrawComponents(const std::vector<TargetComponent> comp) {
    for (const TargetComponent &t : comp) {
      aos::vision::ImageBBox bbox;
      GetBBox(*(t.img), &bbox);
      overlay_.DrawBBox(bbox, {255, 0, 0});

      overlay_.StartNewProfile();
      for (int i = 0; i < bbox.maxx - bbox.minx; i += 10) {
        double y0 = t.a * i * i + t.b * i + t.c_0;
        double y1 = t.a * i * i + t.b * i + t.c_1;
        overlay_.AddPoint(aos::vision::Vector<2>(i + t.mini, y0), {255, 0, 0});
        overlay_.AddPoint(aos::vision::Vector<2>(i + t.mini, y1), {255, 0, 0});
      }
    }
  }

 private:
  // implementation of the filter pipeline.
  TargetFinder finder_;
  aos::vision::BlobStreamViewer *viewer_ = nullptr;
  aos::vision::PixelLinesOverlay overlay_;
  std::vector<aos::vision::OverlayBase *> overlays_;
};

}  // namespace vision
}  // namespace y2017

int main(int argc, char **argv) {
  y2017::vision::FilterHarnessExample filter_harness;
  aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
                                  aos::vision::CameraParams());
}


//
// Created by haohanwang on 1/22/16.
//

#ifndef ALGORITHMS_LASSO_HPP
#define ALGORITHMS_LASSO_HPP

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

using namespace Eigen;
using namespace std;

class Lasso {
private:
    MatrixXf X;
    MatrixXf beta;
    VectorXf y;
    float lambda;
    float learningRate;
    float progress;
public:
    void setX(MatrixXf);
    void setY(VectorXf);
    void setLambda(float);
    void setLearningRage(float);

    MatrixXf getX(void);
    MatrixXf getBeta(void);
    VectorXf getY(void);
    float getLambda(void);
    float getLearningRate(void);

    virtual void assertReadyToRun();
    void train();
    void train(MatrixXf, VectorXf, float, float, float);

    VectorXf predict();
    VectorXf predict(MatrixXf);

    Lasso();
};

#endif //ALGORITHMS_LASSO_HPP


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/assign.hpp>
#include <boost/multi_array.hpp>
using namespace boost;

//////////////////////////////////////////
void case1()
{
    multi_array<int, 3> ma(extents[2][3][4]);

    auto shape = ma.shape();
    for (size_t i = 0; i < ma.num_dimensions(); ++i)
    {
        cout << shape[i] << ",";
    }
    cout << endl << ma.num_elements() << endl;

    for (int i = 0,  v = 0; i < 2; ++i)
        for (int j = 0; j < 3;++j)
            for (int k = 0;k < 4;++k)
            {
                ma[i][j][k] = v++;
            }

    for (int i = 0; i < 2; ++i)
    {
        for (int j = 0; j < 3;++j)
        {
            for (int k = 0;k < 4;++k)
            {
                cout << ma[i][j][k] << ",";
            }
            cout << endl;
        }
        cout << endl;
    }

    //cout << ma[2][3][4];

    std::array<size_t, 3> idx = {0,1,2};
    ma(idx) = 10;
    cout << ma(idx) << endl;
}

//////////////////////////////////////////
void case2()
{
    multi_array<int, 3> ma(extents[2][3][4]);
    assert(ma.shape()[0] == 2);

    std::array<std::size_t, 3> arr = {4,3,2};
    ma.reshape(arr);
    assert(ma.shape()[0] == 4);

    ma.resize(extents[2][9][9]);
    assert(ma.num_elements() == 2*9*9);
    assert(ma.shape()[1] == 9);
}

//////////////////////////////////////////
void case3()
{
    typedef multi_array<int, 2> ma_type;
    multi_array<int, 2> ma(extents[3][4]) ;

    typedef ma_type::index_range range;
    //indices[range(0,2)][range(0,2)];

    auto view = ma[indices[range(0,2)][range(0,2)] ];

    cout << view.num_elements() << endl;
    for (int i = 0; i < 2; ++i)
    {
        for (int j = 0; j < 2;++j)
        {
            cout << view[i][j] << ",";
        }
        cout << endl;
    }
    cout << *view.shape() << endl;

}

//////////////////////////////////////////
void case4()
{
    int arr[12];
    for (int i = 0;i < 12;++i)
    {   arr[i] = i; }

    multi_array_ref<int, 2> mar(arr, extents[3][4]);

    for (size_t i = 0; i < 3; ++i)
    {
        cout << "(";
        for(size_t j = 0;j < 4;++j)
        {
            cout << mar[i][j]++;
            cout << (j!=3?',':' ');
        }
        cout << ")" << endl;
    }

    const_multi_array_ref<int, 2> cmar(arr, extents[2][6]);

    for (size_t i = 0; i < 2; ++i)
    {
        cout << "(";
        for(size_t j = 0;j < 6;++j)
        {
            cout << cmar[i][j];
            cout << (j!=5?',':' ');
        }
        cout << ")" << endl;
    }

}

//////////////////////////////////////////
void case5()
{
    //multi_array<int, 3> ma(vector<int>(assign::list_of(2)(2)));
    multi_array<int, 3> ma(vector<int>{2,2,2});
    auto shape = ma.shape();
    for (size_t i = 0; i < ma.num_dimensions(); ++i)
    {
        cout << shape[i] << ",";
    }

}

//////////////////////////////////////////
void case6()
{
    typedef multi_array<int, 3> ma_type;
    typedef ma_type::extent_range range;
    ma_type ma(extents [range(1,5)][4][range(-2,2)]);
    ma[1][0][-2] = 10;

    ma.reindex(1);
    assert(ma[1][1][1] == 10);
    ma.reindex(std::array<int,3>{1,0,-4});
    assert(ma[1][0][-4] == 10);

    cout << *ma.index_bases() << endl;
}
//////////////////////////////////////////
void case7()
{
    using namespace boost::detail::multi_array;

    typedef multi_array<int, 3> ma_type;
    typedef ma_type::index_range range;
    ma_type ma(extents[9][8][7]);

    auto view = ma[indices[range()< 3L][2L<=range()<= 5L][range()] ];
    cout << *view.shape() << endl;

}
//////////////////////////////////////////

int main()
{
    case1();
    case2();
    case3();
    case4();
    case5();
    case6();
    case7();
}


#ifndef BOOST_METAPARSE_GETTING_STARTED_5_HPP
#define BOOST_METAPARSE_GETTING_STARTED_5_HPP

// Automatically generated header file

// Definitions before section 4.2
#include "4_2.hpp"

// Definitions of section 4.2
#include <boost/metaparse/transform.hpp>

#include <boost/mpl/plus.hpp>

#include <boost/mpl/at.hpp>

template <class Vector>
 struct eval_plus :
   boost::mpl::plus<
     typename boost::mpl::at_c<Vector, 0>::type,
     typename boost::mpl::at_c<Vector, 2>::type
   > {};

// query:
//   eval_plus<
//     boost::mpl::vector<
//       mpl_::integral_c<int, 11>,
//       mpl_::char_<'+'>,
//       mpl_::integral_c<int, 2>
//     >>::type

#include <boost/mpl/quote.hpp>

using exp_parser6 =
 build_parser<
   transform<
     sequence<int_token, plus_token, int_token>,
     boost::mpl::quote1<eval_plus>
   >
 >;

// query:
//    exp_parser6::apply<BOOST_METAPARSE_STRING("11 + 2")>::type

#endif


#include "dbg.hh"
#include <cmath>
#include <Eigen/LU>

namespace dbg {

#define KT84DBG_CPP_MAKE_EIGEN_MATRIX_FUNC_SUB1(Type, Size)\
    Type&                           at  (Eigen::Matrix<Type, Size, Size>& m, int i, int j) { return m(i, j); }\
    Eigen::Matrix<Type, Size, 1>    row (Eigen::Matrix<Type, Size, Size>& m, int i, Eigen::Matrix<Type, Size, 1>& dst) { return dst = m.row(i); }\
    Eigen::Matrix<Type, Size, 1>    col (Eigen::Matrix<Type, Size, Size>& m, int i, Eigen::Matrix<Type, Size, 1>& dst) { return dst = m.col(i); }\
    Type                            det (Eigen::Matrix<Type, Size, Size>& m) { return m.determinant(); }\
    Eigen::Matrix<Type, Size, Size> add (Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Eigen::Matrix<Type, Size, Size>& dst) { return dst = m1 + m2; }\
    Eigen::Matrix<Type, Size, Size> sub (Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Eigen::Matrix<Type, Size, Size>& dst) { return dst = m1 - m2; }\
    Eigen::Matrix<Type, Size, Size> mul (Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Eigen::Matrix<Type, Size, Size>& dst) { return dst = m1 * m2; }\
    Eigen::Matrix<Type, Size, Size> mul (Type s, Eigen::Matrix<Type, Size, Size>& m, Eigen::Matrix<Type, Size, Size>& dst) { return dst = s * m; }\
    Eigen::Matrix<Type, Size, Size> wsum(Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Type w1, Type w2, Eigen::Matrix<Type, Size, Size>& dst) { return dst = w1 * m1 + w2 * m2; }\
    Eigen::Matrix<Type, Size, Size> wsum(Eigen::Matrix<Type, Size, Size>& m1, Eigen::Matrix<Type, Size, Size>& m2, Eigen::Matrix<Type, Size, Size>& m3, Type w1, Type w2, Type w3, Eigen::Matrix<Type, Size, Size>& dst) { return dst = w1 * m1 + w2 * m2 + w3 * m3; }\
    Eigen::Matrix<Type, Size, Size> rand(Eigen::Matrix<Type, Size, Size>& m, int rows, int cols) { return m = Eigen::Matrix<Type, Size, Size>::Random(rows, cols); }\
    Eigen::Matrix<Type, Size, Size> zero(Eigen::Matrix<Type, Size, Size>& m, int rows, int cols) { return m = Eigen::Matrix<Type, Size, Size>::Zero  (rows, cols); }\
    Eigen::Matrix<Type, Size, Size> rand(Eigen::Matrix<Type, Size, Size>& m) { return m = Eigen::Matrix<Type, Size, Size>::Random(Size, Size); }\
    Eigen::Matrix<Type, Size, Size> zero(Eigen::Matrix<Type, Size, Size>& m) { return m = Eigen::Matrix<Type, Size, Size>::Zero  (Size, Size); }\

#define KT84DBG_CPP_MAKE_EIGEN_VECTOR_FUNC_SUB1(Type, Rows, Cols)\
    Type&                           at   (Eigen::Matrix<Type, Rows, Cols>& v, int i) { return v[i]; }\
    Type                            dot  (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2) { return v1.dot(v2); }\
    double                          norm(Eigen::Matrix<Type, Rows, Cols>& v) { return std::sqrt(v.dot(v)); }\
    Eigen::Matrix<Type, Rows, Cols> normalize(Eigen::Matrix<Type, Rows, Cols>& v) { return v = v / norm(v); }\
    Eigen::Matrix<Type, Rows, Cols> cross(Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Eigen::Matrix<Type, Rows, Cols>& dst) {\
        dst.resize(3);\
        dst[0] = v1[1] * v2[2] - v1[2] * v2[1];\
        dst[1] = v1[2] * v2[0] - v1[0] * v2[2];\
        dst[2] = v1[0] * v2[1] - v1[1] * v2[0];\
        return dst;\
    }\
    Eigen::Matrix<Type, Rows, Cols> add  (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Eigen::Matrix<Type, Rows, Cols>& dst) { return dst = v1 + v2; }\
    Eigen::Matrix<Type, Rows, Cols> sub  (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Eigen::Matrix<Type, Rows, Cols>& dst) { return dst = v1 - v2; }\
    Eigen::Matrix<Type, Rows, Cols> mul  (Type s, Eigen::Matrix<Type, Rows, Cols>& v, Eigen::Matrix<Type, Rows, Cols>& dst) { return dst = s * v; }\
    /* TODO: matrix-vector multiplication */\
    Eigen::Matrix<Type, Rows, Cols> wsum (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Type w1, Type w2, Eigen::Matrix<Type, Rows, Cols>& dst) { return dst = w1 * v1 + w2 * v2; }\
    Eigen::Matrix<Type, Rows, Cols> wsum (Eigen::Matrix<Type, Rows, Cols>& v1, Eigen::Matrix<Type, Rows, Cols>& v2, Eigen::Matrix<Type, Rows, Cols>& v3, Type w1, Type w2, Type w3, Eigen::Matrix<Type, Rows, Cols>& dst) { return dst = w1 * v1 + w2 * v2 + w3 * v3; }\
    Eigen::Matrix<Type, Rows, Cols> rand(Eigen::Matrix<Type, Rows, Cols>& v, int size) { return v = Eigen::Matrix<Type, Rows, Cols>::Random(size); }\
    Eigen::Matrix<Type, Rows, Cols> zero(Eigen::Matrix<Type, Rows, Cols>& v, int size) { return v = Eigen::Matrix<Type, Rows, Cols>::Zero  (size); }\
    Eigen::Matrix<Type, Rows, Cols> rand(Eigen::Matrix<Type, Rows, Cols>& v) { return v = Eigen::Matrix<Type, Rows, Cols>::Random(Rows * Cols); }\
    Eigen::Matrix<Type, Rows, Cols> zero(Eigen::Matrix<Type, Rows, Cols>& v) { return v = Eigen::Matrix<Type, Rows, Cols>::Zero  (Rows * Cols); }\

#define KT84DBG_CPP_MAKE_EIGEN_VECTOR_FUNC_SUB2(Type, Size)\
    KT84DBG_CPP_MAKE_EIGEN_VECTOR_FUNC_SUB1(Type, Size, 1)\
    KT84DBG_CPP_MAKE_EIGEN_VECTOR_FUNC_SUB1(Type, 1, Size)\

#define KT84DBG_CPP_MAKE_EIGEN_FUNC_SUB1(Type, Size)\
    KT84DBG_CPP_MAKE_EIGEN_MATRIX_FUNC_SUB1(Type, Size)\
    KT84DBG_CPP_MAKE_EIGEN_VECTOR_FUNC_SUB2(Type, Size)\

#define KT84DBG_CPP_MAKE_EIGEN_FUNC_SUB2(Type)\
    KT84DBG_CPP_MAKE_EIGEN_FUNC_SUB1(Type,  2)\
    KT84DBG_CPP_MAKE_EIGEN_FUNC_SUB1(Type,  3)\
    KT84DBG_CPP_MAKE_EIGEN_FUNC_SUB1(Type,  4)\
    KT84DBG_CPP_MAKE_EIGEN_FUNC_SUB1(Type, -1)\

#define KT84DBG_CPP_MAKE_STL_FUNC_SUB1(Type)\
    Type& at(std::vector<Type>& c, int i) { return c[i]; }\

#define KT84DBG_CPP_MAKE_VARIABLE_SUB1(Type, TypeSuffix, Size, SizeSuffix)\
    Eigen::Matrix<Type, Size, Size> mat##SizeSuffix##TypeSuffix [pool_size];\
    Eigen::Matrix<Type, Size, 1>    vec##SizeSuffix##TypeSuffix [pool_size];\
    Eigen::Matrix<Type, 1, Size>    cvec##SizeSuffix##TypeSuffix[pool_size];\

#define KT84DBG_CPP_MAKE_VARIABLE_SUB2(Type, TypeSuffix)\
    Type x##TypeSuffix[pool_size];\
    KT84DBG_CPP_MAKE_VARIABLE_SUB1(Type, TypeSuffix,  2, 2)\
    KT84DBG_CPP_MAKE_VARIABLE_SUB1(Type, TypeSuffix,  3, 3)\
    KT84DBG_CPP_MAKE_VARIABLE_SUB1(Type, TypeSuffix,  4, 4)\
    KT84DBG_CPP_MAKE_VARIABLE_SUB1(Type, TypeSuffix, -1, x)\

#define KT84DBG_CPP_MAKE_SUB1(Type, TypeSuffix)\
    KT84DBG_CPP_MAKE_EIGEN_FUNC_SUB2(Type)\
    KT84DBG_CPP_MAKE_STL_FUNC_SUB1(Type)\
    KT84DBG_CPP_MAKE_VARIABLE_SUB2(Type, TypeSuffix)\

KT84DBG_CPP_MAKE_SUB1(int   , i)
KT84DBG_CPP_MAKE_SUB1(double, d)
KT84DBG_CPP_MAKE_SUB1(float , f)

#define KT84DBG_CPP_EIGEN_COPY_SUB1(TypeSrc, TypeDst, Size)\
    void copy(Eigen::Matrix<TypeSrc, Size, Size>& src, Eigen::Matrix<TypeDst, Size, Size>& dst) { dst = src.cast<TypeDst>(); }\
    void copy(Eigen::Matrix<TypeSrc, Size, 1   >& src, Eigen::Matrix<TypeDst, Size, 1   >& dst) { dst = src.cast<TypeDst>(); }\
    void copy(Eigen::Matrix<TypeSrc, 1   , Size>& src, Eigen::Matrix<TypeDst, 1   , Size>& dst) { dst = src.cast<TypeDst>(); }\

#define KT84DBG_CPP_EIGEN_COPY_SUB2(TypeSrc, TypeDst)\
    KT84DBG_CPP_EIGEN_COPY_SUB1(TypeSrc, TypeDst,  2)\
    KT84DBG_CPP_EIGEN_COPY_SUB1(TypeSrc, TypeDst,  3)\
    KT84DBG_CPP_EIGEN_COPY_SUB1(TypeSrc, TypeDst,  4)\
    KT84DBG_CPP_EIGEN_COPY_SUB1(TypeSrc, TypeDst, -1)\

KT84DBG_CPP_EIGEN_COPY_SUB2(int   , int   )
KT84DBG_CPP_EIGEN_COPY_SUB2(int   , double)
KT84DBG_CPP_EIGEN_COPY_SUB2(int   , float )
KT84DBG_CPP_EIGEN_COPY_SUB2(double, int   )
KT84DBG_CPP_EIGEN_COPY_SUB2(double, double)
KT84DBG_CPP_EIGEN_COPY_SUB2(double, float )
KT84DBG_CPP_EIGEN_COPY_SUB2(float , int   )
KT84DBG_CPP_EIGEN_COPY_SUB2(float , double)
KT84DBG_CPP_EIGEN_COPY_SUB2(float , float )

}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <Eigen/Core>
#include <smooth/bundle.hpp>
#include <smooth/feedback/compat/ipopt.hpp>
#include <smooth/feedback/ocp.hpp>
#include <smooth/se2.hpp>

#include <chrono>
#include <iostream>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

template<typename T>
using X = smooth::Bundle<smooth::SE2<T>, Eigen::Vector3<T>>;

template<typename T>
using U = Eigen::Vector2<T>;

template<typename T>
using Vec = Eigen::VectorX<T>;

/// @brief Objective function
const auto obj = []<typename T>(T tf, const X<T> &, const X<T> &, const Vec<T> & q) -> T {
  return tf + q.x();
};

/// @brief Dynamics
const auto f = []<typename T>(T, const X<T> & x, const U<T> & u) -> smooth::Tangent<X<T>> {
  smooth::Tangent<X<T>> ret;
  ret.segment(0, 3) = x.template part<1>();
  ret(3)            = u.x();
  ret(4)            = T(0);
  ret(5)            = u.y();
  return ret;
};

/// @brief Integrals
const auto g = []<typename T>(T, const X<T> &, const U<T> & u) -> Vec<T> {
  return Vec<T>{{u.squaredNorm()}};
};

/// @brief Running constraints
const auto cr = []<typename T>(T, const X<T> &, const U<T> & u) -> Vec<T> { return u; };

/// @brief End constraints
const auto ce = []<typename T>(T tf, const X<T> & x0, const X<T> & xf, const Vec<T> &) -> Vec<T> {
  const smooth::SE2<T> target(smooth::SO2<T>(-0.5), Eigen::Vector2<T>{2, 0.5});
  Vec<T> ret(10);
  ret << tf, x0.template part<0>().log(), x0.template part<1>(), xf.template part<0>() - target;
  return ret;
};

/// @brief Range to std::vector
const auto r2v = []<std::ranges::range R>(const R & r) {
  return std::vector(std::ranges::begin(r), std::ranges::end(r));
};

int main()
{
  // define optimal control problem
  smooth::feedback::
    OCP<X<double>, U<double>, decltype(obj), decltype(f), decltype(g), decltype(cr), decltype(ce)>
      ocp{
        .nx    = smooth::Dof<X<double>>,
        .nu    = smooth::Dof<U<double>>,
        .nq    = 1,
        .ncr   = 2,
        .nce   = 10,
        .theta = obj,
        .f     = f,
        .g     = g,
        .cr    = cr,
        .crl   = Vec<double>{{-1, -1}},
        .cru   = Vec<double>{{1, 1}},
        .ce    = ce,
        .cel   = Vec<double>{{3, 0, 0, 0, 0, 0, 0, 0, 0, 0}},
        .ceu   = Vec<double>{{15, 0, 0, 0, 0, 0, 0, 0, 0, 0}},
      };

  const auto xl = []<typename T>(T) -> X<T> { return X<T>::Identity(); };
  const auto ul = []<typename T>(T) -> U<T> { return Eigen::Vector2<T>::Constant(0.01); };

  assert(smooth::feedback::check_ocp(ocp));

  const auto flatocp = smooth::feedback::flatten_ocp(ocp, xl, ul);

  assert(smooth::feedback::check_ocp(flatocp));

  // target optimality
  const double target_err = 1e-6;

  // define mesh
  smooth::feedback::Mesh<5, 10> mesh;

  // declare solution variable
  std::vector<smooth::feedback::OCPSolution<X<double>, U<double>>> sols;
  std::optional<smooth::feedback::NLPSolution> nlpsol;

  const auto t0 = std::chrono::high_resolution_clock::now();

  for (auto iter = 0u; iter < 10; ++iter) {
    std::cout << "---------- ITERATION " << iter << " ----------" << std::endl;
    std::cout << "mesh: " << mesh.N_ivals() << " intervals, " << mesh.N_colloc()
              << " collocation pts" << std::endl;

    // transcribe optimal control problem to nonlinear programming problem
    const auto nlp = smooth::feedback::ocp_to_nlp(flatocp, mesh);

    // solve nonlinear programming problem
    std::cout << "solving..." << std::endl;
    nlpsol = smooth::feedback::solve_nlp_ipopt(
      nlp,
      nlpsol,
      {
        {"print_level", 5},
      },
      {
        {"linear_solver", "mumps"}, {"hessian_approximation", "limited-memory"},
        // {"derivative_test", "first-order"},
        // {"print_timing_statistics", "yes"},
      },
      {
        {"tol", 1e-6},
      });

    // convert solution of nlp insto solution of ocp
    auto flatsol = smooth::feedback::nlpsol_to_ocpsol(flatocp, mesh, nlpsol.value());

    // store unflattened solution
    sols.push_back(smooth::feedback::unflatten_ocpsol<X<double>, U<double>>(flatsol, xl, ul));

    // calculate errors
    auto errs = smooth::feedback::mesh_dyn_error(
      flatocp.nx, flatocp.f, mesh, flatsol.t0, flatsol.tf, flatsol.x, flatsol.u);

    std::cout << "interval errors " << errs.transpose() << std::endl;

    if (errs.maxCoeff() > target_err) {
      smooth::feedback::mesh_refine(mesh, errs, 0.1 * target_err);
      nlpsol = smooth::feedback::ocpsol_to_nlpsol(flatocp, mesh, flatsol);
    } else {
      break;
    }
  }

  const auto dur = std::chrono::high_resolution_clock::now() - t0;

  std::cout << "TOTAL TIME: " << std::chrono::duration_cast<std::chrono::milliseconds>(dur).count()
            << "ms" << std::endl;

#ifdef ENABLE_PLOTTING
  using namespace matplot;

  const auto [nodes, weights] = mesh.all_nodes_and_weights();

  const auto tt       = linspace(0., sols.back().tf, 500);
  const auto tt_nodes = r2v(sols.back().tf * nodes);

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(
      transform(tt, [&](double t) { return sol.x(t).part<0>().r2().x(); }),
      transform(tt, [&](double t) { return sol.x(t).part<0>().r2().y(); }),
      "-r")
      ->line_width(lw);
  }
  legend(std::vector<std::string>{"path"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.x(t).part<1>().x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.x(t).part<1>().y(); }), "-g")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.x(t).part<1>().z(); }), "-b")->line_width(lw);
  }
  legend({"vx", "vy", "wz"});

  figure();
  hold(on);
  plot(tt_nodes, transform(tt_nodes, [](auto) { return 0; }), "xk")->marker_size(10);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.lambda_dyn(t).x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.lambda_dyn(t).y(); }), "-b")->line_width(lw);
  }
  legend({"nodes", "lambda_x", "lambda_y"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.lambda_cr(t).x(); }), "-r")->line_width(lw);
  }
  legend(std::vector<std::string>{"lambda_{cr}"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&sol](double t) { return sol.u(t).x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&sol](double t) { return sol.u(t).y(); }), "-b")->line_width(lw);
  }
  legend({"throttle", "steering"});

  show();
#endif

  return EXIT_SUCCESS;
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * 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.
 *
 *
 * Gicp.cpp
 *
 *  Created on: Jan 31, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include <Eigen/LU> // for inverse and determinant

#include <memory>
#include <iostream>
#include "mrob/pc_registration.hpp" // GICP function is defined here

using namespace mrob;

int PCRegistration::gicp(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y,
           const Eigen::Ref<const MatX> covX, const Eigen::Ref<const MatX> covY, SE3 &T, double tol)
{
    assert(X.cols() == 3  && "PCRegistration::Gicp: Incorrect sizing, we expect Nx3");
    assert(X.rows() >= 3  && "PCRegistration::Gicp: Incorrect sizing, we expect at least 3 correspondences (not aligned)");
    assert(Y.rows() == X.rows()  && "PCRegistration::Gicp: Same number of correspondences");
    uint_t N = X.rows();
    // TODO precalculation of T by reduced Arun
    // TODO different number of iterations and convergence criterion

    // Initialize Jacobian and Hessian
    Mat61 J = Mat61::Zero();
    Mat6 H = Mat6::Zero();
    uint_t iters = 0;
    double deltaUpdate = 1e3;
    do
    {
        J.setZero();
        H.setZero();
        // not vectoried operations (due to Jacobian)
        for ( uint_t i = 0; i < N ; ++i)
        {
            // 1) Calculate residual r = y - Tx and the inverse of joint covariance
            Mat31 Txi = T.transform(X.row(i));
            Mat31 r = Y.row(i).transpose() - Txi;
            Mat3 Li = (covY.block<3,3>(3*i,0) + T.R() * covX.block<3,3>(3*i,0) * T.R().transpose()).inverse();

            // 2) Calculate Jacobian for residual Jf = df1/d xi = r1' Li * Jr, where Jr = [(Tx)^ ; -I])
            Mat<3,6> Jr;
            Jr << hat3(Txi) , -Mat3::Identity();
            Mat<1,6> Ji = r.transpose() * Li * Jr;
            J += Ji;//Eigen manages this for us

            // 3) Hessian Hi ~ Jr' * Li * Jr
            Mat6 Hi = Jr.transpose() * Li * Jr;
            H += Hi;
        }
        // 4) Update Solution
        Mat61 dxi = -H.inverse()*J;
        T.update_lhs(dxi); //Left side update
        deltaUpdate = dxi.norm();
        iters++;

    }while(deltaUpdate > tol && iters < 20);

    return iters; // number of iterations
}


/*
 * Copyright 2009-2019 The VOTCA Development Team (http://www.votca.org)
 *
 * 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 "../../../include/votca/csg/potentialfunctions/potentialfunctioncbspl.h"
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <votca/tools/table.h>

using namespace std;
using namespace votca::tools;

namespace votca {
namespace csg {

PotentialFunctionCBSPL::PotentialFunctionCBSPL(const string &name, Index nlam,
                                               double min, double max)
    : PotentialFunction(name, nlam, min, max) {

  /* Here nlam_ is the total number of coeff values that are to be optimized
   * To ensure that potential and force go to zero smoothly near cut-off,
   * as suggested in Ref. PCCP, 11, 1901, 2009, coeff values leading up to
   * cut-off and beyond take a value of zero.
   *
   * Since region less than rmin is not sampled sufficiently for stability
   * first  nexcl_ coefficients are not optimized instead their values are
   * extrapolated from first statistically significant knot values near rmin
   */

  Index nknots;

  nknots = lam_.size();

  nbreak_ = nknots - 2;

  dr_ = (cut_off_) / (double(nbreak_ - 1));

  // break point locations
  // since ncoeff = nbreak +2 , r values for last two coefficients are also
  // computed
  rbreak_ = Eigen::VectorXd::Zero(nknots);

  for (Index i = 0; i < nknots; i++) {
    rbreak_(i) = double(i) * dr_;
  }

  // exclude knots corresponding to r <=  min_
  nexcl_ = std::min((Index)(min_ / dr_), nbreak_ - 2) + 1;

  // account for finite numerical division of  min_/ dr_
  // e.g. 0.24/0.02 may result in 11.99999999999999
  if (rbreak_(nexcl_) == min_) {
    nexcl_++;
  }

  // fixing last 4 knots to zeros is reasonable
  ncutcoeff_ = 4;

  // check if we have enough parameters to optimize
  if ((Index(lam_.size()) - nexcl_ - ncutcoeff_) < 1) {
    throw std::runtime_error(
        "In potential " + name_ +
        ": no parameters to optimize!\n"
        "All the knot values fall in the range of either excluded (due to high "
        "repulsive region) or cut-off region.\n"
        "This issue can be resolved by one or combination of following steps:\n"
        "1. Make sure you are using large-enough cut-off for this CG "
        "potential.\n"
        "2. Make sure the CG-MD runs are sufficiently Index and CG-MD RDF are "
        "statistically reliable.\n"
        "3. Use more knot values.\n");
  }

  M_ = Eigen::MatrixXd::Zero(4, 4);
  M_(0, 0) = 1.0;
  M_(0, 1) = 4.0;
  M_(0, 2) = 1.0;
  M_(0, 3) = 0.0;
  M_(1, 0) = -3.0;
  M_(1, 1) = 0.0;
  M_(1, 2) = 3.0;
  M_(1, 3) = 0.0;
  M_(2, 0) = 3.0;
  M_(2, 1) = -6.0;
  M_(2, 2) = 3.0;
  M_(2, 3) = 0.0;
  M_(3, 0) = -1.0;
  M_(3, 1) = 3.0;
  M_(3, 2) = -3.0;
  M_(3, 3) = 1.0;
  M_ /= 6.0;
}

Index PotentialFunctionCBSPL::getOptParamSize() const {

  return lam_.size() - nexcl_ - ncutcoeff_;
}

void PotentialFunctionCBSPL::setParam(string filename) {

  Table param;
  param.Load(filename);
  lam_.setZero();

  if (param.size() != lam_.size()) {

    throw std::runtime_error("In potential " + name_ +
                             ": parameters size mismatch!\n"
                             "Check input parameter file \"" +
                             filename + "\" \nThere should be " +
                             boost::lexical_cast<string>(lam_.size()) +
                             " parameters");
  } else {
    // force last  ncutcoeff_ to zero
    Index nonzero = lam_.size() - ncutcoeff_;
    lam_.head(nonzero) = param.y().head(nonzero);
  }
}

void PotentialFunctionCBSPL::SaveParam(const string &filename) {

  extrapolExclParam();

  Table param;
  param.SetHasYErr(false);
  param.resize(lam_.size());

  // write extrapolated knots with flag 'o'
  // points close to rmin can also be stastically not reliable
  // so flag 3 more points next to rmin as 'o'
  for (Index i = 0; i < nexcl_ + 3; i++) {
    param.set(i, rbreak_(i), lam_(i), 'o');
  }

  for (Index i = nexcl_ + 3; i < lam_.size(); i++) {
    param.set(i, rbreak_(i), lam_(i), 'i');
  }

  param.Save(filename);
}

void PotentialFunctionCBSPL::SavePotTab(const string &filename, double step,
                                        double rmin, double rcut) {
  extrapolExclParam();
  PotentialFunction::SavePotTab(filename, step, rmin, rcut);
}

void PotentialFunctionCBSPL::SavePotTab(const string &filename, double step) {
  extrapolExclParam();
  PotentialFunction::SavePotTab(filename, step);
}

void PotentialFunctionCBSPL::extrapolExclParam() {

  double u0 = lam_(nexcl_);
  double m = (lam_(nexcl_ + 1) - lam_(nexcl_)) /
             (rbreak_(nexcl_ + 1) - rbreak_(nexcl_));
  double r0 = rbreak_(nexcl_);

  /* If the slope m is positive then the potential core
   * will be attractive. So, artificially forcing core to be
   * repulsive by setting m = -m
   */
  if (m > 0) {
    cout << name_ << " potential's extrapolated core is attractive!" << endl;
    cout << "Artifically enforcing repulsive core.\n" << endl;
    m *= -1.0;
  }
  // using linear extrapolation
  // u(r) = ar + b
  // a = m
  // b = - m*r0 + u0
  // m = (u1-u0)/(r1-r0)

  double a = m;
  double b = -1.0 * m * r0 + u0;
  for (Index i = 0; i < nexcl_; i++) {
    lam_(i) = a * rbreak_(i) + b;
  }
}

void PotentialFunctionCBSPL::setOptParam(Index i, double val) {

  lam_(i + nexcl_) = val;
}

double PotentialFunctionCBSPL::getOptParam(Index i) const {

  return lam_(i + nexcl_);
}

double PotentialFunctionCBSPL::CalculateF(double r) const {

  if (r <= cut_off_) {

    double u = 0.0;
    Index indx = std::min((Index)(r / dr_), nbreak_ - 2);
    double rk = (double)indx * dr_;
    double t = (r - rk) / dr_;

    Eigen::Vector4d R = Eigen::Vector4d::Zero();
    R(0) = 1.0;
    R(1) = t;
    R(2) = t * t;
    R(3) = t * t * t;
    Eigen::Vector4d B = lam_.segment<4>(indx);
    u += ((R.transpose() * M_) * B).value();
    return u;

  } else {
    return 0.0;
  }
}

// calculate first derivative w.r.t. ith parameter
double PotentialFunctionCBSPL::CalculateDF(Index i, double r) const {

  // since first  nexcl_ parameters are not optimized for stability reasons

  if (r <= cut_off_) {

    Index i_opt = i + nexcl_;
    Index indx;
    double rk;

    indx = std::min((Index)(r / dr_), nbreak_ - 2);
    rk = (double)indx * dr_;

    if (i_opt >= indx && i_opt <= indx + 3) {

      Eigen::Vector4d R = Eigen::Vector4d::Zero();

      double t = (r - rk) / dr_;

      R(0) = 1.0;
      R(1) = t;
      R(2) = t * t;
      R(3) = t * t * t;

      Eigen::Vector4d RM = R.transpose() * M_;

      return RM(i_opt - indx);

    } else {
      return 0.0;
    }

  } else {
    return 0.0;
  }
}

// calculate second derivative w.r.t. ith parameter
double PotentialFunctionCBSPL::CalculateD2F(Index, Index, double) const {

  return 0.0;
}

}  // namespace csg
}  // namespace votca


#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "sensor_msgs/Joy.h"
#include "create_driver/vicon_driver.h"
#include "geometry_msgs/Twist.h"

#include "create_controller/ControlMsgs.h"

#include <string>
#include <vector>
#include <map>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>

#define PI 3.141592
using namespace Eigen;
using namespace std;
Vector3d position(-5,-5,-5);

void position_callback(const geometry_msgs::Vector3::ConstPtr& msg)
{
	position(0) = msg->x;
	position(1) = msg->y;
	position(2) = msg->z;
}

int main(int argc, char **argv)
{
	// ROS Initalization
	ros::init(argc, argv, "falcon_ellipse_feedback");

	ros::NodeHandle n;
	ros::NodeHandle private_n("~");

	// List of robot names
	vector<string> robot_names;
	XmlRpc::XmlRpcValue robot_list;
	private_n.getParam("robot_list", robot_list);
	ROS_ASSERT(robot_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < robot_list.size(); i++)
	{
		ROS_ASSERT(robot_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
		robot_names.push_back(static_cast<string>(robot_list[i]));
	}

	// Number of robots
	const int num_robots = robot_names.size();

	// ROS Subscribers
	map<string, create_driver::ViconStream> vicon;
	vector<string>::iterator name_it;
	vector<ros::Subscriber> sub;
	for (name_it = robot_names.begin(); name_it != robot_names.end(); name_it++)
	{
		vicon.insert(pair<string, create_driver::ViconStream>(*name_it, create_driver::ViconStream()));
		sub.push_back(n.subscribe("/" + *name_it + "/tf", 10, &create_driver::ViconStream::callback, &vicon[*name_it]));
	}

	sub.push_back(n.subscribe("position", 10, position_callback));

	ros::Publisher joy_pub = n.advertise<sensor_msgs::Joy>("joy", 10);
	ros::Publisher force_pub = n.advertise<geometry_msgs::Vector3>("force", 10);

	// ROS loop
	ros::Rate loop_rate(1000); // 1 kHz
	bool uninitialized = true;
	while (ros::ok())
	{
		ros::spinOnce();

		Vector3d joy(position(0)/0.06,  position(1)/0.06, (2.0*(position(2)-0.073))/0.1035 - 1.0);

		Vector3d force;
		double resistance;

		double p[num_robots];
		double q[num_robots];

		for (int i = 0; i < num_robots; i++) {
			p[i] = cos(vicon[robot_names[i]].theta());
			q[i] = sin(vicon[robot_names[i]].theta());
		}

		double p_bar = 0.0;
		double q_bar = 0.0;

		for (int i = 0; i < num_robots; i++) {
			p_bar += p[i]/num_robots;
			q_bar += q[i]/num_robots;
		}

		double r = sqrt(pow(p_bar, 2) + pow(q_bar, 2))/2.0;

		double phi = atan2(q_bar, p_bar);

		double b = 0.2;

		double a = sqrt(pow(r, 2) + pow(b, 2));

		double x = joy(0) - r*cos(phi);
		double y = joy(1) - r*sin(phi);

		double x_barE = x * cos(phi) + y*sin(phi);
		double y_barE = -x * sin(phi) + y*cos(phi);

		double c = pow(x_barE,2) / pow(a,2) + pow(y_barE,2) / pow(b,2);

		double lowerbound = 1.0;
		double upperbound = 25.0;
		double gain = 0.0;
		if(lowerbound < c && c < upperbound){
			gain = (-500)*(pow(upperbound,2) - pow(lowerbound,2) )*(pow(c,2) - pow(lowerbound,2) ) / pow((pow(c,2) - pow(upperbound,2) ),3);

		}

		double v_bar1 = -2 * x_barE/pow(a,2);
		double v_bar2 = -2 * y_barE/pow(b,2);

		double v_bar_norm = sqrt(pow(v_bar1, 2) + pow(v_bar2, 2));

		Vector3d v_bar(v_bar1*cos(phi)-v_bar2*sin(phi), v_bar1*sin(phi)+v_bar2*cos(phi), 0);
		force = gain*v_bar_norm*v_bar;
		// if (joy.norm()<0.25) {
		// 	resistance =1;
		// } else {
		// 	double radius = joy.norm() - 0.25;

		// 	double sum_etheta = fabs(etheta[0])+ fabs(etheta[1])+fabs(etheta[2])+fabs(etheta[3]);
		// 	resistance = 1; // + 6 * (radius * sum_etheta);
		// 	printf("%f\n", sum_etheta);
		// }

		// for (int i = 0; i < 3; i++)
		// {
		// 	force(i) = -5*joy(i)*resistance;
		// }

		geometry_msgs::Vector3 fmsg;
		fmsg.x = force(0);
		fmsg.y = force(1);
		fmsg.z = -joy(2)*5;
		force_pub.publish(fmsg);

		// Deadband
		if (joy.norm() < 0.25) {
			for (int i = 0; i < 2; ++i)
			{
				joy(i) =0;
			}
		}

		sensor_msgs::Joy jmsg;
		jmsg.axes.resize(3);
		jmsg.axes[0] = -joy(0);//position(0)/0.06; // -1.0 to 1.0
		jmsg.axes[1] = joy(1);//position(1)/0.06;
		jmsg.axes[2] = joy(2);//(2.0*(position(2)-0.073))/0.1035 - 1.0;//32767
		joy_pub.publish(jmsg);

		loop_rate.sleep();
	}

	return 0;
}


#include <chrono>
#include <pcl/io/ply_io.h>
#include <Eigen/Dense>

#include "../Edge_Extraction/Difference_Eigenvalues.h"
#include "../hough3d-code/hough3dlines.h"
#include "../hough3d-code/vector3d.h"
#include "icp.h"
#include "filter.h"
#include "findTransformation.h"
#include "intersection.h"
#include "resultWriter.h"
#include "line_descriptor.h"
#include "boundingBox.h"

#include <pcl/segmentation/cpc_segmentation.h>

using namespace boost::system;
namespace filesys = boost::filesystem;

bool calculateOverlappingRatio = false;

int main(int argc, char *argv[]){

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ>);

    const std::string input_fileA = argv[1];
    const std::string input_fileB = argv[2];
    const std::string edges_output_file = argv[3];
    const std::string lines_output_file = argv[4];
    const std::string filtered_statistical_output_file = argv[5];
    const std::string filtered_median_output_file = argv[6];
    const std::string merged_output_file = argv[7];

    if( pcl::io::loadPLYFile (input_fileA, *cloudA) == -1 ){

        std::cerr << "File not found\n";
        return -1;
    }

    std::cout << input_fileA << " loaded.\n";

    if(  pcl::io::loadPLYFile (input_fileB, *cloudB) == -1 ){

        std::cerr << "File not found\n";
        return -1;
    }

    std::cout << input_fileB << " loaded.\n";

    //copy original PointClouds to a buffer to use them in the final merging step
    ResultWriter resultWriter(*cloudA, *cloudB);

    std::chrono::steady_clock::time_point begin;
    std::chrono::steady_clock::time_point end;

    //3D convolve
    begin = std::chrono::steady_clock::now();
       applyGaussianKernel(cloudA, filtered_median_output_file, true);
       applyGaussianKernel(cloudB, filtered_median_output_file+"2.ply", true);
    end = std::chrono::steady_clock::now();
    std::cout << "3D convolve:  " << std::chrono::duration_cast<std::chrono::seconds>(end - begin).count() << "s" << std::endl;

    //Extract edges
    begin = std::chrono::steady_clock::now();
        pcl::PointCloud<pcl::PointXYZ>::Ptr extractedEdgesA = extract_edges(cloudA, edges_output_file, true);
        pcl::PointCloud<pcl::PointXYZ>::Ptr extractedEdgesB = extract_edges(cloudB, edges_output_file+"2.ply", true);
    end = std::chrono::steady_clock::now();
    std::cout << "Edge extraction:  " << std::chrono::duration_cast<std::chrono::seconds>(end - begin).count() << "s" << std::endl;

    //Filter
    begin = std::chrono::steady_clock::now();
        applyStatisticalOutlierFilter(extractedEdgesA, filtered_statistical_output_file, true);
        applyStatisticalOutlierFilter(extractedEdgesB, filtered_statistical_output_file+"2.ply", true);
    end = std::chrono::steady_clock::now();
    std::cout << "Filtering:  " << std::chrono::duration_cast<std::chrono::seconds>(end - begin).count() << "s" << std::endl;   begin = std::chrono::steady_clock::now();

    //Center pointcloud A on top of XY plane

        //Calculate bounding box
        const size_t sizeA =  extractedEdgesA -> points.size();

        BoundingBox bbA, bbB;

        bbA.setCoefficients(extractedEdgesA);

        const float xShift = (bbA.minX + bbA.maxX) / 2.0;
        const float yShift = (bbA.minY + bbA.maxY) / 2.0;

        std::cout << "Shifting by X: " << -xShift << " Y:" << -yShift << " Z:" << -bbA.minZ << std::endl;

        //Center cloud A
        for(size_t i = 0; i < sizeA; ++i){

            extractedEdgesA -> points[i].x -= xShift;
            extractedEdgesA -> points[i].y -= yShift;
            extractedEdgesA -> points[i].z -= bbA.minZ;

         }

        //Center cloud B
        for(size_t i = 0; i < extractedEdgesB -> points.size(); ++i){

            extractedEdgesB -> points[i].x -= xShift;
            extractedEdgesB -> points[i].y -= yShift;
            extractedEdgesB -> points[i].z -= bbA.minZ;

         }

        bbA.minX -= xShift;
        bbA.maxX -= xShift;
        bbA.minY -= yShift;
        bbA.maxY -= yShift;
        bbA.maxZ -= bbA.minZ;
        bbA.minZ  = 0;

    end = std::chrono::steady_clock::now();
    std::cout << "Centering pointclouds:  " << std::chrono::duration_cast<std::chrono::seconds>(end - begin).count() << "s" << std::endl;

    //Hough transform
    begin = std::chrono::steady_clock::now();
        std::vector<Vector3d> pcA;
        std::vector<Vector3d> pcB;

        for (size_t i = 0; i < extractedEdgesA -> points.size (); ++i)
            pcA.push_back(Vector3d(extractedEdgesA->points[i].x, extractedEdgesA->points[i].y, extractedEdgesA->points[i].z));

         for (size_t i = 0; i < extractedEdgesB -> points.size (); ++i)
            pcB.push_back(Vector3d(extractedEdgesB->points[i].x, extractedEdgesB->points[i].y, extractedEdgesB->points[i].z));

        Eigen::MatrixXf detected_linesA = hough_transform(pcA, lines_output_file, true);
        Eigen::MatrixXf detected_linesB = hough_transform(pcB, lines_output_file+"2.ply", true);

    end = std::chrono::steady_clock::now();
    std::cout << "Hough transform:  " << std::chrono::duration_cast<std::chrono::seconds>(end - begin).count() << "s" << std::endl;

    //x y score
    std::tuple<float, float, float> maxTranslation = std::tuple<float, float, float>(0, 0, -1);
    float maximizingAngle;

    begin = std::chrono::steady_clock::now();
    std::vector<float> possibleRotations = findPossibleRotations(detected_linesA, detected_linesB);
    end = std::chrono::steady_clock::now();
    std::cout << "Rotation angle search:  " << std::chrono::duration_cast<std::chrono::seconds>(end - begin).count() << "s" << std::endl;

    begin = std::chrono::steady_clock::now();
    for(const auto angle: possibleRotations){

       std::cout << "Trying angle " << angle << "..." << std::endl;

        pcl::PointCloud<pcl::PointXYZ>::Ptr tempB(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::transformPointCloud(*extractedEdgesB, *tempB, calculateTMatrix(angle, 0, 0));

        bbB.setCoefficients(tempB);

        //bbA.print();
        //bbB.print();

        //USEFUL FOR DEBUGGING
        //pcl::PLYWriter w;
        //w.write("experimental1.ply", *extractedEdgesA, true, false);
        //w.write("experimental2.ply", *tempB, true, false);

       std::tuple<float, float, float> translation = findTranslation(angle, detected_linesA, detected_linesB, bbA, bbB);//min_y, max_y, min_yB, max_yB );

        if(std::get<2>(maxTranslation) < std::get<2>(translation)){
            maxTranslation = translation;
            maximizingAngle = angle;
        }

    }
    end = std::chrono::steady_clock::now();
    std::cout << "Transformation search:  " << std::chrono::duration_cast<std::chrono::seconds>(end - begin).count() << "s" << std::endl;

    float tX = std::get<0>(maxTranslation);
    float tY = std::get<1>(maxTranslation);

    std::cout << "Transformation parameters:    angle: " << maximizingAngle << "        X: " << tX << "         Y: " << tY << std::endl;

    //PerformTransformation based on line matching

    resultWriter.saveTransformedPointClouds( merged_output_file+"original_config.ply" );

    //BUG: sometimes these 2 lines need to be commented out, sometimes not. We suggest comparing to experimental2.ply to find out whether to comment them out or not.
    //resultWriter.transformDynamicPointCloud( calculateTMatrix(0, -xShift, -yShift, -bbA.minZ) );
    resultWriter.transformDynamicPointCloud( calculateTMatrix(maximizingAngle, 0, 0) );
    //resultWriter.transformDynamicPointCloud( calculateTMatrix(0, xShift, yShift, bbA.minZ) );

    resultWriter.transformDynamicPointCloud( calculateTMatrix(0, 0, tY) );
    resultWriter.saveTransformedPointClouds( merged_output_file+"rotated_ty.ply" );

    resultWriter.transformDynamicPointCloud( calculateTMatrix(0, tX, 0) );
    resultWriter.saveTransformedPointClouds( merged_output_file+"rotated_ty_tx.ply" );

    resultWriter.saveTransformedPointClouds( merged_output_file+"before_icp.ply" );

    pcl::transformPointCloud(*extractedEdgesA, *extractedEdgesA, calculateTMatrix(0, xShift, yShift, bbA.minZ) );
    pcl::transformPointCloud(*extractedEdgesB, *extractedEdgesB, calculateTMatrix(maximizingAngle, tX + xShift, tY + yShift, bbA.minZ) );

    IntersectionInterface ii;
    ii.setStaticPointCloud(extractedEdgesA);
    ii.setDynamicPointCloud(extractedEdgesB);
    pcl::CropHull<pcl::PointXYZ> intersectionFilter = ii.getIntersectionFilter();

    pcl::PointCloud<pcl::PointXYZ>::Ptr iA(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr iB(new pcl::PointCloud<pcl::PointXYZ>);

    intersectionFilter.setInputCloud( resultWriter.getStaticPointCloud() );
    intersectionFilter.filter(*iA);

    intersectionFilter.setInputCloud( resultWriter.getDynamicPointCloud() );
    intersectionFilter.filter(*iB);

    pcl::PLYWriter w;
    //USEFUL FOR DEBUGGING
    //w.write("hullA.ply", *iA, true, false);
    //w.write("hullB.ply", *iB, true, false);
    //std::cout << "hull files written" << std::endl;

    std::cout << "Running ICP matching..." << std::endl;
    begin = std::chrono::steady_clock::now();
        Eigen::Matrix4f icpCorrectionTransformation = registerPointClouds(iA, iB);
    end = std::chrono::steady_clock::now();
    std::cout << "ICP matching finished:  " << std::chrono::duration_cast<std::chrono::seconds>(end - begin).count() << "s" << std::endl;

    resultWriter.transformDynamicPointCloud( icpCorrectionTransformation );
    resultWriter.saveTransformedPointClouds( merged_output_file );
    std::cout << "ICP corrected pointcloud written" << std::endl;

    if(calculateOverlappingRatio){

        std::cout << "Calculating overlapping ratio..." << std::endl;

        pcl::PointCloud<pcl::PointXYZ>::Ptr correctedEdgesB(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::transformPointCloud(*extractedEdgesB, *correctedEdgesB, icpCorrectionTransformation);

        IntersectionInterface iiOverlap;
        iiOverlap.setStaticPointCloud(extractedEdgesA);
        iiOverlap.setDynamicPointCloud(correctedEdgesB);
        double intersectionVolume = iiOverlap.getIntersectionHull().getTotalVolume();

        pcl::ConvexHull<pcl::PointXYZ> hullTotal;

        pcl::PointCloud<pcl::PointXYZ>::Ptr mergedPC(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::transformPointCloud(*cloudB, *mergedPC, icpCorrectionTransformation * calculateTMatrix(maximizingAngle, tX + xShift, tY + yShift, bbA.minZ));

        *mergedPC += *cloudA;

        hullTotal.setInputCloud(mergedPC);
        double totalVolume = hullTotal.getTotalVolume();

        std::cout << "Intersection volume: " << intersectionVolume << std::endl;
        std::cout << "Total volume: " << totalVolume << std::endl;

        std::cout << "Overlapping ratio: " << intersectionVolume / totalVolume << std::endl;

    }

}


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#define _USE_MATH_DEFINES
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include "specialtypes.hpp"
#include "ublas_cholesky.hpp"
#include "posterior_mcmc.hpp"

namespace bnu = boost::numeric::ublas;

int determinant_sign(const bnu::permutation_matrix<std::size_t>& pm)
{
  int pm_sign=1;
  std::size_t size = pm.size();
  for (std::size_t i = 0; i < size; ++i)
    if (i != pm(i))
      pm_sign *= -1.0; // swap_rows would swap a pair of rows here, so we change sign
  return pm_sign;
}

double determinant(bnu::matrix<double>& m ) {
  bnu::permutation_matrix<std::size_t> pm(m.size1());
  double det = 1.0;
  if( bnu::lu_factorize(m,pm) ) {
    det = 0.0;
  } else {
    for(int i = 0; i < m.size1(); i++)
      det *= m(i,i); // multiply by elements on diagonal
    det = det * determinant_sign( pm );
  }
  return det;
}

double gauss(const vectord& x, const vectord& mu, const matrixd& sigma)
{
  const double tpi = boost::math::constants::two_pi<double>();
  double n = static_cast<double>(x.size());
  const vectord vd = x-mu;
  matrixd invS = sigma;
  bayesopt::utils::inverse_cholesky(sigma,invS);
  matrixd sig = sigma;

  return pow(tpi,n/2)*pow(determinant(sig),0.5)*exp(-0.5*inner_prod(vd,prod(invS,vd)));
}

class Posterior: public bayesopt::RBOptimizable
{
  double evaluate(const vectord& x)
  {
    vectord mu1(2), mu2(2), mu3(2);
    matrixd s1(2,2), s2(2,2), s3(2,2);

    mu1 <<= 0,0;
    mu2 <<= 1,1;
    mu3 <<= -4,2;

    s1 <<= 1,0,
      0,1;

    s2 <<= 4,0,
      0,0.6;

    s3 <<= 4,0,
      0,0.6;

    return gauss(x,mu1,s1) + gauss(x,mu2,s2) + gauss(x,mu3,s3);
  }
};

int main()
{
  randEngine reng;
  Posterior post;
  bayesopt::MCMCSampler sampler(&post,2,reng);
  vectord x = zvectord(2);
  sampler.run(x);
  sampler.printParticles();

  return 0;
}


/**
 * @file gravity_filter.hpp
 * @author Fujii Naomichi
 * @copyright (c) 2021 Fujii Naomichi
 * SPDX-License-Identifier: MIT
 */

#pragma once

#include <math.h>
#include <fpu.hpp>
#include <Eigen/Core>
#include "board.hpp"

/**
 * @brief IMUの測定値から重力の影響を取り除くフィルタ
 */
class GravityFilter {
public:
    /**
     * @brief 内部状態をリセットする
     */
    void reset(void) {
        _initialized = false;
    }

    /**
     * @brief フィルタに新たな入力を与えて出力を更新する
     * @param accel 加速度センサーの測定値
     * @param gyro ジャイロスコープの測定値
     */
    void update(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro) {
        using namespace Eigen;

        static constexpr float GYRO_GAIN_P = 1.0;
        static constexpr float GYRO_GAIN_I = 0.001;
        static constexpr float GRAVITY_LOW_THRESHOLD = 0.0625f;
        static constexpr float GRAVITY_COMPENSATION = 0.001f;

        if (!_initialized) {
            // 重力加速度ベクトルを初期化する
            _initialized = true;
            _gravity = accel;
            _gyro_error_integ = Vector3f::Zero();
        }

        // 加速度ベクトルと重力加速度ベクトルの成す角度を求める
        Vector3f gyro_error;
        float gravity_scale = fpu::sqrt(_gravity.squaredNorm());
        if (GRAVITY_LOW_THRESHOLD < gravity_scale) {
            gyro_error = accel.cross(_gravity) / (gravity_scale * gravity_scale);
            _gyro_error_integ += gyro_error;
        }
        else {
            // 重力が異様に小さいときは加速度センサーによる角速度の補正を減らす
            gyro_error = Vector3f::Zero();
            _gyro_error_integ *= fpu::max(1.0f - 1.0f / IMU_OUTPUT_RATE, 0.0f);
        }

        // 角速度を加速度センサーから得た角度誤差で補正する
        Vector3f delta_omega = GYRO_GAIN_P * gyro_error + GYRO_GAIN_I * _gyro_error_integ;
        _compensated_gyro = gyro + delta_omega;

        // 重力ベクトルを回転する
        Matrix3f Rt = rotationMatrixTransposed(_compensated_gyro * (1.0f / IMU_OUTPUT_RATE));
        _gravity = Rt * _gravity;

        // 重力加速度ベクトルの大きさを徐々に加速度の大きさに近づける
        // 重力が小さいときは大きさではなくベクトルそのものを使って補正する
        float accel_scale = fpu::sqrt(accel.squaredNorm());
        if (GRAVITY_LOW_THRESHOLD < fpu::min(accel_scale, gravity_scale)) {
            _gravity *= ((1.0f - GRAVITY_COMPENSATION) + GRAVITY_COMPENSATION * accel_scale / gravity_scale);
        }
        else {
            _gravity = (1.0f - GRAVITY_COMPENSATION) * _gravity + GRAVITY_COMPENSATION * accel;
        }

        // 加速度ベクトルから重力の影響を除去する
        _compensated_accel = accel - _gravity;
    }

    /**
     * @brief 重力加速度ベクトル(の反力)を取得する
     * @return 重力加速度ベクトル(の反力) X, Y, Z [m/s^2]
     */
    const Eigen::Vector3f& gravity(void) const {
        return _gravity;
    }

    /**
     * @brief 重力を除去済みの加速度を取得する
     * @return 加速度 X, Y, Z [m/s^2]
     */
    const Eigen::Vector3f& acceleration(void) const {
        return _compensated_accel;
    }

    /**
     * @brief 重力で補正済みの角速度を取得する
     * @return 角速度 X, Y, Z [rad/s]
     */
    const Eigen::Vector3f& angularVelocity(void) const {
        return _compensated_gyro;
    }

private:
    static Eigen::Matrix3f rotationMatrixTransposed(const Eigen::Vector3f& gyro) {
        float z = gyro.z();
        float y = gyro.y();
        float x = gyro.x();
        return Eigen::Matrix3f{
            {1.0f, x * y + z, x * z - y},
            {-z, 1.0f - x * y * z, x + y * z},
            {y, -x, 1.0f},
        };
    }

    bool _initialized = false;
    Eigen::Vector3f _gravity;
    Eigen::Vector3f _compensated_accel;
    Eigen::Vector3f _compensated_gyro;
    Eigen::Vector3f _gyro_error_integ;
};


/**
  This file creates a one-layer neural network to calculate the beat from 16
  inputs.

  We want range of tempo: 35 - 250 bpm //TODO
*/
#include "exception.hh"
#include "network.hh"
#include "timer.hh"

#include <Eigen/Dense>
#include <iostream>
#include <random>
#include <utility>

#include <sys/resource.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;

constexpr size_t batch_size = 1;
constexpr size_t input_size = 16;

/* use squared error as loss function */
float loss_function( const float target, const float actual )
{
  return ( target - actual ) * ( target - actual );
}

/* partial derivative of loss with respect to neural network output */
float compute_pd_loss_wrt_output( const float target, const float actual )
{
  return -2 * ( target - actual );
}

/* compute input */
Matrix<float, batch_size, input_size> gen_time( float tempo, float offset )
{
  Matrix<float, batch_size, input_size> ret_mat;
  for ( auto i = 0; i < 16; i++ ) {
    ret_mat( i ) = tempo * i + offset;
  }
  return ret_mat;
}

float learning_rate = 0.00001;

void program_body()
{
  /* remove limit on stack size */
  const rlimit limits { RLIM_INFINITY, RLIM_INFINITY };
  CheckSystemCall( "setrlimit", setrlimit( RLIMIT_STACK, &limits ) );

  /* seed C RNG for Eigen random weight initialization */
  srand( Timer::timestamp_ns() );

  /* construct neural network on heap */
  auto nn = make_unique<Network<float, batch_size, input_size, 1>>();
  nn->layer0.initializeWeightsRandomly();

  int tempo = 50;
  float offset = 0;
  for ( tempo = 70; tempo > 50; tempo-- ) {
    /* test true function */
    int i = 0;
    while ( true ) {
      if ( i == 5 )
        break;
      i += 1;
      /* step 1: construct a unique problem instance */
      Matrix<float, batch_size, input_size> input = gen_time( tempo, offset );

      /* step 2: forward propagate and calculate loss functiom */
      nn->apply( input );
      cout << "nn maps input: tempo: " << tempo << " offset " << offset << " => " << nn->output()( 0, 0 ) << endl;

      /* step 3: backpropagate error */
      nn->computeDeltas();
      nn->evaluateGradients( input );

      const float pd_loss_wrt_output = compute_pd_loss_wrt_output( tempo, nn->output()( 0, 0 ) );
      cout << "Original loss: " << pd_loss_wrt_output << "\n";

      // TODO: static eta -> dynamic eta
      auto four_third_lr = 4.0 / 3 * learning_rate;
      auto two_third_lr = 2.0 / 3 * learning_rate;

      /* calculate three loss */
      float current_loss = loss_function( nn->output()( 0, 0 ), tempo );
      Matrix<float, input_size, 1> current_weights;
      for ( int j = 0; j < 16; j++ ) {
        current_weights( j ) = nn->layer0.weights()( j );
      }
      auto current_biase = nn->layer0.biases()( 0 );

      /* loss for 4/3 eta */
      for ( int j = 0; j < 16; j++ ) {
        nn->layer0.weights()( j ) -= four_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, j );
      }
      nn->layer0.biases()( 0 ) -= four_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 16 );
      nn->apply( input );
      auto loss_four_third_lr = loss_function( nn->output()( 0, 0 ), tempo );

      /* loss for 2/3 eta */
      for ( int j = 0; j < 16; j++ ) {
        nn->layer0.weights()( j )
          = current_weights( j ) - two_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, j );
      }
      nn->layer0.biases()( 0 )
        = current_biase - two_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 16 );
      nn->apply( input );
      auto loss_two_third_lr = loss_function( nn->output()( 0, 0 ), tempo );

      cout << current_loss << " " << loss_four_third_lr << " " << loss_two_third_lr << endl;
      auto min_loss = min( min( current_loss, loss_four_third_lr ), loss_two_third_lr );
      if ( min_loss == current_loss ) {
        learning_rate *= 2.0 / 3;
        for ( int j = 0; j < 16; j++ ) {
          nn->layer0.weights()( j ) = current_weights( j );
        }
        nn->layer0.biases()( 0 ) = current_biase;
      } else if ( min_loss == loss_four_third_lr ) {
        learning_rate *= 4.0 / 3;
        for ( int j = 0; j < 16; j++ ) {
          nn->layer0.weights()( j )
            = current_weights( j ) - four_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, j );
        }
        nn->layer0.biases()( 0 )
          = current_biase - four_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 16 );
      } else {
        for ( int j = 0; j < 16; j++ ) {
          nn->layer0.weights()( j )
            = current_weights( j ) - two_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, j );
        }
        nn->layer0.biases()( 0 )
          = current_biase - two_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 16 );
      }
      cout << "weights: " << nn->layer0.weights() << endl;
      cout << "biase: " << nn->layer0.biases()( 0 ) << endl;
    }
  }
  for ( int i = 40; i < 80; i++ ) {
    Matrix<float, batch_size, input_size> input = gen_time( i, 0 );
    nn->apply( input );
    cout << "input: " << i << " output: " << nn->output()( 0, 0 ) << endl;
    // cout << nn->output()( 0, 0 ) << endl;
    //  cout << i << endl;
  }
  cout << "yay!" << endl;
}

int main( int argc, char*[] )
{
  try {
    if ( argc <= 0 ) {
      abort();
    }

    program_body();

    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


/****************************************************************************
 * Copyright (c) 2012-2020 by the DataTransferKit authors                   *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the DataTransferKit library. DataTransferKit is     *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include <Teuchos_UnitTestHarness.hpp>

#include <Kokkos_Core.hpp>

#include <DTK_CompactlySupportedRadialBasisFunctions.hpp>

#include <boost/math/tools/polynomial.hpp>
#include <boost/math/tools/rational.hpp>

template <typename DeviceType, typename RadialBasisFunction>
void check_polynomial( boost::math::tools::polynomial<double> const &poly,
                       std::vector<double> const &radii,
                       RadialBasisFunction const &rbf,
                       Teuchos::FancyOStream &out, bool &success )
{
    using ExecutionSpace = typename DeviceType::execution_space;
    int const n = radii.size();
    Kokkos::View<double *, DeviceType> r( "radii", n );
    auto r_host = Kokkos::create_mirror_view( r );
    for ( int i = 0; i < n; ++i )
        r_host( i ) = radii[i];
    Kokkos::deep_copy( r, r_host );

    std::vector<double> p = poly.data();
    std::vector<double> values;
    for ( auto const &x : radii )
        values.push_back(
            boost::math::tools::evaluate_polynomial( p.data(), x, p.size() ) );

    Kokkos::View<double *, DeviceType> v( "values", n );
    Kokkos::parallel_for( "evaluate",
                          Kokkos::RangePolicy<ExecutionSpace>( 0, n ),
                          KOKKOS_LAMBDA( int i ) { v( i ) = rbf( r( i ) ); } );
    Kokkos::fence();
    auto v_host = Kokkos::create_mirror_view( v );
    Kokkos::deep_copy( v_host, v );
    double const relative_tolerance = 1.0e-8;
    TEST_COMPARE_FLOATING_ARRAYS( v_host, values, relative_tolerance );
}

TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( CompactlySupportedRadialBasisFunctions,
                                   polynomial_rbf, DeviceType )
{
    int const n = 10;
    std::vector<double> r( n );
    for ( int i = 0; i < n; ++i )
        r[i] = static_cast<double>( i ) / n;

    check_polynomial<DeviceType>(
        boost::math::tools::pow(
            boost::math::tools::polynomial<double>{1.0, -1.0}, 2 ),
        r, DataTransferKit::Wendland<0>(), out, success );

    check_polynomial<DeviceType>(
        boost::math::tools::pow(
            boost::math::tools::polynomial<double>{1.0, -1.0}, 4 ) *
            boost::math::tools::polynomial<double>{1.0, 4.0},
        r, DataTransferKit::Wendland<2>(), out, success );

    check_polynomial<DeviceType>(
        boost::math::tools::pow(
            boost::math::tools::polynomial<double>{1.0, -1.0}, 6 ) *
            boost::math::tools::polynomial<double>{3.0, 18.0, 35.0},
        r, DataTransferKit::Wendland<4>(), out, success );

    check_polynomial<DeviceType>(
        boost::math::tools::pow(
            boost::math::tools::polynomial<double>{1.0, -1.0}, 8 ) *
            boost::math::tools::polynomial<double>{1.0, 8.0, 25.0, 32.0},
        r, DataTransferKit::Wendland<6>(), out, success );

    check_polynomial<DeviceType>(
        boost::math::tools::pow(
            boost::math::tools::polynomial<double>{1.0, -1.0}, 4 ) *
            boost::math::tools::polynomial<double>{4.0, 16.0, 12.0, 3.0},
        r, DataTransferKit::Wu<2>(), out, success );

    check_polynomial<DeviceType>(
        boost::math::tools::pow(
            boost::math::tools::polynomial<double>{1.0, -1.0}, 6 ) *
            boost::math::tools::polynomial<double>{6.0, 36.0, 82.0, 72.0, 30.0,
                                                   5.0},
        r, DataTransferKit::Wu<4>(), out, success );
}

TEUCHOS_UNIT_TEST_TEMPLATE_1_DECL( CompactlySupportedRadialBasisFunctions,
                                   wrap_rbf, DeviceType )
{
    struct X
    {
        KOKKOS_INLINE_FUNCTION double operator()( double x ) const { return x; }
    };
    DataTransferKit::RadialBasisFunction<X> rbf( 2. );
    TEST_EQUALITY( rbf( 1. ), .5 );
    TEST_EQUALITY( rbf( 2. ), 1. );
    TEST_EQUALITY( rbf( 4. ), 2. );
}

// Include the test macros.
#include "DataTransferKit_ETIHelperMacros.h"

// Create the test group
#define UNIT_TEST_GROUP( NODE )                                                \
    using DeviceType##NODE = typename NODE::device_type;                       \
    TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT(                                      \
        CompactlySupportedRadialBasisFunctions, polynomial_rbf,                \
        DeviceType##NODE )                                                     \
    TEUCHOS_UNIT_TEST_TEMPLATE_1_INSTANT(                                      \
        CompactlySupportedRadialBasisFunctions, wrap_rbf, DeviceType##NODE )
// Demangle the types
DTK_ETI_MANGLING_TYPEDEFS()

// Instantiate the tests
DTK_INSTANTIATE_N( UNIT_TEST_GROUP )


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <stan/math/prim/mat.hpp>
#include <math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <limits>
#include <vector>

class NormalTestRig : public VectorRealRNGTestRig {
 public:
  /*
   * The default NormalTestRig constructor initializes the TestRig with
   * valid and invalid parameters for a random number generator with two
   * arguments.
   */
  NormalTestRig()
      : VectorRealRNGTestRig(
            10000,  // Number of samples used for quantiles tests
            10,     // Length of vectors for vectorization tests
            {-2.5, -1.7, -0.1, 0.0, 2.0, 5.8},  // Valid values for p1
            {-3, -2, -1, 0, 2, 6},              // Valid integer values for p1
            {}, {}, {0.1, 1.0, 2.5, 4.0}, {1, 2, 3, 4}, {-2.7, -1.5, -0.5, 0.0},
            {-3, -2, -1, 0}) {}

  /*
   * This function wraps up the random number generator for testing.
   *
   * The tested rng can have up to three parameters. Any unused parameters can
   * be ignored.
   */
  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& mean, const T2& sd, const T3& unused,
                        T_rng& rng) const {
    return stan::math::normal_rng(mean, sd, rng);
  }

  /*
   * This function builds the quantiles that we will supply to
   * assert_matches_quantiles to test the normal_rng
   */
  std::vector<double> generate_quantiles(double mu, double sigma,
                                         double unused) const {
    std::vector<double> quantiles;
    double K = stan::math::round(2 * std::pow(N_, 0.4));
    boost::math::normal_distribution<> dist(mu, sigma);

    for (int i = 1; i < K; ++i) {
      double frac = i / K;
      quantiles.push_back(quantile(dist, frac));
    }
    quantiles.push_back(std::numeric_limits<double>::max());

    return quantiles;
  }
};

TEST(ProbDistributionsNormal, errorCheck) {
  /*
   * This test verifies that normal_rng throws errors in the right places.
   *
   * It does so by calling test_rig::generate_samples for all possible
   * combinations of calling arguments.
   */
  check_dist_throws_all_types(NormalTestRig());
}

TEST(ProbDistributionsNormal, distributionTest) {
  /*
   * This test checks that the normal_rng is actually generating numbers from
   * the correct distributions. Quantiles are computed from
   * test_rig::generate_quantiles
   *
   * It does so for all possible combinations of calling arguments.
   */
  check_quantiles_real_real(NormalTestRig());
}


#include "problemes.h"
#include "chiffres.h"
#include "utilitaires.h"

#include <boost/rational.hpp>
#include <fstream>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef boost::rational<nombre> fraction;

ENREGISTRER_PROBLEME(112, "Bouncy numbers") {
    // Working from left-to-right if no digit is exceeded by the digit to its left it is called an
    // increasing number; for example, 134468.
    //
    // Similarly if no digit is exceeded by the digit to its right it is called a decreasing number;
    // for example, 66420.
    //
    // We shall call a positive integer that is neither increasing nor decreasing a "bouncy" number;
    // for example, 155349.
    //
    // Clearly there cannot be any bouncy numbers below one-hundred, but just over half of the numbers
    // below one-thousand (525) are bouncy. In fact, the least number for which the proportion of
    // bouncy numbers first reaches 50% is 538.
    //
    // Surprisingly, bouncy numbers become more and more common and by the time we reach 21780 the
    // proportion of bouncy numbers is equal to 90%.
    //
    // Find the least number for which the proportion of bouncy numbers is exactly 99%.
    fraction limite(99, 100);
    nombre ratio_numerateur = 0;
    nombre ratio_denominateur = 0;

    nombre resultat = 0;
    for (nombre n = 1;; ++n) {
        const auto chiffres = chiffres::extraire_chiffres(n);
        ++ratio_denominateur;
        if (!std::is_sorted(chiffres.begin(), chiffres.end())
            && !std::is_sorted(chiffres.rbegin(), chiffres.rend()))
            ++ratio_numerateur;
        if (ratio_numerateur >= limite * ratio_denominateur) {
            resultat = n;
            break;
        }
    }

    return std::to_string(resultat);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testRegularHessianFactor.cpp
 * @author  Frank Dellaert
 * @date    March 4, 2014
 */

#include <gtsam/linear/RegularHessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/assign/list_of.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::assign;

/* ************************************************************************* */
TEST(RegularHessianFactor, Constructors)
{
  // First construct a regular JacobianFactor
  // 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I]
  Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2;
  Vector2 b(1,2);
  vector<pair<Key, Matrix> > terms;
  terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3);
  RegularJacobianFactor<2> jf(terms, b);

  // Test conversion from JacobianFactor
  RegularHessianFactor<2> factor(jf);

  // 0.5*|A*x-b|^2 = 0.5*(Ax-b)'*(Ax-b) = 0.5*x'*A'A*x - x'*A'b + 0.5*b'*b
  // Compare with comment in HessianFactor: E(x) = 0.5 x^T G x - x^T g + 0.5 f
  // Hence G = I6, g A'*b = [b;b;b], and f = b'*b = 1+4 = 5
  Matrix G11 = I_2x2;
  Matrix G12 = I_2x2;
  Matrix G13 = I_2x2;

  Matrix G22 = I_2x2;
  Matrix G23 = I_2x2;

  Matrix G33 = I_2x2;

  Vector2 g1 = b, g2 = b, g3 = b;

  double f = 5;

  // Test ternary constructor
  RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
  EXPECT(assert_equal(factor,factor2));

  // Test n-way constructor
  vector<Key> keys; keys += 0, 1, 3;
  vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
  vector<Vector> gs; gs += g1, g2, g3;
  RegularHessianFactor<2> factor3(keys, Gs, gs, f);
  EXPECT(assert_equal(factor, factor3));

  // Test constructor from Gaussian Factor Graph
  GaussianFactorGraph gfg;
  gfg += jf;
  RegularHessianFactor<2> factor4(gfg);
  EXPECT(assert_equal(factor, factor4));
  GaussianFactorGraph gfg2;
  gfg2 += factor;
  RegularHessianFactor<2> factor5(gfg);
  EXPECT(assert_equal(factor, factor5));

  // Test constructor from Information matrix
  Matrix info = factor.augmentedInformation();
  vector<size_t> dims; dims += 2, 2, 2;
  SymmetricBlockMatrix sym(dims, info, true);
  RegularHessianFactor<2> factor6(keys, sym);
  EXPECT(assert_equal(factor, factor6));

  // multiplyHessianAdd:
  {
  // brute force
  Matrix AtA = factor.information();
  HessianFactor::const_iterator i1 = factor.begin();
  HessianFactor::const_iterator i2 = i1 + 1;
  Vector X(6); X << 1,2,3,4,5,6;
  Vector Y(6); Y << 9, 12, 9, 12, 9, 12;
  EXPECT(assert_equal(Y,AtA*X));

  VectorValues x = map_list_of<Key, Vector>
    (0, Vector2(1,2))
    (1, Vector2(3,4))
    (3, Vector2(5,6));

  VectorValues expected;
  expected.insert(0, Y.segment<2>(0));
  expected.insert(1, Y.segment<2>(2));
  expected.insert(3, Y.segment<2>(4));

  // VectorValues version
  double alpha = 1.0;
  VectorValues actualVV;
  actualVV.insert(0, Vector2::Zero());
  actualVV.insert(1, Vector2::Zero());
  actualVV.insert(3, Vector2::Zero());
  factor.multiplyHessianAdd(alpha, x, actualVV);
  EXPECT(assert_equal(expected, actualVV));

  // RAW ACCESS
  Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12;
  Vector fast_y = Vector8::Zero();
  double xvalues[8] = {1,2,3,4,0,0,5,6};
  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
  EXPECT(assert_equal(expected_y, fast_y));

  // now, do it with non-zero y
  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
  EXPECT(assert_equal(2*expected_y, fast_y));

  // check some expressions
  EXPECT(assert_equal(G12,factor.info().aboveDiagonalBlock(i1 - factor.begin(), i2 - factor.begin())));
  EXPECT(assert_equal(G22,factor.info().diagonalBlock(i2 - factor.begin())));
  }
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */