/*  $Id$
 *
 *  Copyright 2010-2011 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib 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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef TSP_H
#define TSP_H

#include <iostream>
#include <vector>
#include <fstream>
#include <set>
#include <ctime>

#include <boost/assert.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/simple_point.hpp>
#include <boost/graph/metric_tsp_approx.hpp>

#include <boost/python.hpp>

namespace ocl {

namespace tsp {

// loosely based on metric tsp example:
// http://www.boost.org/doc/libs/1_46_1/libs/graph/test/metric_tsp_approx.cpp

//add edges to the graph (for each node connect it to all other nodes)
template< typename VertexListGraph,
          typename PointContainer,
          typename WeightMap,
          typename VertexIndexMap>
void connectAllEuclidean(VertexListGraph& g,
                        const PointContainer& points,  // vector of (x,y) points
                        WeightMap wmap,            // Property maps passed by value
                        const VertexIndexMap vmap) // Property maps passed by value
{
    using namespace boost;
    using namespace std;
    typedef typename graph_traits<VertexListGraph>::edge_descriptor Edge;
    typedef typename graph_traits<VertexListGraph>::vertex_iterator VItr;
    Edge e;
    bool inserted;
    pair<VItr, VItr> verts(vertices(g));
    for (VItr src(verts.first); src != verts.second; src++) {
        for (VItr dest(src); dest != verts.second; dest++) {
            if (dest != src) {
                double weight( sqrt(
                    pow(static_cast<double>( points[vmap[*src ]].x - points[vmap[*dest]].x), 2.0) +
                    pow(static_cast<double>( points[vmap[*dest]].y - points[vmap[*src ]].y), 2.0)) );
                boost::tie(e, inserted) = add_edge(*src, *dest, g);
                wmap[e] = weight; // passed by value??
            }
        }
    }
}

class TSPSolver {
    typedef boost::adjacency_matrix< boost::undirectedS,
                                     boost::no_property,
                                     boost::property< boost::edge_weight_t, double,
                                     boost::property< boost::edge_index_t, long unsigned int> >,
                                     boost::no_property
                                    > Graph;
    typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
    typedef boost::graph_traits<Graph>::edge_descriptor Edge;
    typedef boost::property_map<Graph, boost::edge_weight_t>::type WeightMap;
    typedef std::vector< boost::simple_point<double> > PointSet; // was std::set
    typedef std::vector< Vertex > Container;

public:
    TSPSolver () {}
    virtual ~TSPSolver() {
        if (g)
            delete g;
    }
    void run() {
        g = new Graph( points.size() );
        // connect all vertices
        WeightMap weight_map = boost::get( boost::edge_weight, *g);
        connectAllEuclidean( *g, points, weight_map, boost::get( boost::vertex_index, *g) );
        length = 0.0;
        // Run the TSP approx, creating the visitor on the fly.
        boost::metric_tsp_approx(*g, boost::make_tsp_tour_len_visitor(*g, std::back_inserter(output), length, weight_map) );
        //length = len;
        //std::cout << "Number of points: " << boost::num_vertices(*g) << std::endl;
        //std::cout << "Number of edges: " << boost::num_edges(*g) << std::endl;
        //std::cout << "Length of tour: " << len << std::endl;
        //std::cout << "vertices in tour: " << output.size() << std::endl;
        //std::cout << "Elapsed: " << t.elapsed() << std::endl;
    }
    void addPoint(double x, double y) {
        boost::simple_point<double> pnt;
        pnt.x=x;
        pnt.y=y;
        points.push_back(pnt);
    }
    void reset() {
        output.clear();
        points.clear();
        if (g)
            delete g;
    }
    void printOutput() const {
        int n=0;
        BOOST_FOREACH( Vertex v, output ) {
            std::cout << n++ << " : " << v << "\n" ;
        }
    }
    double getLength() const {
        return length;
    }
    boost::python::list getOutput() const {
        boost::python::list plist;
        BOOST_FOREACH(Vertex v, output) {
            plist.append( v );
        }
        return plist;
    }

protected:
    Container output;
    PointSet points;
    Graph* g;
    double length;
};

} // end tsp namespace

} // end ocl namespace
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_INV_INCLUDE
#define MTL_MATRIX_INV_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/identity.hpp>
#include <boost/numeric/mtl/operation/upper_trisolve.hpp>
#include <boost/numeric/mtl/operation/lu.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/vector/unit_vector.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace mat {

namespace traits {

    /// Return type of inv(Matrix)
    /** Might be specialized later for the sake of efficiency **/
    template <typename Matrix>
    struct inv
    {
	typedef typename Collection<Matrix>::value_type    value_type;
	typedef ::mtl::mat::dense2D<value_type>         type;
    };

} // traits

/// Invert upper triangular matrix
template <typename Matrix, typename MatrixOut>
void inv_upper(Matrix const& A, MatrixOut& Inv)
{
    vampir_trace<5019> tracer;
    typedef typename Collection<Matrix>::value_type    value_type;
    typedef typename Collection<Matrix>::size_type     size_type;

    const size_type N= num_rows(A);
    MTL_DEBUG_THROW_IF(num_cols(A) != N, matrix_not_square());
    MTL_DEBUG_THROW_IF(N != num_rows(Inv) || num_cols(A) != num_cols(Inv), incompatible_size());

    Inv= math::zero(value_type());

    for (size_type k= 0; k < N; ++k) {
	irange r(k+1);
	typename mtl::ColumnInMatrix<MatrixOut>::type col_k(Inv[r][k]);
	upper_trisolve(A[r][r], vec::unit_vector<value_type>(k, k+1), col_k, mtl::tag::regular_diagonal());
    }
}

/// Invert upper triangular matrix
template <typename Matrix>
inline typename traits::inv<Matrix>::type
inv_upper(Matrix const& A)
{
    typedef typename Collection<Matrix>::size_type     size_type;
    const size_type N= num_rows(A);
    typename traits::inv<Matrix>::type Inv(N, N);
    inv_upper(A, Inv);
    return Inv;
}

#if 0
/// Invert lower triangular matrix
template <typename Matrix, typename MatrixOut>
inline void inv_lower(Matrix const& A, MatrixOut& Inv)
{
    vampir_trace<5020> tracer;
    typedef typename Collection<Matrix>::value_type    value_type;
    typedef typename Collection<Matrix>::size_type     size_type;

    const size_type N= num_rows(A);
    MTL_DEBUG_THROW_IF(num_cols(A) != N, matrix_not_square());
    MTL_DEBUG_THROW_IF(N != num_rows(Inv) || num_cols(A) != num_cols(Inv), incompatible_size());

    Inv= math::zero(value_type());

    for (size_type k= 0; k < N; ++k) {
	irange r(k, N);
	typename mtl::ColumnInMatrix<MatrixOut>::type col_k(Inv[r][k]);
	lower_trisolve(A[r][r], unit_vector<value_type>(0, N-k), col_k, mtl::tag::regular_diagonal());
    }
}

template <typename Matrix>
typename traits::inv<Matrix>::type
inline inv_lower(Matrix const& A)
{
    typedef typename Collection<Matrix>::size_type     size_type;
    const size_type N= num_rows(A);
    typename traits::inv<Matrix>::type Inv(N, N);
    inv_lower(A, Inv);
    return Inv;
}
#endif

#if 1
/// Invert lower triangular matrix
template <typename Matrix>
typename traits::inv<Matrix>::type
inline inv_lower(Matrix const& A)
{
    vampir_trace<5020> tracer;
    Matrix T(trans(A)); // Shouldn't be needed
    return typename traits::inv<Matrix>::type(trans(inv_upper(T)));
}
#endif

/// Invert matrix
/** Uses pivoting LU factorization and triangular inversion
    \sa \ref lu, \ref inv_upper, \ref inv_lower **/
template <typename Matrix, typename MatrixOut>
inline void inv(Matrix const& A, MatrixOut& Inv)
{
    vampir_trace<5021> tracer;
    typedef typename Collection<Matrix>::size_type     size_type;
    typedef typename Collection<Matrix>::value_type    value_type;
    typedef typename traits::inv<Matrix>::type         result_type;

    const size_type N= num_rows(A);
    MTL_THROW_IF(num_cols(A) != num_cols(A), matrix_not_square());
    MTL_DEBUG_THROW_IF(N != num_rows(Inv) || num_cols(A) != num_cols(Inv), incompatible_size());

    if (N == 1) {
	Inv[0][0]= value_type(1) / A[0][0];
	return;
    }

    result_type                    PLU(A);
    mtl::dense_vector<size_type, vec::parameters<> >   Pv(num_rows(A));

    lu(PLU, Pv);
    result_type  PU(upper(PLU)), PL(strict_lower(PLU));
    for (size_type i= 0; i < num_rows(A); i++)
	PL[i][i]= value_type(1);

    Inv= inv_upper(PU) * inv_lower(PL) * permutation(Pv);
}

template <typename Matrix>
typename traits::inv<Matrix>::type
inline inv(Matrix const& A)
{
    typedef typename Collection<Matrix>::size_type     size_type;
    const size_type N= num_rows(A);
    typename traits::inv<Matrix>::type Inv(N, N);
    inv(A, Inv);
    return Inv;
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_INV_INCLUDE


/******************************************************************************
 *
 * 	filename   : Camera.h
 *  author     : Do Won Cha
 *  content    : Container class to output a buffer to image.
 *
 ******************************************************************************/

#pragma once
#ifndef _RAY_CAMERA_
#define _RAY_CAMERA_

#include <Eigen/Core>
#include <cassert>

#include "ray.hpp"
#include "../utility.h"

namespace raytracer
{

using namespace Eigen;

class Camera
{
public:
  Camera() :
    position  (0.0f, 0.0f, 0.0f),
    target_    (0.0f, 0.0f, -1.0f),
    right_     (1.0f, 0.0f, 0.0f),
    up_        (0.0f, 1.0f, 0.0f),
    forward_   (0.0f, 0.0f, 1.0f),
    l(-0.1f), r(0.1f), t(0.1f), b(-0.1f), d(0.1f)
  {
  }

  Camera(int width, int height) :
    position  (0.0f, 0.0f, 0.0f),
    target_    (0.0f, 0.0f, -1.0f),
    right_     (1.0f, 0.0f, 0.0f),
    up_        (0.0f, 1.0f, 0.0f),
    forward_   (0.0f, 0.0f, 1.0f),
    l(-0.1f), r(0.1f), t(0.1f), b(-0.1f), d(0.1f),
    screen_width_(width),
    screen_height_(height)
  {}

  // default offset is to the center of the pixel
  Ray GetRayFromEye(int x, int y) const
  {
    assert(x < screen_width_);
    assert(y < screen_height_);

  	float invW = 1.0f / screen_width_;
  	float invH = 1.0f / screen_height_;

  	float u = l + (r - l) * ((float)x + 0.5f) * invW;
  	float v = b + (t - b) * ((float)y + 0.5f) * invH;

    // NOTE: Had to change up calculation to negative to set y to bottom?
    // forward_ is also negative
  	Vector3f dir = ((right_ * u) - (up_ * v) - (forward_ * d)).normalized();

  	return Ray(position, dir);
  }

  //
  Ray GetRayFromEye(int x, int y, float offsetx, float offsety) const
  {
    assert( x < screen_width_ );
    assert( y < screen_height_ );

    offsetx = Utility::clamp(0.0f, offsetx, 1.0f);
    offsety = Utility::clamp(0.0f, offsety, 1.0f);

    float invW = 1.0f / screen_width_;
    float invH = 1.0f / screen_height_;

    float u = l + (r - l) * (x + offsetx) * invW;
  	float v = b + (t - b) * (y + offsety) * invH;

    Vector3f dir = ((right_ * u) - (up_ * v) - (forward_ * d)).normalized();

  	return Ray(position, dir);
  }

  void resize(int width, int height)
  {
    screen_width_ = width;
    screen_height_ = height;
  }

  int screen_width()  const { return screen_width_; }
  int screen_height() const { return screen_height_; }

private:
  Vector3f position;                // position vector
  Vector3f target_;                  // What are we looking at
  Vector3f forward_;                 // forward_ vector
  Vector3f up_;                      // up_ vector, typically y-axis
  Vector3f right_;                   // right_ vector
  float l, r, t, b, d;              // Viewport constants
  int screen_width_, screen_height_;    // Resolution of camera
};

}     // end of namespace raytracer

#endif // _RAY_CAMERA_


#include <vector>
#include <sstream>
#define EIGEN_QUATERNION_PLUGIN <quaternion_plugin.h>
#include <Eigen/Dense>
#define private public
#include "DualQuaternion.h"
#undef private
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/operators.h>

namespace py = pybind11;
using namespace Eigen;
typedef double Float;

PYBIND11_MODULE(_eigen_dq, m) {
    py::class_<Quaternion<Float> >(m, "quat")
      .def(py::init<>())
      .def(py::init<Float, Float, Float, Float>())
      .def(py::init<Matrix<Float, 4, 1>&>())
      .def(py::init<Matrix<Float, 3, 3>&>())
      .def(py::init([](Float ang, Matrix<Float, 3, 1>& axis) {
          return std::unique_ptr<Quaternion<Float>>(new Quaternion<Float>(AngleAxis<Float>(ang, axis)));}))
      .def_property("data", &Quaternion<Float>::getData, &Quaternion<Float>::setData)
      .def_property("w", &Quaternion<Float>::getW, &Quaternion<Float>::setW)
      .def_property("x", &Quaternion<Float>::getX, &Quaternion<Float>::setX)
      .def_property("y", &Quaternion<Float>::getY, &Quaternion<Float>::setY)
      .def_property("z", &Quaternion<Float>::getZ, &Quaternion<Float>::setZ)
      .def_property("vec", &Quaternion<Float>::getVec, &Quaternion<Float>::setVec)
      .def_static("identity", &Quaternion<Float>::Identity)
      .def("set_identity", &Quaternion<Float>::setIdentity)
      .def("squared_norm", &Quaternion<Float>::squaredNorm)
      .def("norm", &Quaternion<Float>::norm)
      .def("normalize", &Quaternion<Float>::normalize)
      .def("normalized", &Quaternion<Float>::normalized)
      .def("angular_distance", &Quaternion<Float>::angularDistance<Quaternion<Float>>)
      .def("dot", &Quaternion<Float>::dot<Quaternion<Float>>)
      .def("slerp", &Quaternion<Float>::slerp<Quaternion<Float>>)
      .def("to_rotation_matrix", &Quaternion<Float>::toRotationMatrix)
      .def("conjugate", &Quaternion<Float>::conjugate)
      .def("inverse", &Quaternion<Float>::inverse)
      .def(py::self * py::self)
      .def(py::self *= py::self)
      .def("__repr__", [](const Quaternion<Float>& inst) {
          std::stringstream ss;
          ss << "< " << inst.getData().transpose() << " >";
          return ss.str();
        });

    py::class_<DualQuaternion<Float> >(m, "dualquat")
      .def(py::init<>())
      .def(py::init<Quaternion<Float>&, Quaternion<Float>&>())
      .def(py::init<Quaternion<Float>&, Matrix<Float, 3, 1>&>())
      .def(py::init([](Quaternion<Float>& q) {
           return std::unique_ptr<DualQuaternion<Float>>(new DualQuaternion<Float>(q, Matrix<Float, 3, 1>::Zero()));}))
      .def_readwrite("real", &DualQuaternion<Float>::m_real)
      .def_readwrite("dual", &DualQuaternion<Float>::m_dual)
      .def_static("zeros", &DualQuaternion<Float>::zeros)
      .def_static("identity", &DualQuaternion<Float>::identity)
      .def(py::self + py::self)
      .def(py::self - py::self)
      .def(py::self * Float())
      .def(Float() * py::self)
      .def(py::self * py::self)
      .def("from_screw", &DualQuaternion<Float>::fromScrew)
      .def("conjugate", &DualQuaternion<Float>::conjugate)
      .def("norm", [](DualQuaternion<Float>& inst) {
           Float x, y; inst.norm(x, y); return std::make_tuple(x, y);
        })
      .def("normalize", &DualQuaternion<Float>::normalize)
      .def("normalized", &DualQuaternion<Float>::normalized)
      .def("transform_point", &DualQuaternion<Float>::transformPoint)
      .def("transform_vector", &DualQuaternion<Float>::transformVector)
      .def("inverse", &DualQuaternion<Float>::inverse)
      .def("exp", &DualQuaternion<Float>::exp)
      .def("log", &DualQuaternion<Float>::log)
      .def("pow", [](DualQuaternion<Float>& inst, Float t) {return (inst.log() * t).exp();})
      .def("rotation", &DualQuaternion<Float>::rotation)
      .def("translation", &DualQuaternion<Float>::translation)
      .def("translation_quaternion", &DualQuaternion<Float>::translationQuaternion)
      .def("to_matrix", &DualQuaternion<Float>::toMatrix)
      .def("__repr__", [](const DualQuaternion<Float>& inst) {
          std::stringstream ss;
          ss << "<real: " << inst.real().getData().transpose()
             << ", dual: "<< inst.dual().getData().transpose() << " >";
          return ss.str();
        });

    m.def("expq", &expq<Float>);
    m.def("logq", &logq<Float>);

#ifdef VERSION_INFO
    m.attr("__version__") = VERSION_INFO;
#else
    m.attr("__version__") = "dev";
#endif
}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/numpy.h>

#include <Eigen/Core>
#include <boost/geometry.hpp>
#include <iostream>
#include <optional>

#include "pyinterp/axis.hpp"
#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/geometry/point.hpp"
#include "pyinterp/detail/math/binning.hpp"
#include "pyinterp/detail/math/descriptive_statistics.hpp"
#include "pyinterp/eigen.hpp"
#include "pyinterp/geodetic/system.hpp"

namespace pyinterp {

/// Group a number of more or less continuous values into a smaller number of
/// "bins" located on a grid.
template <typename T>
class Binning2D {
 public:
  /// Statistics handled by this object.
  using Accumulators = detail::math::Accumulators<T>;
  using DescriptiveStatistics = detail::math::DescriptiveStatistics<T>;

  /// Default constructor
  ///
  /// @param x Definition of the bin centers for the X axis of the grid.
  /// @param y Definition of the bin centers for the Y axis of the grid.
  /// @param wgs WGS of the coordinate system used to manipulate geographic
  /// coordinates. If this parameter is not set, the handled coordinates will be
  /// considered as Cartesian coordinates. Otherwise, "x" and "y" are considered
  /// to represents the longitudes and latitudes on a grid.
  Binning2D(std::shared_ptr<Axis<double>> x, std::shared_ptr<Axis<double>> y,
            std::optional<geodetic::System> wgs)
      : x_(std::move(x)),
        y_(std::move(y)),
        acc_(x_->size(), y_->size()),
        wgs_(std::move(wgs)) {}

  /// Default destructor
  virtual ~Binning2D() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Binning2D(const Binning2D& rhs) = delete;

  /// Move constructor
  ///
  /// @param rhs right value
  Binning2D(Binning2D&& rhs) noexcept = delete;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Binning2D& rhs) -> Binning2D& = delete;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Binning2D&& rhs) noexcept -> Binning2D& = delete;

  /// Inserts new values in the grid from Z values for X, Y data coordinates.
  void push(const pybind11::array_t<T>& x, const pybind11::array_t<T>& y,
            const pybind11::array_t<T>& z, const bool simple) {
    detail::check_array_ndim("x", 1, x, "y", 1, y, "z", 1, z);
    detail::check_ndarray_shape("x", x, "y", y, "z", z);

    if (simple) {
      // Nearest
      push_nearest(x, y, z);
    } else if (!wgs_) {
      // Cartesian linear
      push_linear<detail::geometry::Point2D,
                  boost::geometry::strategy::area::cartesian<>>(
          x, y, z, boost::geometry::strategy::area::cartesian<>());
    } else {
      // Geographic linear
      auto strategy = boost::geometry::strategy::area::geographic<
          boost::geometry::strategy::vincenty, 5>(
          boost::geometry::srs::spheroid(wgs_->semi_major_axis(),
                                         wgs_->semi_minor_axis()));
      push_linear<detail::geometry::GeographicPoint2D,
                  boost::geometry::strategy::area::geographic<
                      boost::geometry::strategy::vincenty, 5>>(x, y, z,
                                                               strategy);
    }
  }

  /// Reset the statistics.
  void clear() {
    acc_ = std::move(Matrix<DescriptiveStatistics>(x_->size(), y_->size()));
  }

  /// Compute the count of points within each bin.
  [[nodiscard]] auto count() const -> pybind11::array_t<uint64_t> {
    return calculate_statistics<decltype(&DescriptiveStatistics::count),
                                uint64_t>(&DescriptiveStatistics::count);
  }

  /// Compute the minimum of values for points within each bin.
  [[nodiscard]] auto min() const -> pybind11::array_t<T> {
    return calculate_statistics(&DescriptiveStatistics::min);
  }

  /// Compute the maximum of values for points within each bin.
  [[nodiscard]] auto max() const -> pybind11::array_t<T> {
    return calculate_statistics(&DescriptiveStatistics::max);
  }

  /// Compute the mean of values for points within each bin.
  [[nodiscard]] auto mean() const -> pybind11::array_t<T> {
    return calculate_statistics(&DescriptiveStatistics::mean);
  }

  /// Compute the variance of values for points within each bin.
  [[nodiscard]] auto variance(const int ddof = 0) const
      -> pybind11::array_t<T> {
    return calculate_statistics(&DescriptiveStatistics::variance, ddof);
  }

  /// Compute the kurtosis of values for points within each bin.
  [[nodiscard]] auto kurtosis() const -> pybind11::array_t<T> {
    return calculate_statistics(&DescriptiveStatistics::kurtosis);
  }

  /// Compute the skewness of values for points within each bin.
  [[nodiscard]] auto skewness() const -> pybind11::array_t<T> {
    return calculate_statistics(&DescriptiveStatistics::skewness);
  }

  /// Compute the sum of values for points within each bin.
  [[nodiscard]] auto sum() const -> pybind11::array_t<T> {
    return calculate_statistics(&DescriptiveStatistics::sum);
  }

  /// Compute the sum of weights within each bin.
  [[nodiscard]] auto sum_of_weights() const -> pybind11::array_t<T> {
    return calculate_statistics(&DescriptiveStatistics::sum_of_weights);
  }

  /// Gets the X-Axis
  [[nodiscard]] inline auto x() const -> std::shared_ptr<Axis<double>> {
    return x_;
  }

  /// Gets the Y-Axis
  [[nodiscard]] inline auto y() const -> std::shared_ptr<Axis<double>> {
    return y_;
  }

  /// Gets the WGS system
  [[nodiscard]] inline auto wgs() const
      -> const std::optional<geodetic::System>& {
    return wgs_;
  }

  /// Pickle support: get state of this instance
  [[nodiscard]] auto getstate() const -> pybind11::tuple {
    return pybind11::make_tuple(
        x_->getstate(), y_->getstate(),
        wgs_.has_value() ? wgs_->getstate() : pybind11::make_tuple(), acc());
  }

  /// Pickle support: set state of this instance
  static auto setstate(const pybind11::tuple& state)
      -> std::unique_ptr<Binning2D<T>> {
    if (state.size() != 4) {
      throw std::invalid_argument("invalid state");
    }

    // Unmarshalling X-Axis
    auto x = std::make_shared<Axis<double>>();
    *x = Axis<double>::setstate(state[0].cast<pybind11::tuple>());

    // Unmarshalling Y-Axis
    auto y = std::make_shared<Axis<double>>();
    *y = Axis<double>::setstate(state[1].cast<pybind11::tuple>());

    // Unmarshalling WGS system
    auto wgs = std::optional<geodetic::System>();
    auto wgs_state = state[2].cast<pybind11::tuple>();
    if (!wgs_state.empty()) {
      *wgs = geodetic::System::setstate(wgs_state);
    }

    // Unmarshalling computed statistics
    auto acc = state[3].cast<Matrix<Accumulators>>();
    if (acc.rows() != x->size() || acc.cols() != y->size()) {
      throw std::invalid_argument("invalid state");
    }

    // Unmarshalling instance
    auto result = std::make_unique<Binning2D<T>>(x, y, wgs);
    {
      auto gil = pybind11::gil_scoped_release();
      result->acc_ = std::move(acc.template cast<DescriptiveStatistics>());
    }
    return result;
  }

  /// Aggregation of statistics
  auto operator+=(const Binning2D& other) -> Binning2D& {
    if (*x_ != *(other.x_) || *y_ != *(other.y_)) {
      throw std::invalid_argument("Unable to combine different grids");
    }
    if ((wgs_ && !other.wgs_) || (!wgs_ && other.wgs_) ||
        (wgs_.has_value() && other.wgs_.has_value() &&
         (*wgs_ != *other.wgs_))) {
      throw std::invalid_argument(
          "Unable to combine different geodetic system");
    }

    for (Eigen::Index ix = 0; ix < acc_.rows(); ++ix) {
      for (Eigen::Index iy = 0; iy < acc_.cols(); ++iy) {
        auto& lhs = acc_(ix, iy);
        auto& rhs = other.acc_(ix, iy);

        // Statistics are defined only in the other instance.
        if (lhs.count() == 0 && rhs.count() != 0) {
          lhs = rhs;
          // If the statistics are defined in both instances they can be
          // combined.
        } else if (lhs.count() != 0 && rhs.count() != 0) {
          lhs += rhs;
        }
      }
    }
    return *this;
  }

 private:
  /// Grid axis
  std::shared_ptr<Axis<double>> x_;
  std::shared_ptr<Axis<double>> y_;

  /// Statistics grid
  Matrix<DescriptiveStatistics> acc_;

  /// Geodetic coordinate system required to calculate areas (optional if the
  /// user wishes to handle Cartesian coordinates).
  std::optional<geodetic::System> wgs_;

  /// Calculation of a given statistical variable.
  template <typename Func, typename Type = T, typename... Args>
  [[nodiscard]] auto calculate_statistics(const Func& func, Args... args) const
      -> pybind11::array_t<Type> {
    pybind11::array_t<Type> z({x_->size(), y_->size()});
    auto _z = z.template mutable_unchecked<2>();
    {
      pybind11::gil_scoped_release release;

      for (Eigen::Index ix = 0; ix < acc_.rows(); ++ix) {
        for (Eigen::Index iy = 0; iy < acc_.cols(); ++iy) {
          _z(ix, iy) = (acc_(ix, iy).*func)(args...);
        }
      }
    }
    return z;
  }

  /// Insertion of data on the nearest bin.
  void push_nearest(const pybind11::array_t<T>& x,
                    const pybind11::array_t<T>& y,
                    const pybind11::array_t<T>& z) {
    auto _x = x.template unchecked<1>();
    auto _y = y.template unchecked<1>();
    auto _z = z.template unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      const auto& x_axis = static_cast<pyinterp::detail::Axis<double>&>(*x_);
      const auto& y_axis = static_cast<pyinterp::detail::Axis<double>&>(*y_);

      for (pybind11::ssize_t idx = 0; idx < x.size(); ++idx) {
        auto value = _z(idx);

        if (!std::isnan(value)) {
          auto ix = x_axis.find_index(_x(idx), true);
          auto iy = y_axis.find_index(_y(idx), true);

          if (ix != -1 && iy != -1) {
            acc_(ix, iy)(value);
          }
        }
      }
    }
  }

  /// Update statistics for the linear binning (ignore zero weights).
  void update_acc(const int64_t ix, const int64_t iy, const T& value,
                  const T& weight) {
    if (!detail::math::is_almost_zero(weight,
                                      std::numeric_limits<T>::epsilon())) {
      acc_(ix, iy)(value, weight);
    }
  }

  /// Set bins with nearest binning.
  template <template <class> class Point, typename Strategy>
  void push_linear(const pybind11::array_t<T>& x, const pybind11::array_t<T>& y,
                   const pybind11::array_t<T>& z, const Strategy& strategy) {
    auto _x = x.template unchecked<1>();
    auto _y = y.template unchecked<1>();
    auto _z = z.template unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      const auto& x_axis = static_cast<pyinterp::detail::Axis<double>&>(*x_);
      const auto& y_axis = static_cast<pyinterp::detail::Axis<double>&>(*y_);

      for (pybind11::ssize_t idx = 0; idx < x.size(); ++idx) {
        auto value = _z(idx);
        if (std::isnan(value)) {
          continue;
        }

        auto x_indexes = x_axis.find_indexes(_x(idx));
        auto y_indexes = y_axis.find_indexes(_y(idx));

        if (x_indexes.has_value() && y_indexes.has_value()) {
          auto [ix0, ix1] = *x_indexes;
          auto [iy0, iy1] = *y_indexes;

          auto x0 = x_axis(ix0);

          auto weights = detail::math::binning_2d<Point, Strategy, double>(
              Point<double>(x_axis.is_angle()
                                ? detail::math::normalize_angle<double>(
                                      _x(idx), x0, 360.0)
                                : _x(idx),
                            _y(idx)),
              Point<double>(x0, y_axis(iy0)),
              Point<double>(x_axis(ix1), y_axis(iy1)), strategy);

          update_acc(ix0, iy0, value, static_cast<T>(std::get<0>(weights)));
          update_acc(ix0, iy1, value, static_cast<T>(std::get<1>(weights)));
          update_acc(ix1, iy1, value, static_cast<T>(std::get<2>(weights)));
          update_acc(ix1, iy0, value, static_cast<T>(std::get<3>(weights)));
        }
      }
    }
  }

  /// Returns the matrix of raw statistics.
  inline auto acc() const -> Matrix<Accumulators> {
    auto gil = pybind11::gil_scoped_release();
    return acc_.template cast<Accumulators>();
  }
};

}  // namespace pyinterp


#ifndef FOOT_STEP_PLANNER_H_
#define FOOT_STEP_PLANNER_H_

#ifdef PLANNER_DEBUG
static constexpr bool planner_debug = true;
#else
static constexpr bool planner_debug = false;
#endif

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <fstream>
#include <utility>
#include <iostream>
#include <deque>
#include <vector>

template <class T>
static void debugPrint(T &&s, const std::string &label)
{
    if (planner_debug)
        std::cout << "Debug planner:" << label << "::" << std::forward<T>(s) << std::endl;
}

static constexpr double dt = 4.0 / 1000.0;   // sampling period (s)
static constexpr double zh = 0.2;            // height of CoM (m)
static constexpr double g = 9.8;             // gravity (m/s^2)
static constexpr double MAX_X_STRIDE = 0.30; //(m)
static constexpr double MAX_Y_STRIDE = 0.08; //(m)
static constexpr double MIN_Y_STRIDE = 0.03; //(m)

struct FootPrint
{
    double time_;
    double x_;                  //(m)ロボットのローカル座標での足配置位置
    double y_;                  //(m)ロボットのローカル座標での足配置位置
    bool support_foot_is_right; //支持脚がどちらか
};

std::vector<FootPrint> foot_plan = {{0, 0, 0, true}, {0.8, 0.0, 0.2, false}, {1.6, 0.3, 0.0, true}, {2.4, 0.6, 0.2, false}, {3.2, 0.9, 0.0, true}, {4.0, 0.9, 0.2, false}, {100.0, 0.9, 0.2, true}};

/**
 * @brief foot step planner.左右の脚で踏み出すのを1セットで1歩とする.
 *
 * @param x_destination (m) 目標地点:X.ロボット進行方向
 * @param y_destination (m) 目標地点:Y
 * @param x_stride (m) 1歩の大きさ.遊脚が身体の前に出る長さ=x_stride/2.
 * @param support_time (s) 歩行周期.(=支持脚が支持している時間:Tsup)
 * @param steps (none) 歩数.目標地点まで何歩で進むか.0以下に指定すると最大のストライドで進む.
 * @return std::vector<FootPrint> ロボットのローカル座標で表した着地位置を返す.
 * @details 今の所x方向への直線移動しか対応していない.
 * @todo y方向の移動の実装
 */
std::vector<FootPrint> footStepPlanner(const double &x_destination, const double &y_destination, const double &x_stride, const double &support_time = (320.0 / 1000.0), const int32_t &steps = 0) noexcept
{
    const double stride_x = [=]() -> double
    {
        if (steps < 1)
        {
            return std::min(x_stride, MAX_X_STRIDE);
        }
        else
        {
            return std::min(MAX_X_STRIDE, x_destination / steps);
        }
    }();
    const double stride_y = std::clamp((y_destination / steps), MIN_Y_STRIDE, MAX_Y_STRIDE);
    const int32_t steps_ = x_destination / stride_x; //(無) 歩数
    std::cout << "x_destination " << x_destination << " stride_x " << stride_x << "steps " << steps << std::endl;
    double x = 0, y = 0, xi = 0, yi = 0;     //(m) CoMのワールド座標 {CoM = Center of Mass}
    double xd = 0, yd = 0, xdi = 0, ydi = 0; // CoMの速度 v(m/s) xdot(t)
    double px = 0.0, py = 0.0;               //(m)　着地位置のワールド座標 これは実用的にはローカルの方が良いのでは？？
    constexpr double Tc = std::sqrt(zh / g); //微分方程式の時定数
    double C = 0.0;                          // cosh((t - t0) / Tc)
    double S = 0.0;                          // sinh((t - t0) / Tc)
    // const double Tsup = support_time - std::fmod(support_time, dt); //誤差を無くすため
    const double Tsup = support_time;
    int_fast64_t step_n = 0;
    debugPrint(Tsup, "Tsup");
    std::deque<std::deque<double>> result;
    std::ofstream velofs("velo.dat");
    std::vector<FootPrint> footprint_list(100);
    for (double t = 0.0; t < (Tsup * static_cast<double>(steps_ * 2 + 1)); ++step_n)
    {
        //決められた次の一歩を着く地点までの遊脚の移動を行っている時のシミュレーション---------------
        for (double t_tmp = 0.0; t_tmp < Tsup; t_tmp += dt, t += dt)
        {
            C = std::cosh(t_tmp / Tc);
            S = std::sinh(t_tmp / Tc);
            x = (xi - px) * C + Tc * xdi * S + px; // x,xdともにn歩目開始時の状態
            y = (yi - py) * C + Tc * ydi * S + py;
            xd = (xi - px) / Tc * S + xdi * C;
            yd = (yi - py) / Tc * S + ydi * C;
            result.push_back({x, y});
            velofs << t << " " << xd << " " << yd << std::endl;
        }
        xi = x;
        yi = y;
        xdi = xd;
        ydi = yd;
        //次の一歩の目標位置を計算------------------
        static constexpr double a = 10;
        static constexpr double b = 1;
        C = std::cosh(Tsup / Tc); //意味は全く無いが分かりやすさのため
        S = std::sinh(Tsup / Tc);
        const double D = a * (C - 1) * (C - 1) + b * (S / Tc) * (S / Tc);
        double x_target = 0.0, y_target = 0.0, xd_target = 0.0, yd_target = 0.0;
        double sx = 0.0, sy = 0.0; //歩行素片による位置
        if (steps_ < step_n)
        {
            sx = 0.0;
            sy = 0.0;
        }
        else
        {
            sx = stride_x / 2;
            sy = ((step_n % 2) ? 1 : -1) * stride_y / 2;
        }
        x_target = px + sx;
        y_target = py + sy;
        std::cout << "x target " << x_target << " y target " << y_target;
        xd_target = (C + 1) / (Tc * S) * sx;
        yd_target = (C - 1) / (Tc * S) * sy;
        px = -a * (C - 1) / D * (x_target - C * xi - Tc * S * xdi) - b * S / (Tc * D) * (xd_target - S * xi / Tc - C * xdi);
        py = -a * (C - 1) / D * (y_target - C * yi - Tc * S * ydi) - b * S / (Tc * D) * (yd_target - S * yi / Tc - C * ydi);
        std::cout << " px::" << px << " py::" << py << std::endl;
        static bool is_right = true;
        footprint_list.push_back({Tsup, px - xi, py - yi, is_right});
        is_right = !is_right;
        result.back().push_back(px);
        result.back().push_back(py);
    }
    std::ofstream ofs("position.dat");
    for (auto &itr : result)
    {
        for (auto &item : itr)
        {
            ofs << item << " ";
        }
        ofs << std::endl;
    }
    return footprint_list;
    // ofs.close();
}

double spline(double x)
{
    return 0.02 * (x * x * x) + 0.01 * (x * x) - 0.1 * (x);
}

std::vector<FootPrint> planAlongSpline() noexcept
{
    const double stride_x = 0.3;
    const double stride_y = 0.20;
    const int32_t steps_ = 30;               //(無) 歩数
    double x = 0, y = 0, xi = 0, yi = 0;     //(m) CoMのワールド座標 {CoM = Center of Mass}
    double xd = 0, yd = 0, xdi = 0, ydi = 0; // CoMの速度 v(m/s) xdot(t)
    double px = 0.0, py = 0.0;               //(m)　着地位置のワールド座標 これは実用的にはローカルの方が良いのでは？？
    constexpr double Tc = std::sqrt(zh / g); //微分方程式の時定数
    double C = 0.0;                          // cosh((t - t0) / Tc)
    double S = 0.0;                          // sinh((t - t0) / Tc)
    const double Tsup = 0.32;
    int_fast64_t step_n = 0;
    double CoM_target_x = 0.0, CoM_target_y = 0.0;
    std::deque<std::deque<double>> result;
    std::ofstream velofs("velo2.dat");
    std::vector<FootPrint> footprint_list(100);
    for (double t = 0.0; t < (Tsup * static_cast<double>(steps_ * 2 + 1)); ++step_n)
    {
        //決められた次の一歩を着く地点までの遊脚の移動を行っている時のシミュレーション---------------
        for (double t_tmp = 0.0; t_tmp < Tsup; t_tmp += dt, t += dt)
        {
            C = std::cosh(t_tmp / Tc);
            S = std::sinh(t_tmp / Tc);
            x = (xi - px) * C + Tc * xdi * S + px; // x,xdともにn歩目開始時の状態
            y = (yi - py) * C + Tc * ydi * S + py;
            xd = (xi - px) / Tc * S + xdi * C;
            yd = (yi - py) / Tc * S + ydi * C;
            CoM_target_x = x;
            CoM_target_y = spline(CoM_target_x);
            result.push_back({x, y, CoM_target_x, CoM_target_y});
            velofs << t << " " << xd << " " << yd << std::endl;
        }
        xi = x;
        yi = y;
        xdi = xd;
        ydi = yd;
        //次の一歩の目標位置を計算------------------
        static constexpr double a = 30;
        static constexpr double b = 1;
        C = std::cosh(Tsup / Tc); //意味は全く無いが分かりやすさのため
        S = std::sinh(Tsup / Tc);
        const double D = a * (C - 1) * (C - 1) + b * (S / Tc) * (S / Tc);
        double x_target = 0.0, y_target = 0.0, xd_target = 0.0, yd_target = 0.0;
        double sx = 0.0, sy = 0.0; //歩行素片による位置

        if (steps_ < step_n)
        {
            sx = 0.0;
            sy = ((step_n % 2) ? 1 : -1) * stride_y / 2;
        }
        else
        {
            sx = stride_x / 2;
            sy = ((step_n % 2) ? 1 : -1) * stride_y / 2;
        }
        double theta = std::atan((spline(x + sx) - y) / sx);
        x_target = px + std::cos(theta) * sx - std::sin(theta) * sy;
        y_target = py + std::sin(theta) * sx + std::cos(theta) * sy;
        std::cout << "theta " << theta << "------------------------------------------ "<< std::endl;
        // std::cout << "(x_target - px)::" << (x_target - px) << " (y_target - py)::" << (y_target - py) << std::endl;
        std::cout << "distance " << std::sqrt((x_target - px) * (x_target - px) + (y_target - py) * (y_target - py)) << std::endl;
        // std::cout << "x target " << x_target << " y target " << y_target;
        xd_target = (C + 1) / (Tc * S) * (x_target - px);
        yd_target = (C - 1) / (Tc * S) * (y_target - py);
        double t_px = px,t_py = py;
        px = -a * (C - 1) / D * (x_target - C * xi - Tc * S * xdi) - b * S / (Tc * D) * (xd_target - S * xi / Tc - C * xdi);
        py = -a * (C - 1) / D * (y_target - C * yi - Tc * S * ydi) - b * S / (Tc * D) * (yd_target - S * yi / Tc - C * ydi);
        std::cout << "x:" << (px - t_px) << " y:" << (py - t_py) << std::endl;
        // std::cout << " px::" << px << " py::" << py << std::endl;
        static bool is_right = true;
        footprint_list.push_back({Tsup, px - xi, py - yi, is_right});
        is_right = !is_right;
        result.back().push_back(px);
        result.back().push_back(py);
    }
    std::ofstream ofs("position2.dat");
    for (auto &itr : result)
    {
        for (auto &item : itr)
        {
            ofs << item << " ";
        }
        ofs << std::endl;
    }
    return footprint_list;
    // ofs.close();
}

#endif //  FOOT_STEP_PLANNER_H_

/* memo
 * 状態をなるべく持たせない.
 * 取り敢えず歩ける事を示したいのでなるべく簡便な実装にする。
 * 一連の重心軌道を全て生成してしまってそこからどうにかするので良い
 * todo 速度が極大な所のtを表示させたい
 *
 *
 *
 */


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

#include "cudaSift.h"

int ImproveHomography(SiftData &data, float *homography, int numLoops, float minScore,
                      float maxAmbiguity, float thresh) {
#ifdef MANAGEDMEM
  SiftPoint *mpts = data.m_data;
#else
  if (data.h_data == NULL) return 0;
  SiftPoint *mpts = data.h_data;
#endif
  float limit = thresh * thresh;
  int numPts = data.numPts;
  Eigen::MatrixXd M(8, 8);
  Eigen::VectorXd A(8);
  Eigen::VectorXd X(8);
  Eigen::VectorXd Y(8);
  for (int i = 0; i < 8; i++) A(i) = homography[i] / homography[8];
  for (int loop = 0; loop < numLoops; loop++) {
    M = Eigen::MatrixXd::Zero(8, 8);
    X = Eigen::VectorXd::Zero(8);
    for (int i = 0; i < numPts; i++) {
      SiftPoint &pt = mpts[i];
      if (pt.score < minScore || pt.ambiguity > maxAmbiguity) continue;
      float den = A(6) * pt.xpos + A(7) * pt.ypos + 1.0f;
      float dx = (A(0) * pt.xpos + A(1) * pt.ypos + A(2)) / den - pt.match_xpos;
      float dy = (A(3) * pt.xpos + A(4) * pt.ypos + A(5)) / den - pt.match_ypos;
      float err = dx * dx + dy * dy;
      float wei = (err < limit ? 1.0f : 0.0f);  // limit / (err + limit);
      Y(0) = pt.xpos;
      Y(1) = pt.ypos;
      Y(2) = 1.0;
      Y(3) = Y(4) = Y(5) = 0.0;
      Y(6) = -pt.xpos * pt.match_xpos;
      Y(7) = -pt.ypos * pt.match_xpos;
      for (int c = 0; c < 8; c++)
        for (int r = 0; r < 8; r++) M(r, c) += (Y(c) * Y(r) * wei);
      X += (Y * pt.match_xpos * wei);
      Y(0) = Y(1) = Y(2) = 0.0;
      Y(3) = pt.xpos;
      Y(4) = pt.ypos;
      Y(5) = 1.0;
      Y(6) = -pt.xpos * pt.match_ypos;
      Y(7) = -pt.ypos * pt.match_ypos;
      for (int c = 0; c < 8; c++)
        for (int r = 0; r < 8; r++) M(r, c) += (Y(c) * Y(r) * wei);
      X += (Y * pt.match_ypos * wei);
    }
    // cv::solve(M, X, A, cv::DECOMP_CHOLESKY);
    A = M.colPivHouseholderQr().solve(X);
  }
  int numfit = 0;
  for (int i = 0; i < numPts; i++) {
    SiftPoint &pt = mpts[i];
    float den = A(6) * pt.xpos + A(7) * pt.ypos + 1.0;
    float dx = (A(0) * pt.xpos + A(1) * pt.ypos + A(2)) / den - pt.match_xpos;
    float dy = (A(3) * pt.xpos + A(4) * pt.ypos + A(5)) / den - pt.match_ypos;
    float err = dx * dx + dy * dy;
    if (err < limit) numfit++;
    pt.match_error = sqrt(err);
  }
  for (int i = 0; i < 8; i++) homography[i] = A(i);
  homography[8] = 1.0f;
  return numfit;
}


#include <examples/ml/LRModel.h>
#include <Utils.h>
#include <MlUtils.h>
#include <Eigen/Dense>
#include <utils/Log.h>
#include <Checksum.h>
#include <algorithm>

LRModel::LRModel(uint64_t d) :
    d(d) {
    weights.resize(d);
}

LRModel::LRModel(const double* w, uint64_t d) :
    d(d) {
    weights.resize(d);
    std::copy(w, w + d, weights.begin());
}

std::unique_ptr<Model> LRModel::deserialize(void* data, uint64_t size) const {
    uint64_t d = size / sizeof(double);
    std::unique_ptr<LRModel> model = std::make_unique<LRModel>(
            reinterpret_cast<double*>(data), d);
    return model;
}

void check_dataset(Dataset& dataset) {
    for (uint64_t i = 0; i < dataset.num_samples(); ++i) {
        for (uint64_t j = 0; j < dataset.num_features(); ++j) {
            const double* s = dataset.sample(i);
            if (std::isnan(s[j]) || std::isinf(s[j])) {
                throw std::runtime_error("Invalid dataset");
            }
        }
    }
}

std::pair<std::unique_ptr<char[]>, uint64_t>
LRModel::serialize() const {
    std::pair<std::unique_ptr<char[]>, uint64_t> res;
    uint64_t size = getSerializedSize();
    res.first.reset(new char[size]);

    res.second = size;
    std::memcpy(res.first.get(), weights.data(), getSerializedSize());

    return res;
}

void LRModel::serializeTo(void* mem) const {
    std::memcpy(mem, weights.data(), getSerializedSize());
}

void LRModel::randomize() {
    for (uint64_t i = 0; i < d; ++i) {
        weights[i] = get_rand_between_0_1();
    }
}

std::unique_ptr<Model> LRModel::copy() const {
    std::unique_ptr<LRModel> new_model =
        std::make_unique<LRModel>(weights.data(), d);
    return new_model;
}

void LRModel::sgd_update(double learning_rate,
        const ModelGradient* gradient) {
    const LRGradient* grad = dynamic_cast<const LRGradient*>(gradient);

    if (grad == nullptr) {
        throw std::runtime_error("Error in dynamic cast");
    }

    for (uint64_t i = 0; i < d; ++i) {
       weights[i] += learning_rate * grad->weights[i];
    }
}

uint64_t LRModel::getSerializedSize() const {
    return d * sizeof(double);
}

void LRModel::loadSerialized(const void* data) {
    cirrus::LOG<cirrus::INFO>("loadSerialized d: ", d);
    const double* v = reinterpret_cast<const double*>(data);
    std::copy(v, v + d, weights.begin());
}

std::unique_ptr<ModelGradient> LRModel::minibatch_grad(
        int /* rank */, const Matrix& dataset,
        double* labels,
        uint64_t labels_size,
        double epsilon) const {
    auto w = weights;
#ifdef DEBUG
    dataset.check_values();
#endif

    const double* dataset_data = dataset.data.get();
    // create Matrix for dataset
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic,
        Eigen::Dynamic, Eigen::RowMajor>>
          ds(const_cast<double*>(dataset_data), dataset.rows, dataset.cols);

    // create weight vector
    Eigen::Map<Eigen::VectorXd> weights(w.data(), d);

    // create vector with labels
    Eigen::Map<Eigen::VectorXd> lab(labels, labels_size);

    // apply logistic function to matrix multiplication
    // between dataset and weights
    auto part1_1 = (ds * weights);
    auto part1 = part1_1.unaryExpr(std::ptr_fun(mlutils::s_1));

    Eigen::Map<Eigen::VectorXd> lbs(labels, labels_size);

    // compute difference between labels and logistic probability
    auto part2 = lbs - part1;
    auto part3 = ds.transpose() * part2;
    auto part4 = weights * 2 * epsilon;
    auto res = part4 + part3;

    std::vector<double> vec_res;
    vec_res.resize(res.size());
    Eigen::VectorXd::Map(vec_res.data(), res.size()) = res;

    std::unique_ptr<LRGradient> ret = std::make_unique<LRGradient>(vec_res);

#ifdef DEBUG
    ret->check_values();
#endif

    return ret;
}

double LRModel::calc_loss(Dataset& dataset) const {
    double total_loss = 0;

    auto w = weights;

#ifdef DEBUG
    dataset.check_values();
#endif
#ifdef DEBUG
    check_dataset(dataset);  // make sure dataset is valid
#endif

    const double* ds_data =
        reinterpret_cast<const double*>(dataset.samples_.data.get());

    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic,
        Eigen::Dynamic, Eigen::RowMajor>>
            ds(const_cast<double*>(ds_data),
                    dataset.samples_.rows, dataset.samples_.cols);

    Eigen::Map<Eigen::VectorXd> weights_eig(w.data(), d);

    // count how many samples are wrongly classified
    uint64_t wrong_count = 0;
    for (uint64_t i = 0; i < dataset.num_samples(); ++i) {
        // get labeled class for the ith sample
        double class_i =
            reinterpret_cast<const double*>(dataset.labels_.get())[i];

        assert(is_integer(class_i));

        int predicted_class = 0;

        auto r1 = ds.row(i) *  weights_eig;
        if (mlutils::s_1(r1) > 0.5) {
            predicted_class = 1;
        }
        if (predicted_class != class_i) {
            wrong_count++;
        }

        double v1 = mlutils::log_aux(1 - mlutils::s_1(ds.row(i) * weights_eig));
        double v2 = mlutils::log_aux(mlutils::s_1(ds.row(i) *  weights_eig));

        double value = class_i *
            mlutils::log_aux(mlutils::s_1(ds.row(i) *  weights_eig)) +
            (1 - class_i) * mlutils::log_aux(1 - mlutils::s_1(
                        ds.row(i) * weights_eig));

        // XXX not sure this check is necessary
        if (value > 0 && value < 1e-6)
            value = 0;

        if (value > 0) {
            std::cout << "ds row: " << std::endl << ds.row(i) << std::endl;
            std::cout << "weights: " << std::endl << weights_eig << std::endl;
            std::cout << "Class: " << class_i << " " << v1 << " " << v2
                << std::endl;
            throw std::runtime_error("Error: logistic loss is > 0");
        }

        total_loss -= value;
    }

    if (total_loss < 0) {
        throw std::runtime_error("total_loss < 0");
    }

    std::cout
        << "Accuracy: " << (1.0 - (1.0 * wrong_count / dataset.num_samples()))
        << std::endl;

    if (std::isnan(total_loss) || std::isinf(total_loss))
        throw std::runtime_error("calc_log_loss generated nan/inf");

    return total_loss;
}

uint64_t LRModel::getSerializedGradientSize() const {
    return d * sizeof(double);
}

std::unique_ptr<ModelGradient> LRModel::loadGradient(void* mem) const {
    auto grad = std::make_unique<LRGradient>(d);

    for (uint64_t i = 0; i < d; ++i) {
        grad->weights[i] = reinterpret_cast<double*>(mem)[i];
    }

    return grad;
}

bool LRModel::is_integer(double n) const {
    return floor(n) == n;
}

double LRModel::checksum() const {
    return crc32(weights.data(), weights.size() * sizeof(double));
}

void LRModel::print() const {
    std::cout << "MODEL: ";
    for (const auto& w : weights) {
        std::cout << " " << w;
    }
    std::cout << std::endl;
}


#include "library_functions.hpp"

#include <Eigen/src/Core/Matrix.h>
#include <cmath>
#include <eigen3/Eigen/Core>
#include "eigen3/Eigen/Geometry"
#include <iostream>
#include <math.h>
#include <vector>
#include "amrl_shared_lib/math/geometry.h"
#include "amrl_shared_lib/math/math_util.h"

#include "ast.hpp"

using AST::ast_ptr;
using AST::Bool;
using AST::bool_ptr;
using AST::Num;
using AST::num_ptr;
using AST::Type;
using AST::Vec;
using AST::vec_ptr;
using Eigen::Vector2f;
using Eigen::Vector3i;
using std::abs;
using std::array;
using std::cos;
using std::cout;
using std::dynamic_pointer_cast;
using std::endl;
using std::invalid_argument;
using std::make_shared;
using std::pow;
using std::sin;
using std::min;
using std::max;
using std::vector;
using geometry::Angle;
using math_util::AngleDist;

#define ASSERT_DIM(expression, dimensionality)                  \
  {                                                             \
    if (expression->dims_ != dimensionality) {                  \
      throw invalid_argument("`" #expression                    \
                             "' has incorrect dimensionality"); \
    }                                                           \
  }

#define ASSERT_TYPE(expression, type)                                \
  {                                                                  \
    if (expression->type_ != type) {                                 \
      throw invalid_argument("type mismatch: expected `" #expression \
                             "' to have type `" #type "'");          \
    }                                                                \
  }

#define ASSERT_DIMS_EQUAL(left, right)                               \
  {                                                                  \
    if (left->dims_ != right->dims_) {                               \
      throw invalid_argument("expected `" #left "' and `" #right     \
                             "' to have the same dimensionalities"); \
    }                                                                \
  }

#define ASSERT_TYPES_EQUAL(left, right)                          \
  {                                                              \
    if (left->type_ != right->type_) {                           \
      throw invalid_argument("expected `" #left "' and `" #right \
                             "' to have the same types");        \
    }                                                            \
  }

ast_ptr Plus(ast_ptr left, ast_ptr right) {
  ASSERT_DIMS_EQUAL(left, right);
  ASSERT_TYPES_EQUAL(left, right);
  if (left->type_ == Type::NUM) {
    num_ptr left_cast = dynamic_pointer_cast<Num>(left);
    num_ptr right_cast = dynamic_pointer_cast<Num>(right);
    Num result(left_cast->value_ + right_cast->value_, left->dims_);
    return make_shared<Num>(result);
  } else if (left->type_ == Type::VEC) {
    vec_ptr left_cast = dynamic_pointer_cast<Vec>(left);
    vec_ptr right_cast = dynamic_pointer_cast<Vec>(right);
    Vec result(left_cast->value_ + right_cast->value_, left->dims_);
    return make_shared<Vec>(result);
  } else {
    throw invalid_argument(
        "expected types of `left' and `right' to be `Type::NUM' or "
        "`Type::VEC'");
  }
}

ast_ptr Minus(ast_ptr left, ast_ptr right) {
  ASSERT_DIMS_EQUAL(left, right);
  ASSERT_TYPES_EQUAL(left, right);
  if (left->type_ == Type::NUM) {
    num_ptr left_cast = dynamic_pointer_cast<Num>(left);
    num_ptr right_cast = dynamic_pointer_cast<Num>(right);
    Num result(left_cast->value_ - right_cast->value_, left->dims_);
    return make_shared<Num>(result);
  } else if (left->type_ == Type::VEC) {
    vec_ptr left_cast = dynamic_pointer_cast<Vec>(left);
    vec_ptr right_cast = dynamic_pointer_cast<Vec>(right);
    Vec result(left_cast->value_ - right_cast->value_, left->dims_);
    return make_shared<Vec>(result);
  } else {
    throw invalid_argument(
        "expected types of `left' and `right' to be `Type::NUM' or "
        "`Type::VEC'");
  }
}

ast_ptr AngleDist(ast_ptr left, ast_ptr right) {
  ASSERT_DIMS_EQUAL(left, right);
  ASSERT_TYPES_EQUAL(left, right);
  if (left->type_ == Type::NUM) {
    num_ptr left_cast = dynamic_pointer_cast<Num>(left);
    num_ptr right_cast = dynamic_pointer_cast<Num>(right);
    Num result(AngleDist(left_cast->value_, right_cast->value_), left->dims_);
    return make_shared<Num>(result);
  } else {
    throw invalid_argument(
        "expected types of `left' and `right' to be `Type::NUM'");
  }
}

ast_ptr Times(ast_ptr left, ast_ptr right) {
  ASSERT_TYPE(left, Type::NUM);

  num_ptr left_cast = dynamic_pointer_cast<Num>(left);
  if (right->type_ == Type::NUM) {
    num_ptr right_cast = dynamic_pointer_cast<Num>(right);
    Num result(left_cast->value_ * right_cast->value_,
               left->dims_ + right->dims_);
    return make_shared<Num>(result);
  } else if (right->type_ == Type::VEC) {
    vec_ptr right_cast = dynamic_pointer_cast<Vec>(right);
    Vec result(left_cast->value_ * right_cast->value_,
               left->dims_ + right->dims_);
    return make_shared<Vec>(result);
  } else {
    throw invalid_argument(
        "expected type of `left' to be `Type::NUM' and/or expected type of "
        "`right' to be `Type::NUM' or `Type::VEC'");
  }
}

ast_ptr DividedBy(ast_ptr left, ast_ptr right) {
  ASSERT_TYPE(left, Type::NUM);

  num_ptr right_cast = dynamic_pointer_cast<Num>(right);
  if (left->type_ == Type::NUM) {
    num_ptr left_cast = dynamic_pointer_cast<Num>(left);
    Num result(left_cast->value_ / right_cast->value_,
               left->dims_ - right->dims_);
    return make_shared<Num>(result);
  } else if (left->type_ == Type::VEC) {
    vec_ptr left_cast = dynamic_pointer_cast<Vec>(right);
    Vec result(left_cast->value_ / right_cast->value_,
               left->dims_ - right->dims_);
    return make_shared<Vec>(result);
  } else {
    throw invalid_argument(
        "expected type of `left' to be `Type::NUM' and/or expected type of "
        "`right' to be `Type::NUM' or `Type::VEC'");
  }
}

ast_ptr Abs(ast_ptr operand) {
  ASSERT_TYPE(operand, Type::NUM);

  num_ptr operand_cast = dynamic_pointer_cast<Num>(operand);
  Num result(abs(operand_cast->value_), operand->dims_);
  return make_shared<Num>(result);
}

ast_ptr Pow(ast_ptr base, ast_ptr power) {
  ASSERT_DIM(power, Vector3i(0, 0, 0));
  ASSERT_TYPE(base, Type::NUM);
  ASSERT_TYPE(power, Type::NUM);

  num_ptr base_cast = dynamic_pointer_cast<Num>(base);
  num_ptr power_cast = dynamic_pointer_cast<Num>(power);
  // TODO(simon) figure out something better for dimensionalities
  Num result(pow(base_cast->value_, power_cast->value_),
             base->dims_ * (int)power_cast->value_);
  return make_shared<Num>(result);
}

ast_ptr Sq(ast_ptr x) {
  ASSERT_TYPE(x, Type::NUM);

  num_ptr x_cast = dynamic_pointer_cast<Num>(x);
  Num result(x_cast->value_ * x_cast->value_, 2 * x_cast->dims_);
  return make_shared<Num>(result);
}

ast_ptr Cos(ast_ptr theta) {
  ASSERT_DIM(theta, Vector3i(0, 0, 0));
  ASSERT_TYPE(theta, Type::NUM);

  num_ptr theta_cast = dynamic_pointer_cast<Num>(theta);
  Num result(cos(theta_cast->value_), {0, 0, 0});
  return make_shared<Num>(result);
}

ast_ptr Sin(ast_ptr theta) {
  ASSERT_DIM(theta, Vector3i(0, 0, 0));
  ASSERT_TYPE(theta, Type::NUM);

  num_ptr theta_cast = dynamic_pointer_cast<Num>(theta);
  Num result(sin(theta_cast->value_), {0, 0, 0});
  return make_shared<Num>(result);
}

ast_ptr Cross(ast_ptr u, ast_ptr v) {
  ASSERT_DIMS_EQUAL(u, v);
  ASSERT_TYPE(u, Type::VEC);
  ASSERT_TYPE(v, Type::VEC);

  vec_ptr u_cast = dynamic_pointer_cast<Vec>(u);
  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  // TODO(simon) check dimensionality is correct
  Num result(u_cast->value_.x() * v_cast->value_.y() +
                 u_cast->value_.y() * v_cast->value_.x(),
             u_cast->dims_);
  return make_shared<Num>(result);
}

ast_ptr Dot(ast_ptr u, ast_ptr v) {
  ASSERT_TYPE(u, Type::VEC);
  ASSERT_TYPE(v, Type::VEC);

  vec_ptr u_cast = dynamic_pointer_cast<Vec>(u);
  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  Num result(u_cast->value_.dot(v_cast->value_), u->dims_);
  return make_shared<Num>(result);
}

ast_ptr SqDist(ast_ptr u, ast_ptr v) {
  ASSERT_DIMS_EQUAL(u, v)
  ASSERT_TYPE(u, Type::VEC);
  ASSERT_TYPE(v, Type::VEC);

  vec_ptr u_cast = dynamic_pointer_cast<Vec>(u);
  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  const float euc_dist = (u_cast->value_ - v_cast->value_).norm();
  Num result(euc_dist, u->dims_);
  return make_shared<Num>(result);
}

ast_ptr Angle(ast_ptr v) {
  ASSERT_TYPE(v, Type::VEC);

  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  Num result(atan2(v_cast->value_.y(), v_cast->value_.x()), {0, 0, 0});
  return make_shared<Num>(result);
}

ast_ptr Heading(ast_ptr theta) {
  ASSERT_DIM(theta, Vector3i(0, 0, 0));
  ASSERT_TYPE(theta, Type::NUM);

  num_ptr theta_cast = dynamic_pointer_cast<Num>(theta);
  Vec result({cos(theta_cast->value_), sin(theta_cast->value_)}, {0, 0, 0});
  return make_shared<Vec>(result);
}

ast_ptr NormSq(ast_ptr v) {
  ASSERT_TYPE(v, Type::VEC);

  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  const float norm = v_cast->value_.norm();
  Num result(pow(norm, 2), v->dims_);
  return make_shared<Num>(result);
}

ast_ptr Perp(ast_ptr v) {
  ASSERT_TYPE(v, Type::VEC);

  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  Vec result({-v_cast->value_.y(), v_cast->value_.x()}, v->dims_);
  return make_shared<Vec>(result);
}

ast_ptr VecX(ast_ptr v) {
  ASSERT_TYPE(v, Type::VEC);

  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  Num result(v_cast->value_.x(), v->dims_);
  return make_shared<Num>(result);
}

ast_ptr VecY(ast_ptr v) {
  ASSERT_TYPE(v, Type::VEC);

  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  Num result(v_cast->value_.y(), v->dims_);
  return make_shared<Num>(result);
}

ast_ptr And(ast_ptr P, ast_ptr Q) {
  ASSERT_TYPE(P, Type::BOOL);
  ASSERT_TYPE(Q, Type::BOOL);

  bool_ptr P_cast = dynamic_pointer_cast<Bool>(P);
  bool_ptr Q_cast = dynamic_pointer_cast<Bool>(Q);
  Bool result(P_cast->value_ && Q_cast->value_);
  return make_shared<Bool>(result);
}

ast_ptr Or(ast_ptr P, ast_ptr Q) {
  ASSERT_TYPE(P, Type::BOOL);
  ASSERT_TYPE(Q, Type::BOOL);

  bool_ptr P_cast = dynamic_pointer_cast<Bool>(P);
  bool_ptr Q_cast = dynamic_pointer_cast<Bool>(Q);
  Bool result(P_cast->value_ || Q_cast->value_);
  return make_shared<Bool>(result);
}

ast_ptr Not(ast_ptr P) {
  ASSERT_TYPE(P, Type::BOOL);

  bool_ptr P_cast = dynamic_pointer_cast<Bool>(P);
  Bool result(!P_cast->value_);
  return make_shared<Bool>(result);
}

ast_ptr Eq(ast_ptr x, ast_ptr y) {
  ASSERT_TYPE(x, Type::NUM);
  ASSERT_TYPE(y, Type::NUM);

  num_ptr x_cast = dynamic_pointer_cast<Num>(x);
  num_ptr y_cast = dynamic_pointer_cast<Num>(y);
  Bool result(x_cast->value_ == y_cast->value_);
  return make_shared<Bool>(result);
}

ast_ptr Lt(ast_ptr x, ast_ptr y) {
  ASSERT_TYPE(x, Type::NUM);
  ASSERT_TYPE(y, Type::NUM);

  num_ptr x_cast = dynamic_pointer_cast<Num>(x);
  num_ptr y_cast = dynamic_pointer_cast<Num>(y);
  Bool result(x_cast->value_ - y_cast->value_ < 0.0);
  return make_shared<Bool>(result);
}

ast_ptr Gt(ast_ptr x, ast_ptr y) {
  ASSERT_TYPE(x, Type::NUM);
  ASSERT_TYPE(y, Type::NUM);

  num_ptr x_cast = dynamic_pointer_cast<Num>(x);
  num_ptr y_cast = dynamic_pointer_cast<Num>(y);
  Bool result(x_cast->value_ - y_cast->value_ > 0);
  return make_shared<Bool>(result);
}

ast_ptr Lte(ast_ptr x, ast_ptr y) {
  ASSERT_TYPE(x, Type::NUM);
  ASSERT_TYPE(y, Type::NUM);

  num_ptr x_cast = dynamic_pointer_cast<Num>(x);
  num_ptr y_cast = dynamic_pointer_cast<Num>(y);
  Bool result(x_cast->value_ <= y_cast->value_);
  return make_shared<Bool>(result);
}

ast_ptr Gte(ast_ptr x, ast_ptr y) {
  ASSERT_TYPE(x, Type::NUM);
  ASSERT_TYPE(y, Type::NUM);

  num_ptr x_cast = dynamic_pointer_cast<Num>(x);
  num_ptr y_cast = dynamic_pointer_cast<Num>(y);
  Bool result(x_cast->value_ >= y_cast->value_);
  return make_shared<Bool>(result);
}

ast_ptr StraightFreePathLength(ast_ptr v,
    const vector<Vector2f> obstacles) {
  //TODO(jaholtz) need to set these to sane defaults (copy from sim)
  const float kRobotLength = 0.5;
  const float kRearAxleOffset = 0.0;
  const float kObstacleMargin = 0.5;
  const float kRobotWidth = 0.44;

  ASSERT_TYPE(v, Type::VEC);
  vec_ptr v_cast = dynamic_pointer_cast<Vec>(v);
  const Vector2f end = v_cast->value_;

  // How much the robot's body extends in front of its base link frame.
  const float l = 0.5 * kRobotLength - kRearAxleOffset + kObstacleMargin;
  // The robot's half-width.
  const float w = 0.5 * kRobotWidth + kObstacleMargin;

  float free_path_length = end.norm();
  const float angle = Angle(end);
  const Eigen::Rotation2Df rot(-angle);

  for (const Vector2f& obst :obstacles) {
    Vector2f pose(obst.x(), obst.y());
    // Assuming robot frame, no transform.
    const Vector2f p = rot * pose;
    // If outside width, or behind robot, skip
    if (fabs(p.y()) > w || p.x() < 0.0f) continue;
    // Calculate distance and store if shorter.
    free_path_length = min(free_path_length, p.x() - l);
  }
  // cout << "Free Length: " << free_path_length << endl;
  // cout << "End Norm: " << end.norm() << endl;
  if (fabs(free_path_length - end.norm()) < geometry::kEpsilon) {
    free_path_length = 9999;
  }
  if (free_path_length < 0.0) {
    free_path_length = 0;
  }
  Num result(free_path_length, {1, 0, 0});
  return make_shared<Num>(result);
}

// TODO(simon) implement everything after this point with AST stuff

float Average(vector<float> xs) {
  float average = 0.0f;
  for (float x : xs) {
    average += x;
  }
  average /= xs.size();
  return average;
}

enum Orientation { CLOCKWISE, COLINEAR, COUNTERCLOCKWISE };

Orientation three_point_orientation(Vector2f p0, Vector2f p1, Vector2f p2) {
  float difference = ((p1.y() - p0.y()) * (p2.x() - p1.x())) -
                     ((p1.x() - p0.x()) * (p2.y() - p1.y()));
  if (abs(difference) < 1.0e-6f) {
    return COLINEAR;
  } else if (difference > 0.0f) {
    return CLOCKWISE;
  } else {
    return COUNTERCLOCKWISE;
  }
}

Polygon ConvexHull(Polygon a, Polygon b) {
  // Create a vector containing every in either polygon.
  vector<Vector2f> all_vertices;
  all_vertices.reserve(a.vertices.size() + b.vertices.size());
  all_vertices.insert(all_vertices.end(), a.vertices.begin(), a.vertices.end());
  all_vertices.insert(all_vertices.end(), b.vertices.begin(), b.vertices.end());

  // Find the leftmost vertex, which is guaranteed to be in the hull.
  size_t leftmost_point_index = 0;
  for (size_t i = 1; i < all_vertices.size(); ++i) {
    if (all_vertices[i].x() < all_vertices[leftmost_point_index].x()) {
      leftmost_point_index = i;
    }
  }

  // Find the rest of the hull using gift-wrapping/Jarvis's algorithm
  vector<Vector2f> hull;
  size_t current_point_index = leftmost_point_index;
  do {
    hull.push_back(all_vertices[current_point_index]);
    size_t next_point_index = (current_point_index + 1) % all_vertices.size();
    for (size_t i = 0; i < all_vertices.size(); ++i) {
      if (three_point_orientation(
              all_vertices[current_point_index], all_vertices[i],
              all_vertices[next_point_index]) == COUNTERCLOCKWISE) {
        next_point_index = i;
      }
    }
    current_point_index = next_point_index;
  } while (current_point_index != leftmost_point_index);

  return {hull};
}

bool PointInPolygon(Vector2f point, Polygon polygon) {
  const size_t vertex_count = polygon.vertices.size();
  const Ray ray = {point, Vector2f(1, 0)};
  size_t crossing_count = 0;
  for (size_t i = 0; i < vertex_count; ++i) {
    const size_t j = (i + 1) % vertex_count;
    const LineSegment edge = {polygon.vertices[i], polygon.vertices[j]};
    if (RayIntersection(ray, edge)) {
      crossing_count += 1;
    }
  }
  return (crossing_count % 2) == 1;
}

float cross2d(Vector2f u, Vector2f v) { return u.x() * v.y() - u.y() * v.x(); }

bool RayIntersection(Ray ray, LineSegment line_segment) {
  const Vector2f p = {-ray.direction.y(), ray.direction.x()};
  const Vector2f a_to_origin = ray.origin - line_segment.a;
  const Vector2f a_to_b = line_segment.b - line_segment.a;

  const float denominator = a_to_b.dot(p);
  if (denominator <= 1.0e-6f) {
    return false;
  }

  const float t1 = abs(cross2d(a_to_b, a_to_origin)) / denominator;
  const float t2 = a_to_origin.dot(p) / denominator;

  return (t2 >= 0.0f) && (t2 <= 1.0f) && (t1 >= 0.0f);
}


//
// Created by huangkun on 2020/2/28.
//

#ifndef OPENGV2_SPLINEBUNDLEADJUSTMENTV2_HPP
#define OPENGV2_SPLINEBUNDLEADJUSTMENTV2_HPP

#include <Eigen/Eigen>
#include <ceres/rotation.h>

#include <opengv2/bundle_adjustment/BundleAdjustmentBase.hpp>
#include <opengv2/spline/BsplineReal.hpp>

namespace opengv2 {
    class SplineBundleAdjustmentV2 : public BundleAdjustmentBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        SplineBundleAdjustmentV2(bool fixTsb, bool robust);

        void
        run(const std::map<double, Bodyframe::Ptr> &keyframes,
            const std::map<int, LandmarkBase::Ptr> &landmarks) override;

        template<class T>
        static inline void
        derToRotMatrix(const Eigen::Matrix<T, 3, 1> &unitY, const T &theta, Eigen::Matrix<T, 3, 3> &Rbw) {
            Rbw.row(1) = unitY.transpose();
            Eigen::Matrix<T, 3, 1> n = unitY.cross(Eigen::Vector3d(0, 0, 1));
            n.normalize();
            Eigen::Matrix<T, 3, 1> z = n.cross(unitY);

            Rbw.row(2) = (ceres::cos(theta) * z + ceres::sin(theta) * n).transpose();
            Rbw.row(2) /= Rbw.row(2).norm();
            Rbw.row(0) = Rbw.row(1).cross(Rbw.row(2)); // X = Y x Z
        }

        static inline void rotMatrixToDer(const Eigen::Matrix3d &Rbw, Eigen::Vector3d &unitY, double &theta) {
            unitY = Rbw.row(1).transpose();

            Eigen::Vector3d n = unitY.cross(Eigen::Vector3d(0, 0, 1));
            n /= n.norm();
            Eigen::Vector3d z = n.cross(unitY);
            z /= z.norm();

            double tmp = Rbw.row(2) * z;
            theta = std::acos(tmp > 1.0 ? 1.0 : tmp);// [0, pi]

            if (Rbw.row(2) * n < 0)
                theta = -theta;
        }

        struct ReprojectionError {
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

            ReprojectionError(const Eigen::Ref<const Eigen::Vector3d> &bearingVector,
                              std::shared_ptr<std::vector<std::vector<double>>> basis,
                              bool isMono)
                    : isMono_(isMono), bearingVector_(bearingVector), basis_(std::move(basis)) {}

            template<typename T>
            bool operator()(const T *const cp0, const T *const cp1, const T *const cp2, const T *const cp3,
                            const T *const lm, const T *const Tcb, T *residuals) const {
                // Map to Eigen type
                Eigen::Map<Eigen::Matrix<T, 4, 1> const> const cp_0(cp0);
                Eigen::Map<Eigen::Matrix<T, 4, 1> const> const cp_1(cp1);
                Eigen::Map<Eigen::Matrix<T, 4, 1> const> const cp_2(cp2);
                Eigen::Map<Eigen::Matrix<T, 4, 1> const> const cp_3(cp3);
                Eigen::Map<Eigen::Matrix<T, 3, 1> const> const lm_e(lm);
                Eigen::Quaternion<T> Qcb(Tcb);
                Qcb.normalize();
                Eigen::Map<Eigen::Matrix<T, 3, 1> const> const tcb(Tcb + 4);
                Eigen::Map<Eigen::Matrix<T, 3, 1>> res(residuals);

                // spline evaluation
                Eigen::Matrix<T, 4, 1> twb_theta = basis_->at(0)[0] * cp_0 + basis_->at(0)[1] * cp_1 +
                                                   basis_->at(0)[2] * cp_2 + basis_->at(0)[3] * cp_3;
                Eigen::Matrix<T, 3, 1> y = basis_->at(1)[0] * cp_0.head(3) + basis_->at(1)[1] * cp_1.head(3) +
                                           basis_->at(1)[2] * cp_2.head(3) + basis_->at(1)[3] * cp_3.head(3);
                y.normalize();

                Eigen::Matrix<T, 3, 3> Rbw;
                derToRotMatrix(y, twb_theta[3], Rbw);

                Eigen::Matrix<T, 3, 1> Xb = Rbw * (lm_e - twb_theta.head(3));
                Eigen::Matrix<T, 3, 1> Xc = Qcb * Xb + tcb;
                if (isMono_)
                    Xc.normalize();

                res = bearingVector_ - Xc;
                return true;
            }

            static ceres::CostFunction *
            Create(const Eigen::Ref<const Eigen::Vector3d> &bearingVector,
                   const std::shared_ptr<std::vector<std::vector<double>>> &basis, bool isMono) {
                return (new ceres::AutoDiffCostFunction<ReprojectionError, 3, 4, 4, 4, 4, 3, 7>(
                        new ReprojectionError(bearingVector, basis, isMono)));
            }

            bool isMono_;
            Eigen::Vector3d bearingVector_;

            // for spline evaluation
            std::shared_ptr<std::vector<std::vector<double>>> basis_;
        };

    protected:
        void optimize(ceres::Problem &problem,
                      const std::map<double, Bodyframe::Ptr> &keyframes,
                      const std::map<int, LandmarkBase::Ptr> &landmarks) override;

        bool fixTsb_;
        bool robust_;

        BsplineReal<4> Tspline_; // twb[3] + angle, by cubic spline
    };
}

#endif //OPENGV2_SPLINEBUNDLEADJUSTMENTV2_HPP


#include <iostream>
#include <string>
#include <cmath>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include <boost/range/numeric.hpp>
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Tools/Subdivider/Uniform/LoopT.hh>
#include <tuple>

typedef OpenMesh::TriMesh_ArrayKernelT<>  MyMesh;

using namespace std;
using namespace boost::numeric::odeint;

const double sigma = 1.1;
const double beta = .4;
const double delta = .1;
const double phi = .4;

typedef boost::array< double , 2 > state_type;

void ODE( const state_type &x , state_type &dxdt , double t )
{
    dxdt[0] = (sigma * x[0]) - (beta*x[0]*x[1]);
    dxdt[1] = (delta * x[0] * x[1]) - (phi * x[1]);
}

double prey_count[100];
double pred_count[100];
int i = 0;
tuple<double, double> write_ODE( const state_type &x , const double t )
{

    prey_count[i] = x[0];
    pred_count[i] = x[1];

    cout << t << '\t' << x[0] << '\t' << x[1] << '\t' << prey_count[i] << endl;

    i++;
    return std::make_tuple(pred_count[i], prey_count[i]);

}

int main()
{
  MyMesh mesh;

  //Create Vertices
  MyMesh::VertexHandle vhandle[3];
  vhandle[0] = mesh.add_vertex(MyMesh::Point( 0, 0 ,  0));
  vhandle[1] = mesh.add_vertex(MyMesh::Point( 1, 0 ,  0));
  vhandle[2] = mesh.add_vertex(MyMesh::Point( 1, 1 ,  0));
  vhandle[3] = mesh.add_vertex(MyMesh::Point( 0, 1 ,  0));

  //Create Faces
  std::vector<MyMesh::VertexHandle>  face_vhandles;
  face_vhandles.clear();
  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[3]);
  mesh.add_face(face_vhandles);

 // Initialize Subdivider Tool
 OpenMesh::Subdivider::Uniform::LoopT<MyMesh> Loop;

 // Execute 5 subdivision steps
 Loop.attach(mesh);
 Loop( 3 );
 Loop.detach();

 // Solve ODE
 state_type x = { 30 , 30}; // initial conditions
 runge_kutta4< state_type > stepper;
 integrate(ODE, x, 0.0, 100.0, 0.01, write_ODE);
  for (int k = 0; k<i; k++)
  { cout << prey_count[k] << endl; }

// integrate_n_steps( stepper , ODE , x , 0.0, 1.0 , 50, write_ODE );

string Output;

OpenMesh::IO::Options wopt;
wopt = OpenMesh::IO::Options::FaceColor;

 mesh.request_face_colors();

for (int count = 0; count<i; count++)
{
	int count_prey = prey_count[count];
	int count_pred = pred_count[count];
	int total_count = count_prey + count_pred;
	int z = 0;
	int y = 0;

	for (MyMesh::FaceIter f_it=mesh.faces_begin();  f_it !=mesh.faces_end(); ++f_it)
	{

		if ( z < count_prey)
			{
		    		mesh.set_color(*f_it,MyMesh::Color(0,0,255));
			}

		else if ( (y > count_prey) && (y < total_count) )
			{
				mesh.set_color(*f_it,MyMesh::Color(255,0,0));

			}

		else
			{
		       	        mesh.set_color(*f_it,MyMesh::Color(0,0,0));
			}

			z++;
			y++;
	}

Output = std::to_string(count) + "output.off";
OpenMesh::IO::write_mesh(mesh, Output, wopt);

}

return 0;

}


#include <QApplication>
#include <QKeyEvent>
#include <QPushButton>
#include "GLModelViewer.h"
#include "GLModel.h"
#include "GLShaderProgram.h"
#include "Grid.h"
#include <iostream>
#include <chrono>
#include <sstream>
#include <fstream>
#include <Eigen/Dense>
#include "Timer.h"
#include "Projection.h"
#include "Volumetric_helper.h"

#if 0
void raycast_volume()
{
	timer.start();

	Eigen::Vector3d origin = T.first.col(3).head<3>();
	Eigen::Vector3d window_coord_norm;

	std::vector<Eigen::Vector3d> output_cloud;

	// Sweep the volume looking for the zero crossing
	for (int y = 0; y < window_height * 0.1; ++y)
	{
		std::cout << "Ray casting to image... " << (double)y / window_height * 100 << "%" << std::endl;

		for (int x = 0; x < window_width * 0.1; ++x)
		{
			window_coord_norm.x() = ((double)x / window_width * 2.0) - 1.0;
			window_coord_norm.y() = ((double)y / window_height * 2.0) - 1.0;
			window_coord_norm.z() = origin.z() + near_plane;
			Eigen::Vector3d direction = (window_coord_norm - origin).normalized();

			std::vector<int> intersections = Grid::find_intersections(grid.data, volume_size, voxel_size, grid.transformation, origin, direction, near_plane, far_plane);
			Grid::sort_intersections(intersections, grid.data, origin);

			for (int i = 1; i < intersections.size(); ++i)
			{
				const Voxeld& prev = grid.data.at(i - 1);
				const Voxeld& curr = grid.data.at(i);

				const bool& same_sign = ((prev.tsdf < 0) == (curr.tsdf < 0));

				if (!same_sign)		// it is a zero-crossing
				{
					output_cloud.push_back(curr.point);
				}
			}
		}
	}

	timer.print_interval("Raycasting volume   : ");

	export_obj("../../data/output_cloud.obj", output_cloud);
}
#endif

// Usage: ./Volumetricd.exe ../../data/monkey.obj 256 4 2 90
int main(int argc, char **argv)
{
	if (argc < 6)
	{
		std::cerr << "Missing parameters. Abort."
			<< std::endl
			<< "Usage:  ./Volumetricd.exe ../../data/monkey.obj 256 8 2 90"
			<< std::endl;
		return EXIT_FAILURE;
	}
	Timer timer;
	const std::string filepath = argv[1];
	const int vol_size = atoi(argv[2]);
	const int vx_size = atoi(argv[3]);
	const int cloud_count = atoi(argv[4]);
	const int rot_interval = atoi(argv[5]);

	std::pair<std::vector<double>, std::vector<double>> depth_buffer;

	//
	// Projection and Modelview Matrices
	//
	Eigen::Matrix4d K = perspective_matrix(fov_y, aspect_ratio, near_plane, far_plane);
	std::pair<Eigen::Matrix4d, Eigen::Matrix4d>	T(Eigen::Matrix4d::Identity(), Eigen::Matrix4d::Identity());

	//
	// Creating volume
	//
	Eigen::Vector3d voxel_size(vx_size, vx_size, vx_size);
	Eigen::Vector3d volume_size(vol_size, vol_size, vol_size);
	Eigen::Vector3d voxel_count(volume_size.x() / voxel_size.x(), volume_size.y() / voxel_size.y(), volume_size.z() / voxel_size.z());
	//
	Eigen::Affine3d grid_affine = Eigen::Affine3d::Identity();
	grid_affine.translate(Eigen::Vector3d(0, 0, -256));
	grid_affine.scale(Eigen::Vector3d(1, 1, -1));	// z is negative inside of screen

	Grid<double> grid(volume_size, voxel_size, grid_affine.matrix());

	//
	// Importing .obj
	//
	timer.start();
	std::vector<Eigen::Vector3d> points3DOrig, pointsTmp;
	import_obj(filepath, points3DOrig);
	timer.print_interval("Importing monkey    : ");
	std::cout << "Monkey point count  : " << points3DOrig.size() << std::endl;

	//
	// Translating and rotating monkey point cloud
	std::pair<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>> cloud;
	//
	Eigen::Affine3d rotate = Eigen::Affine3d::Identity();
	Eigen::Affine3d translate = Eigen::Affine3d::Identity();
	translate.translate(Eigen::Vector3d(0, 0, -256));

	//
	// Compute first cloud
	//
	for (Eigen::Vector3d p3d : points3DOrig)
	{
		Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
		rot /= rot.w();
		cloud.first.push_back(rot.head<3>());
	}
	//
	// Update grid with first cloud
	//
	timer.start();
	create_depth_buffer<double>(depth_buffer.first, cloud.first, K, Eigen::Matrix4d::Identity(), far_plane);
	timer.print_interval("CPU compute depth   : ");

	timer.start();
	update_volume(grid, depth_buffer.first, K, T.first);
	timer.print_interval("CPU Update volume   : ");

	//
	// Compute next clouds
	Eigen::Matrix4d cloud_mat = Eigen::Matrix4d::Identity();
	Timer iter_timer;
	for (int i = 1; i < cloud_count; ++i)
	{
		std::cout << std::endl << i << " : " << i * rot_interval << std::endl;
		iter_timer.start();

		// Rotation matrix
		rotate = Eigen::Affine3d::Identity();
		rotate.rotate(Eigen::AngleAxisd(DegToRad(i * rot_interval), Eigen::Vector3d::UnitY()));

		cloud.second.clear();
		for (Eigen::Vector3d p3d : points3DOrig)
		{
			Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
			rot /= rot.w();
			cloud.second.push_back(rot.head<3>());
		}

		//export_obj("../../data/cloud_cpu_2.obj", cloud.second);

		timer.start();
		create_depth_buffer<double>(depth_buffer.second, cloud.second, K, Eigen::Matrix4d::Identity(), far_plane);
		timer.print_interval("Compute depth buffer: ");

		//export_depth_buffer("../../data/cpu_depth_buffer_2.obj", depth_buffer.second);

		timer.start();
		Eigen::Matrix4d icp_mat;
		ComputeRigidTransform(cloud.first, cloud.second, icp_mat);
		timer.print_interval("Compute rigid transf: ");

		//std::cout << std::fixed << std::endl << "icp_mat " << std::endl << icp_mat << std::endl;

		// accumulate matrix
		cloud_mat = cloud_mat * icp_mat;

		//std::cout << std::fixed << std::endl << "cloud_mat " << std::endl << cloud_mat << std::endl;

		timer.start();
		//update_volume(grid, depth_buffer.second, K, cloud_mat.inverse());
		update_volume(grid, depth_buffer.second, K, cloud_mat.inverse());
		timer.print_interval("Update volume       : ");

		// copy second point cloud to first
		cloud.first = cloud.second;
		//depth_buffer.first = depth_buffer.second;

		iter_timer.print_interval("Iteration time      : ");
	}

	//std::cout << "------- // --------" << std::endl;
	//for (int i = 0; i <  grid.data.size(); ++i)
	//{
	//	const Eigen::Vector3d& point = grid.data[i].point;

	//	std::cout << point.transpose() << "\t\t" << grid.data[i].tsdf << " " << grid.data[i].weight << std::endl;
	//}
	//std::cout << "------- // --------" << std::endl;

//	timer.start();
//	export_volume("../../data/grid_volume_cpu.obj", grid.data);
//	timer.print_interval("Exporting volume    : ");
//	return 0;

	QApplication app(argc, argv);

	//
	// setup opengl viewer
	//
	GLModelViewer glwidget;
	glwidget.resize(640, 480);
	glwidget.setPerspective(60.0f, 0.1f, 10240.0f);
	glwidget.move(320, 0);
	glwidget.setWindowTitle("Point Cloud");
	glwidget.setWeelSpeed(0.1f);
	glwidget.setPosition(0, 0, -0.5f);
	glwidget.show();

	Eigen::Matrix4d to_origin = Eigen::Matrix4d::Identity();
	to_origin.col(3) << -(volume_size.x() / 2.0), -(volume_size.y() / 2.0), -(volume_size.z() / 2.0), 1.0;	// set translate

	std::vector<Eigen::Vector4f> vertices, colors;

	int i = 0;
	for (int z = 0; z <= volume_size.z(); z += voxel_size.z())
	{
		for (int y = 0; y <= volume_size.y(); y += voxel_size.y())
		{
			for (int x = 0; x <= volume_size.x(); x += voxel_size.x(), i++)
			{
				const float tsdf = grid.data.at(i).tsdf;

				//Eigen::Vector4d p = grid_affine.matrix() * to_origin * Eigen::Vector4d(x, y, z, 1);
				Eigen::Vector4d p = to_origin * Eigen::Vector4d(x, y, z, 1);
				p /= p.w();

				if (tsdf > 0.1)
				{
					vertices.push_back(p.cast<float>());
					colors.push_back(Eigen::Vector4f(0, 1, 0, 1));
				}
				else if (tsdf < -0.1)
				{
					vertices.push_back(p.cast<float>());
					colors.push_back(Eigen::Vector4f(1, 0, 0, 1));
				}
			}
		}
	}

	//
	// setup model
	//
	std::shared_ptr<GLModel> model(new GLModel);
	model->initGL();
	model->setVertices(&vertices[0][0], vertices.size(), 4);
	model->setColors(&colors[0][0], colors.size(), 4);
	glwidget.addModel(model);

	//
	// setup kinect shader program
	//
	std::shared_ptr<GLShaderProgram> kinectShaderProgram(new GLShaderProgram);
	if (kinectShaderProgram->build("color.vert", "color.frag"))
		model->setShaderProgram(kinectShaderProgram);

	return app.exec();
}


// Original work Copyright (c) 2017, University of Minnesota
// Modified work Copyright 2020, Yue Peng
//
// ADMM-Elastic Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other materials
//    provided with the distribution.
// THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF MINNESOTA, DULUTH OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
// IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef ADMM_FORCE_H
#define ADMM_FORCE_H 1

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

namespace admm {

//
//	Lame constants
//
class Lame {
public:
    static Lame rubber(){ return Lame(10000000,0.499); } // true rubber
    static Lame soft_rubber(){ return Lame(10000000,0.399); } // fun rubber!
    static Lame very_soft_rubber(){ return Lame(1000000,0.299); } // more funner!

    long double mu, lambda;
    long double bulk_modulus() const { return lambda + (2.0/3.0)*mu; }

    // Hard strain limiting (e.g. [0.95,1.05]), default no limit
    // with  min: -inf to 1, max: 1 to inf.
    // In practice if max>99 it's basically no limiting.
    long double limit_min, limit_max;

    // k: Youngs (Pa), measure of stretch
    // v: Poisson, measure of incompressibility
    Lame( long double k, long double v ) :
        mu(k/(2.0*(1.0+v))),
        lambda(k*v/((1.0+v)*(1.0-2.0*v))),
        limit_min(-100.0),
        limit_max(100.0) {
    }

    // Use custom mu, lambda
    Lame(): limit_min(-100.0),
        limit_max(100.0) {}
};

//
//	Base class tets: Linear (non-corotated) elastic
//
class EnergyTerm {
private:
    typedef Eigen::Matrix<double,Eigen::Dynamic,1> VecX;
    typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatX;
    typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SparseMat;
    int g_index; // global idx (starting row of reduction matrix)

public:

    virtual ~EnergyTerm() {}

    // Called by the solver to create the global reduction and weight matrices
    inline void get_reduction( std::vector< Eigen::Triplet<double> > &triplets, std::vector<double> &weights );

    // Called by the solver for a local step update
    //inline void update( const SparseMat &D, const VecX &x, VecX &z, VecX &u );

    // Called by the solver for a local step update
    inline void update_z(const SparseMat &D, const SparseMat &W_inv, const SparseMat &W, const VecX &x, VecX &z, const VecX &u , const VecX &c);

    // Called by the solver for DR update
    inline void update_DR_z(const SparseMat &W_inv, const SparseMat &W, VecX &z,
                                         const VecX &s, const VecX &u, const VecX &c );

    inline void update_DR_xzu_z(const SparseMat &W_inv, const SparseMat &W, VecX &z,
                                         const VecX &s);

    // Called by the solver for a local step update
    inline void update_u(const SparseMat &D, const SparseMat &W, const VecX &x, const VecX &z, VecX &u , const VecX &c);

    // Computes energy of the force (used for debugging)
    inline double energy( const SparseMat &D, const VecX &x );

    inline double get_all_energy(const VecX &x );
    // Compute energy and gradient for a first-order opt. solver
//	inline double gradient( const SparseMat &D, const VecX &x, VecX &grad );
    inline void gradient( const SparseMat &D, const VecX &x, VecX &grad );

    inline void get_all_gradient(const VecX &z, VecX &grad );

    inline void compute_x_gradient(const VecX &grad, VecX &all_grad );

    // Dimension of deformation gradient.
    virtual int get_dim() const = 0;

    // Return the scalar weight of the energy term
    virtual double get_weight() const = 0;

    virtual double get_volume() const = 0;

protected:

    virtual void get_gradient_for_vertices( const VecX &grad, VecX &all_grad) = 0;

    // Get a local reduction matrix
    virtual void get_reduction( std::vector< Eigen::Triplet<double> > &triplets ) = 0;

    // Proximal update
    virtual void prox( const MatX &W, VecX &zi, const VecX &vi) = 0;

    // Returns energy of the force
    virtual double energy( const VecX &F ) = 0;

    virtual double energyLBFGS( const VecX &F ) = 0;

    // Computes a first-order update (energy and gradient)
    virtual void gradient(const VecX &x, VecX &grad) = 0;

    virtual void get_gradient( const VecX &F, VecX &grad ) = 0;

}; // end class EnergyTerm

//
//  Implementation
//

inline void EnergyTerm::get_reduction( std::vector< Eigen::Triplet<double> > &triplets, std::vector<double> &weights ){
    std::vector< Eigen::Triplet<double> > temp_triplets;

    get_reduction( temp_triplets );
    int n_trips = temp_triplets.size();
    g_index = weights.size();

    for( int i=0; i<n_trips; ++i ){
        const Eigen::Triplet<double> &trip = temp_triplets[i];
        triplets.emplace_back( trip.row()+g_index, trip.col(), trip.value() );
    }

    int dim = get_dim();
    double w = get_weight();
    if( w <= 0.0 ){
        throw std::runtime_error("**EnergyTerm::get_reduction Error: Some weight leq 0");
    }
    for( int i=0; i<dim; ++i ){ weights.emplace_back( w ); }
}

inline void EnergyTerm::update_u( const SparseMat &D, const SparseMat &W, const VecX &x, const VecX &z, VecX &u, const VecX &c ){
    int dof = x.rows();
    int dim = get_dim();
    VecX Dix = D.block(g_index,0,dim,dof)*x;
    VecX ui = u.segment(g_index,dim);
//    VecX zi = W.block(g_index,g_index,dim,dim) * z.segment(g_index,dim);
    VecX zi = z.segment(g_index,dim);
    VecX ci = c.segment(g_index,dim);
    ui += (Dix - zi - ci);
    u.segment(g_index,dim) = ui;
}

inline void EnergyTerm::update_z( const SparseMat &D, const SparseMat &W_inv, const SparseMat &W, const VecX &x, VecX &z, const VecX &u, const VecX &c ){
    int dof = x.rows();
    const int dim = get_dim();
    VecX Dix = D.block(g_index,0,dim,dof)*x;
    VecX ui = u.segment(g_index,dim);
    VecX ci = c.segment(g_index,dim);
    VecX vi = Dix + ui - ci;
//    VecX zi = W_inv.block(g_index,g_index,dim,dim) * vi;
//    vi = zi;
    VecX zi = vi;
    Eigen::Matrix<double,9,9> Wi = W.block(g_index,g_index,dim,dim);
    prox(Wi, zi, vi);
    z.segment(g_index,dim) = zi;
}

inline void EnergyTerm::update_DR_z( const SparseMat &W_inv, const SparseMat &W, VecX &z,
                                     const VecX &s, const VecX &u, const VecX &c ){
    const int dim = get_dim();
    VecX ui = u.segment(g_index,dim);
    VecX ci = c.segment(g_index,dim);
    VecX si = s.segment(g_index,dim);
    VecX vi = 2*ui - si - ci;
//    VecX zi = W_inv.block(g_index,g_index,dim,dim) * vi;
//    vi = zi;
    VecX zi = vi;
    Eigen::Matrix<double,9,9> Wi = W.block(g_index,g_index,dim,dim);
    prox(Wi, zi, vi);
    z.segment(g_index,dim) = zi;
}

inline void EnergyTerm::update_DR_xzu_z( const SparseMat &W_inv, const SparseMat &W, VecX &z,
                                     const VecX &s){
    const int dim = get_dim();
    VecX si = s.segment(g_index,dim);
//    VecX zi = W_inv.block(g_index,g_index,dim,dim) * si;
//    si = zi;
    VecX zi = si;

    Eigen::Matrix<double,9,9> Wi = W.block(g_index,g_index,dim,dim);
    prox(Wi, zi, si);
    z.segment(g_index,dim) = zi;
}

inline double EnergyTerm::energy( const SparseMat &D, const VecX &x ){
    (void)(D);
    int dim = get_dim();
    VecX Dix = x.segment(g_index,dim);
    return energy( Dix );
}

inline double EnergyTerm::get_all_energy( const VecX &x ){
    int dim = get_dim();
    VecX Dix = x.segment(g_index,dim);
    return energyLBFGS( Dix );
}

inline void EnergyTerm::gradient( const SparseMat &D, const VecX &x, VecX &grad ){
    int dof = x.rows();
    int dim = get_dim();
    VecX Dix = D.block(g_index,0,dim,dof)*x;
    gradient( Dix, grad );
}

inline void EnergyTerm::get_all_gradient( const VecX &z, VecX &grad ){
    int dim = get_dim();
    VecX zi = z.segment(g_index,dim);
    VecX grad_z = grad.segment(g_index,dim);
    get_gradient( zi, grad_z );
    grad.segment(g_index,dim) = grad_z;
}

inline void EnergyTerm::compute_x_gradient(const VecX &grad, VecX &all_grad ){
    int dim = get_dim();
    VecX gi = grad.segment(g_index,dim);
    get_gradient_for_vertices(gi, all_grad);
}
} // end namespace admm

#endif


#ifndef _TRIFORCE_MESH_GEN_H_
#define _TRIFORCE_MESH_GEN_H_
#include <array>
#include <map>
#include <vector>
#include <string>
#include <fstream>
#include <Eigen/Dense>
#include "compat.h"
template <typename Scalar>
class TriforceMeshFactory{
    public:
        typedef mtao::compat::array<int, 3> Face;
        typedef mtao::compat::array<int, 2> Edge;
        typedef Scalar Scalar;
        typedef typename Eigen::Matrix<Scalar,3,1> Vector;
        using VecVector = mtao::vector<Vector>;
        TriforceMeshFactory(int depth=3);
        void triforce(const Face & f, int depth);
        int add_edge(Edge e);
        void write(const std::string & filename);
        void write(std::ostream & outstream);
        const std::vector<Face> faces() const {return m_faces;}
        const VecVector vertices() const {return m_vertices;}

    private:
        const int m_depth = 0;
        VecVector m_vertices;
        std::vector<Face> m_faces;
        mtao::IndexMap<2> m_edges;
        //std::map<Edge,  int> m_edges;

};

template <typename T>
TriforceMeshFactory<T>::TriforceMeshFactory(int depth): m_depth(depth) {
}
template <typename T>
void TriforceMeshFactory<T>::run_in_place(VecVector& vertices, std::vector<Face>& faces) {
    //Create icosahedron base

    Scalar gr = .5 * (1 + std::sqrt(Scalar(5)));
    m_vertices.resize(5);

    m_vertices[ 0] = Vector(     0,    - 1,      0);
    m_vertices[ 1] = Vector(     0,      1,      0);
    m_vertices[ 2] = Vector(   - 1,      0,      0);
    m_vertices[ 3] = Vector(     1,      0,      0);
    m_vertices[ 4] = Vector(     0,      0,      1);

    triforce({{ 0, 1, 4}},depth);
    triforce({{ 1, 2, 4}},depth);
    triforce({{ 2, 3, 4}},depth);
    triforce({{ 3, 0, 4}},depth);

}
template <typename T>
void TriforceMeshFactory<T>::triforce(const Face & f, int depth) {
    if(depth <= 0) {
        m_faces.push_back(f);
    } else {
        int e01 = add_edge({{f[0],f[1]}});
        int e12 = add_edge({{f[1],f[2]}});
        int e02 = add_edge({{f[0],f[2]}});
        triforce({{f[0],e01,e02}},depth-1);
        triforce({{f[1],e12,e01}},depth-1);
        triforce({{f[2],e02,e12}},depth-1);
        triforce({{e01 ,e12,e02}},depth-1);
    }

}

template <typename T>
int TriforceMeshFactory<T>::add_edge(Edge e) {
    if(e[0] > e[1]) {
        int tmp = e[0];
        e[0] = e[1];
        e[1] = tmp;
    }
    auto it = m_edges.find(e);
    if(it != m_edges.end()) {
        return it->second;
    } else {
        m_edges[e] = m_vertices.size();
        m_vertices.push_back(
                (m_vertices[e[0]] + m_vertices[e[1]]).normalized()
                );
        return m_vertices.size()-1;
    }

}

template <typename T>
void TriforceMeshFactory<T>::write(const std::string & filename) {
    std::ofstream outstream(filename.c_str());
    write(outstream);
}

template <typename T>
void TriforceMeshFactory<T>::write(std::ostream & outstream) {
    outstream << "#Icosahedral subdivision to depth " << m_depth << std::endl;
    for(auto&& v: m_vertices) {
        outstream << "v " << v.transpose() << std::endl;
    }

    for(auto&& f: m_faces) {
        outstream << "f " << f[0]+1 << " " << f[1]+1 << " " << f[2]+1 << std::endl;
    }
}
#endif


#include "Object.h"
#include "Ray.h"
#include "Camera.h"
#include "Sphere.h"
#include "Plane.h"
#include "read_json.h"
#include "write_ppm.h"
#include "viewing_ray.h"
#include "first_hit.h"
#include <Eigen/Core>
#include <vector>
#include <iostream>
#include <memory>
#include <limits>
#include <functional>

int main(int argc, char * argv[])
{
  // list of colors used for per-object id coloring
  const std::vector<unsigned char> color_map = {
    228,26,28,
    55,126,184,
    77,175,74,
    152,78,163,
    255,127,0,
    255,255,51,
    166,86,40,
    247,129,191,
    153,153,153
  };

  Camera camera;
  std::vector< std::shared_ptr<Object> > objects;
  // Read a camera and scene description from given .json file
  read_json(argc<=1?"../data/sphere-and-plane.json":argv[1],camera,objects);

  int width = 640;
  int height = 360;
  std::vector<unsigned char> id_image(3*width*height);
  std::vector<unsigned char> normal_image(3*width*height);
  std::vector<unsigned char> depth_image(1*width*height);
  // For each pixel (i,j)
  for(unsigned i=0; i<height; ++i)
  {
    for(unsigned j=0; j<width; ++j)
    {
      // Set background color
      normal_image[0+3*(j+width*i)] = 0;
      normal_image[1+3*(j+width*i)] = 0;
      normal_image[2+3*(j+width*i)] = 0;
      depth_image[j+width*i] = 0;
      id_image[0+3*(j+width*i)] = 0;
      id_image[1+3*(j+width*i)] = 0;
      id_image[2+3*(j+width*i)] = 0;

      // Compute viewing ray
      Ray ray;
      viewing_ray(camera,i,j,width,height,ray);

      // Find first visible object hit by ray and its surface normal n
      double t;
      Eigen::Vector3d n;
      int hit_id;
      if(first_hit(ray,1.0,objects,hit_id,t,n))
      {
        // object-id image
        const int color_id = hit_id%(color_map.size()/3);
        id_image[0+3*(j+width*i)] = color_map[0+3*color_id];
        id_image[1+3*(j+width*i)] = color_map[1+3*color_id];
        id_image[2+3*(j+width*i)] = color_map[2+3*color_id];

        // depth image
        const double zNear = camera.d;
        double linearized_depth = zNear/(t*ray.direction.norm());
        linearized_depth = linearized_depth<1?linearized_depth:1;
        depth_image[j+width*i] = 255.0*(linearized_depth);

        // set pixel color to value computed from hit point, light, and n
        // normal image
        auto normal_to_rgb = [](const Eigen::Vector3d & n, unsigned char & r, unsigned char & g, unsigned char & b)
        {
          r = 255.0*(n(0)*0.5+0.5);
          g = 255.0*(n(1)*0.5+0.5);
          b = 255.0*(n(2)*0.5+0.5);
        };
        normal_to_rgb(
          n,
          normal_image[0+3*(j+width*i)],
          normal_image[1+3*(j+width*i)],
          normal_image[2+3*(j+width*i)]);
      }
    }
  }

  write_ppm("normal.ppm",normal_image,width,height,3);
  write_ppm("depth.ppm",depth_image,width,height,1);
  write_ppm("id.ppm",id_image,width,height,3);
}