#include <iostream>
#include <iomanip>
#include <chrono>
#include <stack>
#include <armadillo>
#include "nanmath.h"

class Clock {
public:
    void tic() {
        ts.push(std::chrono::high_resolution_clock::now());
    }

    long toc() {
        if(ts.empty()) {
            throw "unmatched tic/toc";
        }
        auto t0 = ts.top();
        ts.pop();
        auto t1 = std::chrono::high_resolution_clock::now();
        return std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
    }
private:
    std::stack<std::chrono::high_resolution_clock::time_point> ts;
};

int main(int argc, char *argv[]) {
    if(argc != 2) {
        std::cerr << "usage: " << argv[0] << " <input_matrix>" << std::endl;
        return -1;
    }

    Clock clk;
    arma::Mat<float> mf;
    arma::Mat<double> md;
    mf.load(argv[1]);
    md.load(argv[1]);

    unsigned n_rows = mf.n_rows, n_cols = mf.n_cols;
    std::cout << "processing " << n_rows << " x " << n_cols << " matrix" << std::endl;
    double sum = 0.0;
    unsigned count = 0;

    clk.tic();
    {
        sum = 0.0;
        count = 0;
        float corr;
        unsigned cnt;
        for(unsigned i = 0; i < n_cols; ++i) {
            for(unsigned j = i + 1; j < n_cols; ++j) {
                nan_corr_float(mf.colptr(i), mf.colptr(j), n_rows, &corr, &cnt);
                sum += corr;
                count += cnt;
            }
        }
    }
    std::cout << std::setw(32) << std::right << "nan_corr_float: "
              << std::setw(12) << std::left << std::to_string(clk.toc()) + " ms"
              << std::setw(12) << std::right << "sum: "
              << std::setw(12) << std::left << sum
              << std::setw(12) << std::right << "count: "
              << std::setw(12) << std::left << count << std::endl;

    clk.tic();
    {
        sum = 0.0;
        count = 0;
        float corr;
        unsigned cnt;
        for(unsigned i = 0; i < n_cols; ++i) {
            for(unsigned j = i + 1; j < n_cols; ++j) {
                nan_corr_float_avx(mf.colptr(i), mf.colptr(j), n_rows, &corr, &cnt);
                sum += corr;
                count += cnt;
            }
        }
    }
    std::cout << std::setw(32) << std::right << "nan_corr_float_avx: "
              << std::setw(12) << std::left << std::to_string(clk.toc()) + " ms"
              << std::setw(12) << std::right << "sum: "
              << std::setw(12) << std::left << sum
              << std::setw(12) << std::right << "count: "
              << std::setw(12) << std::left << count << std::endl;

    clk.tic();
    {
        sum = 0.0;
        count = 0;
        double corr;
        unsigned cnt;
        for(unsigned i = 0; i < n_cols; ++i) {
            for(unsigned j = i + 1; j < n_cols; ++j) {
                nan_corr_double(md.colptr(i), md.colptr(j), n_rows, &corr, &cnt);
                sum += corr;
                count += cnt;
            }
        }
    }
    std::cout << std::setw(32) << std::right << "nan_corr_double: "
              << std::setw(12) << std::left << std::to_string(clk.toc()) + " ms"
              << std::setw(12) << std::right << "sum: "
              << std::setw(12) << std::left << sum
              << std::setw(12) << std::right << "count: "
              << std::setw(12) << std::left << count << std::endl;

    clk.tic();
    {
        sum = 0.0;
        count = 0;
        double corr;
        unsigned cnt;
        for(unsigned i = 0; i < n_cols; ++i) {
            for(unsigned j = i + 1; j < n_cols; ++j) {
                nan_corr_double_avx(md.colptr(i), md.colptr(j), n_rows, &corr, &cnt);
                sum += corr;
                count += cnt;
            }
        }
    }
    std::cout << std::setw(32) << std::right << "nan_corr_double_avx: "
              << std::setw(12) << std::left << std::to_string(clk.toc()) + " ms"
              << std::setw(12) << std::right << "sum: "
              << std::setw(12) << std::left << sum
              << std::setw(12) << std::right << "count: "
              << std::setw(12) << std::left << count << std::endl;

    return 0;
}


#pragma once

// system includes ---------------------------------------------------------
#include <fstream>
#include <iostream>

#include <boost/static_assert.hpp>
#include <regex>

// own includes ------------------------------------------------------------
#include "enum/enum.hpp"
#include "spectral_basis.hpp"
#include "spectral_basis_factory_base.hpp"
#include "spectral_elem.hpp"
#include "spectral_elem_accessor.hpp"
#include "spectral_function.hpp"

namespace boltzmann {

namespace local_ {
template <typename BASIS>
struct CMP
{
  /**
   * @brief lexicographical ordering for basis elements
   *
   *
   * @return
   */
  template <typename E>
  bool operator()(const E& e1, const E& e2) const
  {
    int l1 = get_xi(e1).get_id().l;
    int l2 = get_xi(e2).get_id().l;

    int t1 = get_xi(e1).get_id().t;
    int t2 = get_xi(e2).get_id().t;

    int k1 = get_phi(e1).get_id().k;
    int k2 = get_phi(e2).get_id().k;

    if (l1 < l2)
      return true;
    else if (l1 == l2) {
      if (k1 < k2) return true;
      if (k1 == k2) return t1 < t2;
    }
    return false;
  }

 private:
  typename BASIS::elem_t::Acc::template get<XiR> get_xi;
  typename BASIS::elem_t::Acc::template get<LaguerreRR> get_phi;
};
}  // end namespace local_

// ----------------------------------------------------------------------
class SpectralBasisFactory : public SpectralBasisFactoryBase<XiR, LaguerreRR>
{
 public:
  /// definition of element ordering, TODO: define this at a global place
  /**
   *
   *
   * @param basis
   * @param K
   * @param L
   * @param beta
   * @param sorted sort basis functions by `l`, the angular index
   */
  static void create(basis_type& basis, int K, int L, double beta, bool sorted = true);

  /**
   *
   *
   * @param basis
   * @param K
   * @param L
   * @param beta
   * @param sorted sort basis functions by `l`, the angular index
   */
  static void create_test(basis_type& basis, int K, int L, double beta, bool sorted = true);

  static void write_basis_descriptor(const basis_type& basis,
                                     std::string fname = "spectral_basis.desc");

  /**
   * @brief read basis_descriptor file
   *
   * @param basis
   * @param descriptor_file
   */
  static void create(basis_type& basis, std::string descriptor_file);
} __attribute__((deprecated));

// ----------------------------------------------------------------------
void
SpectralBasisFactory::create(basis_type& basis, int K, int L, double beta, bool sorted)
{
  for (int k = 0; k < K; ++k) {
    if (k % 2 == 0) {
      // l = 0
      fa_type xir(0, 0);
      fr_type phi(1. / beta, k);
      basis.add_elem(xir, phi);
    }
    for (int l = 1; l <= L; ++l) {
      for (int t = 0; t < 2; ++t) {
        if (k % 2 == l % 2) {
          fa_type xir(t, l);
          fr_type phi(1. / beta, k);

          basis.add_elem(xir, phi);
        }
      }
    }
  }

  /// sort basis functions by l-index
  if (sorted) {
    basis.sort(local_::CMP<basis_type>());
  }

  basis.finalize();
}

// ----------------------------------------------------------------------
void
SpectralBasisFactory::create_test(basis_type& basis, int K, int L, double beta, bool sorted)
{
  for (int k = 0; k < K; ++k) {
    if (k % 2 == 0) {
      // l = 0
      fa_type xir(0, 0);

      double fw = 1. / beta;
      if (k == 0 || k == 2) fw = 0;
      fr_type phi(fw, k);
      basis.add_elem(xir, phi);
    }

    for (int l = 1; l <= L; ++l) {
      for (int t = 0; t < 2; ++t) {
        if (k % 2 == l % 2) {
          fa_type xir(t, l);
          double fw = 1 / beta;
          if (k == 1 && l == 1) fw = 0;
          fr_type phi(fw, k);
          basis.add_elem(xir, phi);
        }
      }
    }
  }

  /// sort basis functions by l-index
  if (sorted) {
    basis.sort(local_::CMP<basis_type>());
  }

  basis.finalize();
}

// ----------------------------------------------------------------------
void
SpectralBasisFactory::write_basis_descriptor(const basis_type& basis, std::string fname)
{
  // typename elem_t::Acc::template get<fa_type> xir_getter;
  // typename elem_t::Acc::template get<fr_type> rr_getter;
  typename elem_t::Acc::get<fa_type> xir_getter;
  typename elem_t::Acc::get<fr_type> rr_getter;

  std::ofstream fout(fname);
  for (auto it = basis.begin(); it != basis.end(); ++it) {
    fout << xir_getter(*it).get_id() << "\t" << rr_getter(*it).get_id() << std::endl;
  }
  fout.close();
}

// ----------------------------------------------------------------------
void
SpectralBasisFactory::create(basis_type& basis, std::string descriptor_file)
{
  std::ifstream ifile;
  ifile.open(descriptor_file);
  std::string line;

  std::regex basisf_regex(
      "(cos|sin)_([0-9]+)[[:space:]]*[(]beta_([0-9.]+),[[:space:]]*k_([0-9]+)[)]");
  // new descriptor
  std::regex basisf_regex_new(
      "(cos|sin)_([0-9.]+)[[:space:]]*[(]fw_([0-9.]+),[[:space:]]*k_([0-9]+)[)]");
  std::smatch basisf_match;

  while (std::getline(ifile, line)) {
    std::regex_search(line, basisf_match, basisf_regex);
    if (basisf_match.size() > 0) {
      auto sincos_match = basisf_match[1];
      auto l_match = basisf_match[2];
      auto beta_match = basisf_match[3];
      auto k_match = basisf_match[4];
      int l = atoi(l_match.str().c_str());
      int k = atoi(k_match.str().c_str());
      double beta = atof(beta_match.str().c_str());

      fr_type phi(1. / beta, k);
      if (sincos_match.str().compare("sin") == 0) {
        fa_type xir(TRIG::SIN, l);
        basis.add_elem(xir, phi);
      } else {
        fa_type xir(TRIG::COS, l);
        basis.add_elem(xir, phi);
      }
    }
    // try to match new descriptor
    std::regex_search(line, basisf_match, basisf_regex_new);
    if (basisf_match.size() > 0) {
      auto sincos_match = basisf_match[1];
      // angular frequency
      auto l_match = basisf_match[2];
      int l = atoi(l_match.str().c_str());
      // exponential weight
      auto fw_match = basisf_match[3];
      double fw = atof(fw_match.str().c_str());
      // laguerre poly index
      auto k_match = basisf_match[4];
      int k = atoi(k_match.str().c_str());

      fr_type phi(fw, k);
      if (sincos_match.str().compare("sin") == 0) {
        fa_type xir(TRIG::SIN, l);
        basis.add_elem(xir, phi);
      } else {
        fa_type xir(TRIG::COS, l);
        basis.add_elem(xir, phi);
      }
    }
  }

  basis.finalize();
  ifile.close();
}

}  // end namespace boltzmann


/*
  Copyright (C) 2017 Ahmed Riza

  This file is part of MathFin.

  This program 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.

  This  program  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 this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/range/irange.hpp>

#include <test/catch.hpp>
#include <base/types.hpp>
#include <base/error.hpp>
#include <time/daycounters/one.hpp>

using namespace MathFin;

#define EPS 1.0E-12

TEST_CASE("OneDayCounter", "[daycounters]") {
  Period p[] = {
    Period(3, TimeUnit::Months),
    Period(6, TimeUnit::Months),
    Period(1, TimeUnit::Years)
  };

  Time expected[] = { 1.0, 1.0, 1.0 };
  Date first(1, Month::January, 2004);
  Date last(31, Month::December, 2004);

  // generate dates from [first, last]
  int days = last - first + 1;
  std::vector<Date> dates;
  for (auto i : boost::irange(0, days)) {
    dates.push_back(first + i);
  }

  REQUIRE(dates[dates.size() - 1] == last);

  DayCounter dayCounter = OneDayCounter();

  int n = sizeof(p) / sizeof(Period);
  for (Date& start : dates) {
    for (auto i : boost::irange(0, n)) {
      Date end = start + p[i];
      Time calcualted = dayCounter.yearFraction(start, end);
      REQUIRE(std::fabs(calcualted - expected[i]) <= EPS);
    }
  }
}


/*
 * GeneralR_TreeManager.hpp
 *
 *  Created on: Nov 14, 2016
 *      Author: zhang huai peng
 */

#ifndef GENERALRTREEMANAGER_HPP_
#define GENERALRTREEMANAGER_HPP_
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <vector>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
namespace sim_mob {

using R_Point = bg::model::point<float, 2, bg::cs::cartesian>;
using R_Box = bg::model::box<R_Point>;
template<typename T> using R_Value = std::pair<R_Box, const T*>;
template<typename T> using R_Tree = bgi::rtree<R_Value<T>, bgi::linear<16> >;

template<typename T>
class GeneralR_TreeManager {
public:
    GeneralR_TreeManager();
    virtual ~GeneralR_TreeManager();

    /**
     * Update all objects into r-tree.
     * @param objectsForR_Tree is a container including all objects into r-tree
     */
    void update(const std::set<T*> &objectsForR_Tree);

    /**
     * Return a collection of objects that are located in the axially-aligned rectangle.
     * @param lowerLeft The lower left corner of the axially-aligned search rectangle.
     * @param upperRight The upper right corner of the axially-aligned search rectangle.
     * @return a collection of objects
     * The caller is responsible to determine the "type" of each object in the returned array.
     */
    std::vector<T const *> objectsInBox(const R_Point &lowerLeft, const R_Point &upperRight) const;

    /**
     * Return nearest object
     * @param xLocation is x coordinate of central location
     * @param yLocation is y coordinate of central location
     * @return nearest object if existed. otherwise nullptr
     */
    const T* searchNearestObject(double xLocation, double yLocation) const;

private:
    /**Internal r-tree to store objects*/
    R_Tree<T>* rTree;
};

}
#define _CLASS_RTREE_GENERAL_FUNCTIONS
#include "GeneralR_TreeManager.cpp"
#endif /* GENERALRTREEMANAGER_HPP_ */


/*
 *
 * Copyright (c) Kresimir Fresl 2002
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * Author acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_1_HPP
#define BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_1_HPP

#include <cassert>

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/vector_traits.hpp>
#include <boost/numeric/bindings/atlas/cblas1_overloads.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_TYPE_CHECK
#  include <boost/type_traits/same_traits.hpp>
#  include <boost/static_assert.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace atlas {

    // x_i <- alpha for all i
    template <typename T, typename Vct>
    inline
    void set (T const& alpha, Vct& x) {
      detail::set (traits::vector_size (x), alpha,
                   traits::vector_storage (x), traits::vector_stride (x));
    }

    // y <- x
    template <typename VctX, typename VctY>
    inline
    void copy (VctX const& x, VctY& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      detail::copy (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    traits::vector_storage (y), traits::vector_stride (y));
    }

    // x <-> y
    template <typename VctX, typename VctY>
    inline
    void swap (VctX& x, VctY& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      detail::swap (traits::vector_size (x),
                    traits::vector_storage (x), traits::vector_stride (x),
                    traits::vector_storage (y), traits::vector_stride (y));
    }

    // x <- alpha * x
    template <typename T, typename Vct>
    inline
    void scal (T const& alpha, Vct& x) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_TYPE_CHECK
      typedef traits::vector_traits<Vct> vtraits;
      BOOST_STATIC_ASSERT(
       (boost::is_same<T, typename vtraits::value_type>::value
        ||
        boost::is_same<T,
          typename traits::type_traits<typename vtraits::value_type>::real_type
        >::value
        ));
#endif
      detail::scal (traits::vector_size (x), alpha,
                    traits::vector_storage (x), traits::vector_stride (x));
    }

    // y <- alpha * x + y
    template <typename T, typename VctX, typename VctY>
    inline
    void axpy (T const& alpha, VctX const& x, VctY& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      detail::axpy (traits::vector_size (x), alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
                    traits::vector_storage (y), traits::vector_stride (y));
    }

    // y <- x + y
    template <typename VctX, typename VctY>
    inline
    void xpy (VctX const& x, VctY& y) {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::vector_traits<VctX>::value_type val_t;
#else
      typedef typename VctX::value_type val_t;
#endif
      axpy ((val_t) 1, x, y);
    }

    // y <- alpha * x + beta * y
    template <typename T, typename VctX, typename VctY>
    inline
    void axpby (T const& alpha, VctX const& x,
                T const& beta, VctY& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      detail::axpby (traits::vector_size (x), alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::vector_storage (x),
#else
                     traits::vector_storage_const (x),
#endif
                     traits::vector_stride (x),
                     beta,
                     traits::vector_storage (y), traits::vector_stride (y));
    }

    ///////////////////////////////////////////

    // dot <- x^T * y
    // .. real & complex types
    template <typename VctX, typename VctY>
    inline
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    typename traits::vector_traits<VctX>::value_type
#else
    typename VctX::value_type
#endif
    dot (VctX const& x, VctY const& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      return detail::dot (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                          traits::vector_storage (x),
#else
                          traits::vector_storage_const (x),
#endif
                          traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                          traits::vector_storage (y),
#else
                          traits::vector_storage_const (y),
#endif
                          traits::vector_stride (y));
    }

    // dot <- x^T * y
    // .. float only -- with double accumulation
    template <typename VctX, typename VctY>
    inline
    double dsdot (VctX const& x, VctY const& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      return cblas_dsdot (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                          traits::vector_storage (x),
#else
                          traits::vector_storage_const (x),
#endif
                          traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                          traits::vector_storage (y),
#else
                          traits::vector_storage_const (y),
#endif
                          traits::vector_stride (y));
    }

    // apdot <- alpha + x^T * y
    // .. float only -- computation uses double precision
    template <typename VctX, typename VctY>
    inline
    float sdsdot (float const alpha, VctX const& x, VctY const& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      return cblas_sdsdot (traits::vector_size (x), alpha,
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                           traits::vector_storage (x),
#else
                           traits::vector_storage_const (x),
#endif
                           traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                           traits::vector_storage (y),
#else
                           traits::vector_storage_const (y),
#endif
                           traits::vector_stride (y));
    }

    // dotu <- x^T * y
    // .. complex types only
    // .. function
    template <typename VctX, typename VctY>
    inline
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    typename traits::vector_traits<VctX>::value_type
#else
    typename VctX::value_type
#endif
    dotu (VctX const& x, VctY const& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typename traits::vector_traits<VctX>::value_type val;
#else
      typename VctX::value_type val;
#endif
      detail::dotu (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    &val);
      return val;
    }
    // .. procedure
    template <typename VctX, typename VctY>
    inline
    void dotu (VctX const& x, VctY const& y,
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
               typename traits::vector_traits<VctX>::value_type& val
#else
               typename VctX::value_type& val
#endif
    ) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      detail::dotu (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    &val);
    }

    // dotc <- x^H * y
    // .. complex types only
    // .. function
    template <typename VctX, typename VctY>
    inline
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    typename traits::vector_traits<VctX>::value_type
#else
    typename VctX::value_type
#endif
    dotc (VctX const& x, VctY const& y) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typename traits::vector_traits<VctX>::value_type val;
#else
      typename VctX::value_type val;
#endif
      detail::dotc (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    &val);
      return val;
    }
    // .. procedure
    template <typename VctX, typename VctY>
    inline
    void dotc (VctX const& x, VctY const& y,
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
               typename traits::vector_traits<VctX>::value_type& val
#else
               typename VctX::value_type& val
#endif
    ) {
      assert (traits::vector_size (y) >= traits::vector_size (x));
      detail::dotc (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (x),
#else
                    traits::vector_storage_const (x),
#endif
                    traits::vector_stride (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                    traits::vector_storage (y),
#else
                    traits::vector_storage_const (y),
#endif
                    traits::vector_stride (y),
                    &val);
    }

    // nrm2 <- ||x||_2
    template <typename Vct>
    inline
    typename traits::type_traits<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typename traits::vector_traits<Vct>::value_type
#else
      typename Vct::value_type
#endif
    >::real_type
    nrm2 (Vct const& x) {
      return detail::nrm2 (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                           traits::vector_storage (x),
#else
                           traits::vector_storage_const (x),
#endif
                           traits::vector_stride (x));
    }

    // asum <- ||re (x)|| + ||im (x)||
    template <typename Vct>
    inline
    typename traits::type_traits<
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typename traits::vector_traits<Vct>::value_type
#else
      typename Vct::value_type
#endif
    >::real_type
    asum (Vct const& x) {
      return detail::asum (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                           traits::vector_storage (x),
#else
                           traits::vector_storage_const (x),
#endif
                           traits::vector_stride (x));
    }

    // iamax <- 1st i: max (|re (x_i)| + |im (x_i)|)
    template <typename Vct>
    inline
    CBLAS_INDEX iamax (Vct const& x) {
      return detail::iamax (traits::vector_size (x),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                            traits::vector_storage (x),
#else
                            traits::vector_storage_const (x),
#endif
                            traits::vector_stride (x));
    }

    // TO DO: plane rotations

  } // namespace atlas

}}}

#endif // BOOST_NUMERIC_BINDINGS_CBLAS_LEVEL_1_HPP


#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
}


#ifndef __compiler_rt_fp_128_h__
#define __compiler_rt_fp_128_h__

#include <limits.h>
#include <stdint.h>
#include "../softfloat/source/include/softfloat.h"
#ifdef _MSC_VER
#include <boost/multiprecision/cpp_int.hpp>
typedef boost::multiprecision::int128_t int128_t;
typedef boost::multiprecision::uint128_t uint128_t;
#endif

#ifdef _MSC_VER
#define REP_C (int128_t)
#else
#define REP_C (__int128)
#endif
#define significandBits 112
#ifdef _MSC_VER
#define typeWidth       (sizeof(int128_t)*CHAR_BIT)
#else
#define typeWidth       (sizeof(__int128)*CHAR_BIT)
#endif
#define exponentBits    (typeWidth - significandBits - 1)
#define maxExponent     ((1 << exponentBits) - 1)
#define exponentBias    (maxExponent >> 1)

#define implicitBit     (REP_C(1) << significandBits)
#define significandMask (implicitBit - 1U)
#define signBit         (REP_C(1) << (significandBits + exponentBits))
#define absMask         (signBit - 1U)
#define exponentMask    (absMask ^ significandMask)
#define oneRep          ((rep_t)exponentBias << significandBits)
#define infRep          exponentMask
#define quietBit        (implicitBit >> 1)
#define qnanRep         (exponentMask | quietBit)

#ifdef _MSC_VER
static __inline int128_t toRep(float128_t x) {
	union { float128_t f; int128_t i; } rep = { x };
    return rep.i;
}
#else
static __inline __int28 toRep(float128_t x) {
	const union { float128_t f; __int28 i; } rep = { .f = x };
	return rep.i;
}
#endif

#endif //__compiler_rt_fp_h__


#include <dlib/graph_utils.h>
#include <dlib/image_io.h>
#include "facerec.h"
#include "utils.h"

using namespace dlib;

std::vector<image_t> jitter_image(
    const image_t& img,
    int count
)
{
    // All this function does is make count copies of img, all slightly jittered by being
    // zoomed, rotated, and translated a little bit differently. They are also randomly
    // mirrored left to right.
    thread_local dlib::rand rnd;

    std::vector<image_t> crops;
    for (int i = 0; i < count; ++i)
        crops.push_back(jitter_image(img,rnd));

    return crops;
}

// Helper function to estimage the age
uint8_t get_estimated_age(matrix<float, 1, number_of_age_classes>& p, float& confidence)
{
	float estimated_age = (0.25f * p(0));
	confidence = p(0);

	for (uint16_t i = 1; i < number_of_age_classes; i++) {
		estimated_age += (static_cast<float>(i) * p(i));
		if (p(i) > confidence) confidence = p(i);
	}

	return std::lround(estimated_age);
}


// 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を表示させたい
 *
 *
 *
 */


//Link to Boost
 #define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
#include <boost/test/unit_test.hpp>
#include <boost/timer.hpp>

#include <vector>
#include <iostream>
#include <boost/math/special_functions/binomial.hpp>
#include "test.h"
#include "../CrossingEquations.h"
#include "../EquationSolver.h"
using namespace std;

BOOST_FIXTURE_TEST_SUITE(CrossingEquations_suite, SimpleTestFixture, * utf::label("CrossingEquations"))

BOOST_DATA_TEST_CASE(Constructor_test, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData cftData(cfg);
    int numberOfScalarsToBootstrap = randomint(1, min(cftData.MaxScalarId(), 5));
    CrossingEquations equations(&cftData, numberOfScalarsToBootstrap);

    int equationNumber = (int)boost::math::binomial_coefficient<double>(numberOfScalarsToBootstrap + 3, 4);
    if (numberOfScalarsToBootstrap >= 4) {
        equationNumber += (int)boost::math::binomial_coefficient<double>(numberOfScalarsToBootstrap, 4);
    }

    BOOST_TEST(equations.EquationNumber() == equationNumber);

    int parameterNumber = cftData.MaxPrimaryId() - 1;
    parameterNumber += (int)boost::math::binomial_coefficient<double>(numberOfScalarsToBootstrap + 2, 3);
    parameterNumber += (int)boost::math::binomial_coefficient<double>(numberOfScalarsToBootstrap + 1, 2) * (cftData.MaxPrimaryId() - 1 -numberOfScalarsToBootstrap);

    BOOST_TEST(equations.ParameterNumber() == parameterNumber);
}

BOOST_DATA_TEST_CASE(GetSetParameter_test, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData cftData(cfg);
    int numberOfScalarsToBootstrap = randomint(1, min(cftData.MaxScalarId(), 5));
    CrossingEquations equations(&cftData, numberOfScalarsToBootstrap);

    for (uint i = 0; i < equations.ParameterNumber(); i++) {
        float_type value = random(-10.0, 10.0);
        equations.SetParameter(i, value);
        float_type actual = equations.GetParameter(i);
        BOOST_TEST_INFO("i=" << i);
        MY_FLOAT_EQUAL(actual, value, tol);
    }
}

BOOST_DATA_TEST_CASE(EquationDerivativeByParameter_test, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData cftData(cfg);
    int numberOfScalarsToBootstrap = randomint(1, min(cftData.MaxScalarId(), 4));
    CrossingEquations equations(&cftData, numberOfScalarsToBootstrap);
    int equationId = randomint(0, equations.EquationNumber() - 1);
    int parameterId = randomint(0, equations.ParameterNumber() - 1);

    cpx_t input = RandomComplex(0.2) + .5;
    float_type actual = equations.EquationDerivativeByParameter(input, equationId, parameterId);

    float_type param = equations.GetParameter(parameterId);
    equations.SetParameter(parameterId, param - inc);
    equations.OnParameterUpdated();
    float_type value1 = equations.EvaluateEquation(input, equationId);
    equations.SetParameter(parameterId, param + inc);
    equations.OnParameterUpdated();
    float_type value2 = equations.EvaluateEquation(input, equationId);

    float_type expected = (value2 - value1) / (2 * inc);

    BOOST_TEST_INFO("numberOfScalarsToBootstrap=" << numberOfScalarsToBootstrap << ", input=" << input << ", equationId=" << equationId << ", parameterId=" << parameterId);
    MY_FLOAT_EQUAL(actual, expected, tol);
}

BOOST_DATA_TEST_CASE(ConstraintsDerivativeByParameter_test, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData cftData(cfg);
    int numberOfScalarsToBootstrap = randomint(1, min(cftData.MaxScalarId(), 4));
    CrossingEquations equations(&cftData, numberOfScalarsToBootstrap);
    int parameterId = randomint(0, equations.ParameterNumber() - 2);
    float_type param = equations.GetParameter(parameterId);
    float_type param2 = equations.GetParameter(parameterId + 1);
    equations.SetParameter(parameterId, param2);
    equations.SetParameter(parameterId + 1, param);

    float_type actual = equations.ConstraintsDerivativeByParameter(parameterId);

    equations.SetParameter(parameterId, param2 - inc);
    float_type value1 = equations.EvaluateConstraints();
    equations.SetParameter(parameterId, param2 + inc);
    float_type value2 = equations.EvaluateConstraints();

    float_type expected = (value2 - value1) / (2 * inc);

    BOOST_TEST_INFO("numberOfScalarsToBootstrap=" << numberOfScalarsToBootstrap << ", parameterId=" << parameterId);
    MY_FLOAT_EQUAL(actual, expected, tol);
}

BOOST_DATA_TEST_CASE(ConstraintsDerivativeByParameter_test2, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData cftData(cfg);
    int numberOfScalarsToBootstrap = randomint(1, min(cftData.MaxScalarId(), 4));
    CrossingEquations equations(&cftData, numberOfScalarsToBootstrap);
    int parameterId = 0;
    equations.SetParameter(parameterId, (cftData.D/2.0 - 1 - 0.3));
    float_type actual = equations.ConstraintsDerivativeByParameter(parameterId);

    float_type param = equations.GetParameter(parameterId);
    equations.SetParameter(parameterId, param - inc);
    float_type value1 = equations.EvaluateConstraints();
    equations.SetParameter(parameterId, param + inc);
    float_type value2 = equations.EvaluateConstraints();

    float_type expected = (value2 - value1) / (2 * inc);

    BOOST_TEST_INFO("numberOfScalarsToBootstrap=" << numberOfScalarsToBootstrap << ", parameterId=" << parameterId << ", value=" << param);
    MY_FLOAT_EQUAL(actual, expected, tol);
}

// test suite end
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_CASE(EquationDerivativeByParameter_Debug_test, * utf::label("CrossingEquations.debug"))
{
    CftConfig cfg(2);
    cfg.OperatorNumbers=vector<int>({5, 4, 4, 5, 4, 3});
    CftData cftData(cfg);
    int numberOfScalarsToBootstrap = 3;
    CrossingEquations equations(&cftData, numberOfScalarsToBootstrap);
    int equationId = 12;
    int parameterId = 2;

    cpx_t input = cpx_t(0.499168,0.00225563);
    float_type actual = equations.EquationDerivativeByParameter(input, equationId, parameterId);
    float_type param = equations.GetParameter(parameterId);
    equations.SetParameter(parameterId, param - inc);
    equations.OnParameterUpdated();
    float_type value1 = equations.EvaluateEquation(input, equationId);
    equations.SetParameter(parameterId, param + inc);
    equations.OnParameterUpdated();
    float_type value2 = equations.EvaluateEquation(input, equationId);

    float_type expected = (value2 - value1) / (2 * inc);

    BOOST_TEST_INFO("numberOfScalarsToBootstrap=" << numberOfScalarsToBootstrap << ", input=" << input << ", equationId=" << equationId << ", parameterId=" << parameterId);
    MY_FLOAT_EQUAL(actual, expected, tol);
}


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

#include <boost/math/special_functions/hypergeometric_2f0.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <map>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

struct hypergeometric_2f0_gen
{
   mp_t operator()(mp_t a1, mp_t a2, mp_t z)
   {
      std::cout << a1 << " " << a2 << " " << z << std::endl;
      mp_t result = boost::math::detail::hypergeometric_2f0_generic_series(a1, a2, z, boost::math::policies::policy<>());
      std::cout << a1 << " " << a2 << " " << z << " " << result << std::endl;
      return result;
   }
};

struct hypergeometric_2f0_gen_spec1
{
   boost::math::tuple<mp_t, mp_t, mp_t, mp_t> operator()(mp_t a1, mp_t z)
   {
      mp_t result = boost::math::detail::hypergeometric_2f0_generic_series(a1, a1 + 0.5, z, boost::math::policies::policy<>());
      std::cout << a1 << " " << a1 + 0.5 << " " << z << " " << result << std::endl;
      return boost::math::make_tuple(a1, a1 + 0.5, z, result);
   }
};

int main(int, char* [])
{
   parameter_info<mp_t> arg1, arg2, arg3;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for 2F0:\n";

   std::string line;
   bool cont;

#if 1
   arg1 = make_periodic_param(mp_t(-20), mp_t(-1), 19);
   arg2 = make_random_param(mp_t(-5), mp_t(5), 8);
   arg1.type |= dummy_param;
   arg2.type |= dummy_param;
   data.insert(hypergeometric_2f0_gen_spec1(), arg1, arg2);

#else

   do {
      get_user_parameter_info(arg1, "a1");
      get_user_parameter_info(arg2, "a2");
      get_user_parameter_info(arg3, "z");
      data.insert(hypergeometric_2f0_gen(), arg1, arg2, arg3);
      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   } while (cont);

#endif
   std::cout << "Enter name of test data file [default=hypergeometric_2f0.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "hypergeometric_2f0.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, line.c_str());

   return 0;
}


#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;
}


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

#include "aslam/calibration/time-delay/error-terms/ErrorTermPose.h"

#include <Eigen/Dense>

#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/EulerAnglesYawPitchRoll.hpp>

namespace aslam {
  namespace calibration {

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

    ErrorTermPose::ErrorTermPose(const aslam::backend::TransformationExpression&
        T, const Input& Tm, const Covariance& sigma2) :
        _T(T),
        _Tm(Tm),
        _sigma2(sigma2) {
      setInvR(_sigma2.inverse());
      aslam::backend::DesignVariable::set_t dv;
      _T.getDesignVariables(dv);
      setDesignVariablesIterator(dv.begin(), dv.end());
    }

    ErrorTermPose::ErrorTermPose(const ErrorTermPose& other) :
        ErrorTermFs<6>(other),
        _T(other._T),
        _Tm(other._Tm),
        _sigma2(other._sigma2) {
    }

    ErrorTermPose& ErrorTermPose::operator =
        (const ErrorTermPose& other) {
      if (this != &other) {
        ErrorTermFs<6>::operator=(other);
       _T = other._T;
       _Tm = other._Tm;
       _sigma2 = other._sigma2;
      }
      return *this;
    }

    ErrorTermPose::~ErrorTermPose() {
    }

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

    const ErrorTermPose::Input& ErrorTermPose::getInput() const {
      return _Tm;
    }

    ErrorTermPose::Input& ErrorTermPose::getInput() {
      return _Tm;
    }

    void ErrorTermPose::setInput(const Input& Tm) {
      _Tm = Tm;
    }

    const ErrorTermPose::Covariance& ErrorTermPose::getCovariance() const {
      return _sigma2;
    }

    ErrorTermPose::Covariance& ErrorTermPose::getCovariance() {
      return _sigma2;
    }

    void ErrorTermPose::setCovariance(const Covariance& sigma2) {
      _sigma2 = sigma2;
    }

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

    double ErrorTermPose::evaluateErrorImplementation() {
      const Eigen::Matrix4d T = _T.toTransformationMatrix();
      Input e;
      e.head<3>() = T.topRightCorner<3, 1>();
      const sm::kinematics::EulerAnglesYawPitchRoll ypr;
      e.tail<3>() =
        ypr.rotationMatrixToParameters(T.topLeftCorner<3, 3>());
      error_t error = _Tm - e;
      error(3) = sm::kinematics::angleMod(error(3));
      error(4) = sm::kinematics::angleMod(error(4));
      error(5) = sm::kinematics::angleMod(error(5));
      setError(error);
      return evaluateChiSquaredError();
    }

    void ErrorTermPose::evaluateJacobiansImplementation(
        aslam::backend::JacobianContainer& jacobians) {
      Eigen::Matrix<double, 6, 6> J = Eigen::Matrix<double, 6, 6>::Identity();
      const Eigen::Matrix4d T = _T.toTransformationMatrix();
      J.topRightCorner<3, 3>() =
        sm::kinematics::crossMx(T.topRightCorner<3, 1>());
      const sm::kinematics::EulerAnglesYawPitchRoll ypr;
      J.bottomRightCorner<3, 3>() = (ypr.parametersToSMatrix(
        ypr.rotationMatrixToParameters(T.topLeftCorner<3, 3>()))).inverse();
      _T.evaluateJacobians(jacobians, -J);
    }

  }
}


#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);
}


// Copyright John Maddock 2006.

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

#include <boost/math/tools/test_data.hpp>
#include <boost/test/included/prg_exec_monitor.hpp>
#include <boost/math/special_functions/ellint_3.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>
#include "mp_t.hpp"

float extern_val;
// confuse the compilers optimiser, and force a truncation to float precision:
float truncate_to_float(float const * pf)
{
   extern_val = *pf;
   return *pf;
}

boost::math::tuple<mp_t, mp_t> generate_data(mp_t n, mp_t phi)
{
   static boost::mt19937 r;
   boost::uniform_real<float> ui(0, 1);
   float k = ui(r);
   mp_t kr(truncate_to_float(&k));
   mp_t result = boost::math::ellint_3(kr, n, phi);
   return boost::math::make_tuple(kr, result);
}

int cpp_main(int argc, char*argv [])
{
   using namespace boost::math::tools;

   parameter_info<mp_t> arg1, arg2;
   test_data<mp_t> data;

   bool cont;
   std::string line;

   if(argc < 1)
      return 1;

   do{
      if(0 == get_user_parameter_info(arg1, "n"))
         return 1;
      if(0 == get_user_parameter_info(arg2, "phi"))
         return 1;

      data.insert(&generate_data, arg1, arg2);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=ellint_pi3_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "ellint_pi3_data.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, line.c_str());

   return 0;
}