/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <cmath>
using std::log;
using std::sqrt;
using std::pow;

#include <fstream>
using std::ofstream;

#include <streambuf>
using std::streambuf;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Mat;
using arma::Row;
using arma::uword;
using arma::abs;
using arma::eps;
using arma::exp;
using arma::exp2;
using arma::exp10;
using arma::trunc_exp;
using arma::log;
using arma::log2;
using arma::log10;
using arma::trunc_log;
using arma::sqrt;
using arma::square;
using arma::floor;
using arma::ceil;
using arma::round;
using arma::sign;
using arma::sin;
using arma::asin;
using arma::sinh;
using arma::asinh;
using arma::cos;
using arma::acos;
using arma::cosh;
using arma::acosh;
using arma::tan;
using arma::atan;
using arma::tanh;
using arma::atanh;
using arma::cumsum;
using arma::hist;
using arma::sort;
using arma::sort_index;
using arma::stable_sort_index;
using arma::trans;
using arma::unique;
using arma::toeplitz;
using arma::circ_toeplitz;
using arma::accu;
using arma::min;
using arma::max;
using arma::prod;
using arma::sum;
using arma::mean;
using arma::median;
using arma::stddev;
using arma::var;
using arma::cor;
using arma::cov;
using arma::diagmat;
using arma::is_finite;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedGenRowVec : public Expected {
    public:
      ExpectedGenRowVec() {
        cout << "Compute ExpectedGenRowVec(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
          InputClass::GenRowVec
        });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genRowVec = *static_cast<Row<double>*>(value.second);
                break;
            }
            ++n;
          }

          cout << "Using input: " << _fileSuffix << endl;

          expectedArmaAbs();
          expectedArmaEps();
          expectedArmaExp();
          expectedArmaExp2();
          expectedArmaExp10();
          expectedArmaTrunc_exp();
          expectedArmaLog();
          expectedArmaLog2();
          expectedArmaLog10();
          expectedArmaTrunc_log();
          expectedArmaSqrt();
          expectedArmaSquare();
          expectedArmaFloor();
          expectedArmaCeil();
          expectedArmaRound();
          expectedArmaSign();
          expectedArmaSin();
          expectedArmaAsin();
          expectedArmaSinh();
          expectedArmaAsinh();
          expectedArmaCos();
          expectedArmaAcos();
          expectedArmaCosh();
          expectedArmaAcosh();
          expectedArmaTan();
          expectedArmaAtan();
          expectedArmaTanh();
          expectedArmaAtanh();
          expectedArmaCumsum();
          expectedArmaHist();
          expectedArmaSort();
          expectedArmaSort_index();
          expectedArmaStable_sort_index();
          expectedArmaTrans();
          expectedArmaUnique();
          expectedArmaNegate();
          expectedArmaReciprocal();
          expectedArmaToeplitz();
          expectedArmaCirc_toeplitz();
          expectedArmaAccu();
          expectedArmaMin();
          expectedArmaMax();
          expectedArmaProd();
          expectedArmaSum();
          expectedArmaMean();
          expectedArmaMedian();
          expectedArmaStddev();
          expectedArmaVar();
          expectedArmaCor();
          expectedArmaCov();
          expectedArmaDiagmat();
          expectedArmaIs_finite();
          expectedMat();
          expectedRowVecSize();
          expectedRowVecT();
   		  expectedRowVecPrint();
          expectedRowVecRaw_print();
   		  expectedRowIs_finite();
       	  expectedRowMinA();
          expectedRowMaxA();
   		  expectedRowMinB();
   		  expectedRowMaxB();
   		  expectedRowIs_empty();
        }

        cout << "done." << endl;
      }

    protected:
      Row<double> _genRowVec;

      void expectedArmaAbs() {
        cout << "- Compute expectedArmaAbs() ... ";
        save<double>("Arma.abs", abs(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaEps() {
        cout << "- Compute expectedArmaAbs() ... ";
        save<double>("Arma.eps", eps(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaExp() {
        cout << "- Compute expectedArmaExp() ... ";
        save<double>("Arma.exp", exp(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaExp2() {
        cout << "- Compute expectedArmaExp2() ... ";
        save<double>("Arma.exp2", exp2(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaExp10() {
        cout << "- Compute expectedArmaExp10() ... ";
        save<double>("Arma.exp10", exp10(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaTrunc_exp() {
        cout << "- Compute expectedArmaTrunc_exp() ... ";
        save<double>("Arma.trunc_exp", trunc_exp(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaLog() {
        cout << "- Compute expectedArmaLog() ... ";
        save<double>("Arma.log", log(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaLog2() {
        cout << "- Compute expectedArmaLog2() ... ";
        save<double>("Arma.log2", log2(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaLog10() {
        cout << "- Compute expectedArmaLog10() ... ";
        save<double>("Arma.log10", log10(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaTrunc_log() {
        cout << "- Compute expectedArmaTrunc_log() ... ";
        save<double>("Arma.trunc_log", trunc_log(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaSqrt() {
        cout << "- Compute expectedArmaSqrt() ... ";
        save<double>("Arma.sqrt", sqrt(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaSquare() {
        cout << "- Compute expectedArmaSquare() ... ";
        save<double>("Arma.square", square(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaFloor() {
        cout << "- Compute expectedArmaFloor() ... ";
        save<double>("Arma.floor", floor(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaCeil() {
        cout << "- Compute expectedArmaCeil() ... ";
        save<double>("Arma.ceil", ceil(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaRound() {
        cout << "- Compute expectedArmaRound() ... ";
        save<double>("Arma.round", round(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaSign() {
        cout << "- Compute expectedArmaSign() ... ";
        save<double>("Arma.sign", sign(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaSin() {
        cout << "- Compute expectedArmaSin() ... ";
        save<double>("Arma.sin", sin(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaAsin() {
        cout << "- Compute expectedArmaAsin() ... ";
        save<double>("Arma.asin", asin(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaSinh() {
        cout << "- Compute expectedArmaSinh() ... ";
        save<double>("Arma.sinh", sinh(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaAsinh() {
        cout << "- Compute expectedArmaAsinh() ... ";
        save<double>("Arma.asinh", asinh(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaCos() {
        cout << "- Compute expectedArmaCos() ... ";
        save<double>("Arma.cos", cos(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaAcos() {
        cout << "- Compute expectedArmaAcos() ... ";
        save<double>("Arma.acos", acos(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaCosh() {
        cout << "- Compute expectedArmaCosh() ... ";
        save<double>("Arma.cosh", cosh(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaAcosh() {
        cout << "- Compute expectedArmaAcosh() ... ";

        /*
         * acosh behaves buggy on some systems, with acosh(inf) = nan instead of inf
         */
        //save<double>("Arma.acosh", acosh(_genRowVec));

        Mat<double> expected = _genRowVec;
        expected.transform([](double value) {
          return log(value + sqrt(pow(value, 2) - 1));
        });
        save<double>("Arma.acosh", expected);

        cout << "done." << endl;
      }

      void expectedArmaTan() {
        cout << "- Compute expectedArmaTan() ... ";
        save<double>("Arma.tan", tan(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaAtan() {
        cout << "- Compute expectedArmaAtan() ... ";
        save<double>("Arma.atan", atan(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaTanh() {
        cout << "- Compute expectedArmaTanh() ... ";
        save<double>("Arma.tanh", tanh(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaAtanh() {
        cout << "- Compute expectedArmaAtanh() ... ";
        save<double>("Arma.atanh", atanh(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaCumsum() {
        cout << "- Compute expectedArmaCumsum() ... ";
        save<double>("Arma.cumsum", cumsum(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaHist() {
        cout << "- Compute expectedArmaHist() ... ";
        save<uword>("Arma.hist", hist(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaSort() {
        if(!_genRowVec.is_finite()) {
          return;
        }

        cout << "- Compute expectedArmaSort() ... ";
        save<double>("Arma.sort", sort(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaSort_index() {
        if(!_genRowVec.is_finite()) {
          return;
        }

        cout << "- Compute expectedArmaSort_index() ... ";
        save<uword>("Arma.sort_index", sort_index(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaStable_sort_index() {
        if(!_genRowVec.is_finite()) {
          return;
        }

        cout << "- Compute expectedArmaStable_sort_index() ... ";
        save<uword>("Arma.stable_sort_index", stable_sort_index(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaTrans() {
        cout << "- Compute expectedArmaTrans() ... ";
        save<double>("Arma.trans", trans(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaUnique() {
        cout << "- Compute expectedArmaUnique() ... ";
        save<double>("Arma.unique", unique(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaNegate() {
        cout << "- Compute expectedArmaNegate() ... ";
        save<double>("Arma.negate", -_genRowVec);
        cout << "done." << endl;
      }

      void expectedArmaReciprocal() {
        cout << "- Compute expectedArmaReciprocal() ... ";
        save<double>("Arma.reciprocal", 1/_genRowVec);
        cout << "done." << endl;
      }

      void expectedArmaToeplitz() {
        cout << "- Compute expectedArmaToeplitz() ... ";
        save<double>("Arma.toeplitz", toeplitz(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaCirc_toeplitz() {
        cout << "- Compute expectedArmaCirc_toeplitz() ... ";
        save<double>("Arma.circ_toeplitz", circ_toeplitz(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaAccu() {
        cout << "- Compute expectedArmaAccu() ... ";
        save<double>("Arma.accu", Mat<double>({accu(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaMin() {
        cout << "- Compute expectedArmaMin() ... ";
        save<double>("Arma.min", Mat<double>({min(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaMax() {
        cout << "- Compute expectedArmaMax() ... ";
        save<double>("Arma.max", Mat<double>({max(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaProd() {
        cout << "- Compute expectedArmaProd() ... ";
        save<double>("Arma.prod", Mat<double>({prod(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaSum() {
        cout << "- Compute expectedArmaSum() ... ";
        save<double>("Arma.sum", Mat<double>({sum(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaMean() {
        cout << "- Compute expectedArmaMean() ... ";
        save<double>("Arma.mean", Mat<double>({mean(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaMedian() {
        cout << "- Compute expectedArmaMedian() ... ";
        save<double>("Arma.median", Mat<double>({median(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaStddev() {
        cout << "- Compute expectedArmaStddev() ... ";
        save<double>("Arma.stddev", Mat<double>({stddev(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaVar() {
        cout << "- Compute expectedArmaVar() ... ";
        save<double>("Arma.var", Mat<double>({var(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaCor() {
        cout << "- Compute expectedArmaCor() ... ";
        save<double>("Arma.cor", Mat<double>({cor(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaCov() {
        cout << "- Compute expectedArmaCov() ... ";
        save<double>("Arma.cov", Mat<double>({cov(_genRowVec)}));
        cout << "done." << endl;
      }

      void expectedArmaDiagmat() {
        cout << "- Compute expectedArmaDiagmat() ... ";
        save<double>("Arma.diagmat", diagmat(_genRowVec));
        cout << "done." << endl;
      }

      void expectedArmaIs_finite() {
        cout << "- Compute expectedArmaIs_finite() ... ";

        if(is_finite(_genRowVec)) {
          save<double>("Arma.is_finite", Mat<double>({1.0}));
        } else {
          save<double>("Arma.is_finite", Mat<double>({0.0}));
        }

        cout << "done." << endl;
      }

      void expectedMat() {
        cout << "- Compute expectedMat() ... ";
        save<double>("Mat", _genRowVec);
        cout << "done." << endl;
      }

	  void expectedRowVecSize() {
		  cout << "- Compute expectedRowVecSize() ... ";
		  save<double>("Row.size", Mat<double>({static_cast<double>(_genRowVec.size())}));
		  cout << "done." << endl;
      }

	  void expectedRowVecT() {
		  cout << "- Compute expectedRowVecT() ... ";
		  save<double>("Row.t", _genRowVec.t());
		  cout << "done." << endl;
      }

	  void expectedRowVecPrint() {
		  cout << "- Compute expectedRowVecPrint() ... ";

		  ofstream expected(_filepath + "Row.print(" + _fileSuffix + ").txt");
		  streambuf* previousBuffer = cout.rdbuf(expected.rdbuf());

		  _genRowVec.print();

		  cout.rdbuf(previousBuffer);

		  cout << "done." << endl;
      }

	  void expectedRowVecRaw_print() {
		  cout << "- Compute expectedRowVecRaw_print() ... ";

		  ofstream expected(_filepath + "Row.raw_print(" + _fileSuffix + ").txt");
		  streambuf* previousBuffer = cout.rdbuf(expected.rdbuf());

		  _genRowVec.raw_print();

		  cout.rdbuf(previousBuffer);

		  cout << "done." << endl;
      }

      void expectedRowIs_finite() {
        cout << "- Compute expectedRowIs_finite() ... ";

        if(_genRowVec.is_finite()) {
          save<double>("Row.is_finite", Row<double>({1}));
        } else {
          save<double>("Row.is_finite", Row<double>({0}));
        }

        cout << "done." << endl;
      }

      void expectedRowMinA() {
        cout << "- Compute expectedRowMinA() ... ";
        double value;
        value = _genRowVec.min();
        save<double>("Row.minA", Row<double>({value}));

        cout << "done." << endl;
      }

      void expectedRowMaxA() {
        cout << "- Compute expectedRowMaxA() ... ";
        double value;
        value = _genRowVec.max();
        save<double>("Row.maxA", Row<double>({value}));
        cout << "done." << endl;
      }

      void expectedRowMinB() {
        cout << "- Compute expectedRowMinB() ... ";
        uword value;
        _genRowVec.min(value);
        save<double>("Row.minB", Row<double>({static_cast<double>(value)}));
        cout << "done." << endl;
      }

      void expectedRowMaxB() {
        cout << "- Compute expectedRowMaxB() ... ";
        uword value;
        _genRowVec.max(value);
        save<double>("Row.maxB", Row<double>({static_cast<double>(value)}));
        cout << "done." << endl;
      }

      void expectedRowIs_empty() {
        cout << "- Compute expectedRowIs_empty() ... ";

        if(_genRowVec.is_empty()) {
          save<double>("Row.is_empty", Row<double>({1}));
        } else {
          save<double>("Row.is_empty", Row<double>({0}));
        }

        cout << "done." << endl;
      }

  };
}


#pragma once
#ifndef __QTM_CORE_HPP__
#define __QTM_CORE_HPP__

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <vector>

#include <boost/core/noncopyable.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace qtm {

/**
 * Clamping the given value in given ranges
 *
 * @tparam type Type of the clamping value
 * @param[in] num Clamping value
 * @param[in] min_value Lower clamping limit
 * @param[in] max_value Upper clamping limit
 */
template <typename type> type clamp(type num, type min_value, type max_value) {
  return std::max(std::min(num, max_value), min_value);
}

/**
 * Core queuing class
 *
 * ...
 *
 * Implements basic construction and solving final states;
 * also available getters/setters of internal characteristics
 */
class qtm final : private boost::noncopyable {
private:
  std::uint64_t channel_count_, queue_size_;
  double la_, mu_, nu_;
  std::int64_t n_;
  boost::numeric::ublas::vector<double> final_states_;

  bool is_fs_outdated_;

  /**
   * Perform auxiliary matrix initialization for future calculation
   */
  static boost::numeric::ublas::matrix<double>
  matrix_init(std::uint64_t channel_count, std::uint64_t queue_size, double la,
              double mu, double nu, std::int64_t n);

public:
  /**
   * Class construnctor
   * Permorm construnction of the intance of the queuing system
   *
   * @param[in] channel_count Channel count of the queuing system
   * @param[in] queue_size Queue size of the queuing system
   * @param[in] la Input flow rate of the queuing system
   * @param[in] mu Output flow rate  of the queuing system
   * @param[in] nu Impatience rate of the queuing system
   * @param[in] n Load sources of the queuing system
   */
  explicit qtm(std::uint64_t channel_count, std::uint64_t queue_size, double la,
               double mu, double nu = 0, std::int64_t n = -1);
  ~qtm(void) = default;

  /**
   * Get the current value of the channel count of the queuing system
   */
  std::uint64_t channel_count(void) const;
  /**
   * Get the current value of the queue size of the queuing system
   */
  std::uint64_t queue_size(void) const;
  /**
   * Get the current value of the load sources of the queuing system
   */
  std::int64_t n(void) const;
  /**
   * Get the current value of the input flow rate of the queuing system
   */
  double la(void) const;
  /**
   * Get the current value of the output flow rate of the queuing system
   */
  double mu(void) const;
  /**
   * Get the current value of the impatience rate of the queuing system
   */
  double nu(void) const;

  /**
   * Set the value of the channel count of the queuing system
   *
   * @param[in] channel_count_ New value of channel count
   */
  void channel_count(std::uint64_t channel_count_);
  /**
   * Set the value of the queue size of the queuing system
   *
   * @param[in] channel_count_ New value of queue size
   */
  void queue_size(std::uint64_t queue_size_);
  /**
   * Set the value of the load sources of the queuing system
   *
   * @param[in] channel_count_ New value of load sources
   */
  void n(std::int64_t n_);
  /**
   * Get the current value of the input flow rate of the queuing system
   *
   * @param[in] channel_count_ New value of input flow rate
   */
  void la(double la_);
  /**
   * Set the value of the output flow rate of the queuing system
   *
   * @param[in] channel_count_ New value of output flow rate
   */
  void mu(double mu_);
  /**
   * Set the value of the impatience rate of the queuing system
   *
   * @param[in] channel_count_ New value of impatience rate
   */
  void nu(double nu_);

  /**
   * Get the current value of the final states of the queuing system
   *
   * @throws std::runtime_error Thrown if final states calculation was not
   * performed
   */
  std::vector<double> const final_states(void) const;
  /**
   * Get information that the final states hasn't been updated after changes
   * to the internal characteristics of the queuing system.
   */
  bool is_fs_outdated(void) const;
  /**
   * Perform calculation he final states of the queuing system.
   */
  std::vector<double> calc_final_states(void);
};

}; // namespace qtm

#endif // !__QTM_CORE_HPP__


#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "control.hpp"

using namespace cv;
using namespace std;
using namespace sim;

#include <Eigen/QR>

/* Set value based on control system performance */
// const double MIN_BALL_RADIUS_PX = 14;
const double MIN_CIRCLE_ECCENTRICITY = 0.01;
const uint32_t MIN_TRAJECTORY_POINTS = 10;

static void polyfit(const vector<CircleMatch> &pnts,
                    vector<double> &coeff,
                    int order)
{
    vector<double> yv(pnts.size());

    for (size_t i = 0; i < pnts.size(); i++)
    {
        yv[i] = pnts[i].pos.y;
    }

    Eigen::MatrixXd A(pnts.size(), order + 1);
    Eigen::VectorXd yv_mapped = Eigen::VectorXd::Map(&yv.front(), yv.size());
    Eigen::VectorXd result;

    assert(pnts.size() >= order + 1);

    for (size_t i = 0; i < pnts.size(); i++)
    {
        for (size_t j = 0; j < order + 1; j++)
        {
            A(i, j) = pow(pnts[i].pos.x, j);
        }
    }

    result = A.householderQr().solve(yv_mapped);

    coeff.resize(order + 1);
    for (size_t i = 0; i < order + 1; i++)
    {
        coeff[i] = result[i];
    }
}

static double getContourEccentricity(const Moments &mu)
{
    double m20s02 = mu.m20 - mu.m02;
    double m20p02 = mu.m20 + mu.m02;

    double bigSqrt = sqrt(m20s02 * m20s02 + 4 * mu.m11 * mu.m11);
    return (m20p02 + bigSqrt) / (m20p02 - bigSqrt);
}

PlaneControl::PlaneControl(bool debugRender) :
    m_isDebugRenderEnabled(debugRender)
{
}

double PlaneControl::getAveragePredictedRadiusPx()
{
    if ( m_ballPoints.size() == 0 )
        return 0;

    double result = 0;

    for ( const CircleMatch &mtch : m_ballPoints )
    {
        result += mtch.radius;
    }

    result /= m_ballPoints.size();

    return result;
}

void PlaneControl::resetPredictions()
{
    m_ballPoints.clear();
}

int PlaneControl::getPlanePositionPrediction(Mat &ballFrame, int planeDistPx)
{
    CircleContour contours;
    vector<CircleMatch> circles;
    int resultControlY = ballFrame.size().height/2;

    getCircleContours(ballFrame, contours);

    for (const vector<Point> &contour : contours)
    {
        CircleMatch match;

        minEnclosingCircle(contour,
                           match.pos,
                           match.radius);

        // if (match.radius < MIN_BALL_RADIUS_PX)
            // continue;

        circles.push_back(match);
    }

    if (circles.size() > 0)
    {
        m_ballPoints.push_back(circles[0]);
    }

    /* Approximate previous points to predict future trajectory */
    if (m_ballPoints.size() > MIN_TRAJECTORY_POINTS)
    {
        vector<double> coeffs;
        polyfit(m_ballPoints, coeffs, 2);

        assert(coeffs.size() == 3);

        if (m_isDebugRenderEnabled)
        {
            for (int x = 0; x < ballFrame.size().width; x++)
            {
                int y = coeffs[0] + x * coeffs[1] + x * x * coeffs[2];

                circle(ballFrame, Point(x, y), 1,
                       Scalar(0, 0, 128), FILLED);
            }
        }

        resultControlY = coeffs[0] +
                         planeDistPx * coeffs[1] +
                         planeDistPx * planeDistPx * coeffs[2];
    }

    /* Render detected centers */
    if (m_isDebugRenderEnabled)
    {
        for (auto pnt_mtc : m_ballPoints)
        {
            circle(ballFrame,
                   pnt_mtc.pos,
                   2,
                   Scalar(255, 0, 128),
                   FILLED);
        }
    }

    return resultControlY;
}

void PlaneControl::getRedFilteredFrame(const Mat &frame, Mat &binResultMask)
{
    /* In real situation GaussianBlur() is required */
    Mat hsv;
    cvtColor(frame, hsv, COLOR_BGR2HSV);

    Mat lowerRedHueRange;
    Mat upperRedHueRange;
    inRange(hsv, cv::Scalar(0, 100, 100),
            cv::Scalar(10, 255, 255),
            lowerRedHueRange);
    inRange(hsv, cv::Scalar(160, 100, 100),
            cv::Scalar(179, 255, 255),
            upperRedHueRange);

    addWeighted(lowerRedHueRange, 1.0,
                upperRedHueRange, 1.0,
                0.0, binResultMask);
}

int PlaneControl::getCircleContours(Mat &frame, CircleContour &outContours)
{
    Mat redFiltered;
    getRedFilteredFrame(frame, redFiltered);

    CircleContour contours;
    findContours(redFiltered, contours,
                 RETR_TREE, CHAIN_APPROX_SIMPLE);

    if (contours.size() > 0)
    {
        for (int i = 0; i < contours.size(); i++)
        {
            /* Simplified eccentricity estimation */

            /* Requirements for ellipse */
            if ( contours[i].size() > 5 )
            {
                RotatedRect result = fitEllipse( contours[i] );

                if ( abs(1.0 - result.size.width / result.size.height) < MIN_CIRCLE_ECCENTRICITY )
                    outContours.push_back(contours[i]);
            }

            if (m_isDebugRenderEnabled)
            {
                drawContours(frame, contours, i, Scalar(255, 0, 0), 3);
            }
        }
    }
}


#include "../../Headers/Edmonton.hpp"
#include <algorithm>
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using namespace std;
using boost::multiprecision::cpp_int;

/*
Since the digit count for the numbers scale at an insane rate, we can safely assume that A and B are >= 90 and < 100. Rest can be bruteforced.
*/

int main(int argc, char *argv[]) {
	int max_digitial_sum = 0;
	for(int a = 90; a < 100; a++) {
		for(int b = 90; b < 100; b++) {
			max_digitial_sum = max(max_digitial_sum, Edmonton::sumOfDigits<int, cpp_int>(boost::multiprecision::pow((cpp_int)a, b)));
		}
	}
	cout << max_digitial_sum << endl;
}


#ifndef _PLAN_CONTAINER_MID_H_
#define _PLAN_CONTAINER_MID_H_

#include <Eigen/Eigen>
#include <vector>

#include <ros/ros.h>

#include <arena_traj_planner/uniform_bspline.h>
#include <arena_traj_planner/polynomial_traj.h>

using std::vector;

struct LandmarkPoint{
  bool is_visited;
  int id;
  Eigen::Vector2d pos;

  LandmarkPoint(Eigen::Vector2d pos,int id){
    this->pos=pos;
    this->id=id;
    this->is_visited=false;
  }

  ~LandmarkPoint(){};
  void setVisited(bool is_visited){
    this->is_visited=is_visited;
  }
};

class GlobalData{
private:
  double dist_next_wp_;
  double dist_tolerance_;
  double rou_thresh_;
  int id_last_landmark_;

  void resetLandmarks(){
    /* init */
    global_path_.clear();
    landmark_points_.clear();
    double rou_t, rou_last=-1;
    double gradient_t,gradient_last=1000;
    Eigen::Vector2d last_landmark=start_pos_;
    int id=0;
    bool flag_landmark_search_done=false;

    int continue_descent_counter=0;

    /* search landmark pt according to curvature radius */
    for (double t = tm_start_; t <= tm_end_; t += 0.01) {
      // get pos
      Eigen::Vector2d pos_t = getPosition(t);
      global_path_.push_back(pos_t);
      if(flag_landmark_search_done){
        continue;
      }
      // calculate curvature radius
      rou_t=calculateCurveRadius(getVelocity(t),getAcceleration(t));
      //std::cout<<"********************* rou="<<rou_t<<std::endl;
      // calculate gradient
      gradient_t=rou_t-rou_last;

      // set continue_descent_counter
      if(gradient_t<-5.0){
        continue_descent_counter++;
      }else{
        continue_descent_counter=0;
      }

      if(std::abs(gradient_t)<0.2){
        gradient_t=gradient_last;
        rou_t=rou_last;
      }

      /* minimum vaule of the curvature radius || curvature radius is so small */
      if((gradient_t>0 && gradient_last<0)|| rou_t<rou_thresh_|| continue_descent_counter>80){ //  //gradient_t*gradient_last<0
        //std::cout<<"in1************ rou="<<rou_t<<std::endl;
        // negelect the start points
        if((pos_t-start_pos_).squaredNorm()<=dist_next_wp_ && rou_t>rou_thresh_){
          rou_last=rou_t;
          gradient_last=gradient_t;
          continue;
        }
        // reset continue_descent_counter

        if(continue_descent_counter>80){
          //std::cout<<"in counter************ rou="<<rou_t<<std::endl;
        }

        //std::cout<<"in2************ rou="<<rou_t<<std::endl;
        // make sure two landmarks are not too close
        if((pos_t-last_landmark).squaredNorm()>2.0)
        {

          landmark_points_.push_back(LandmarkPoint(pos_t,id));
          last_landmark=pos_t;
          id++;
          continue_descent_counter=0;
          //std::cout<<"add************ rou="<<rou_t<<std::endl;
        }

      }else if((pos_t-end_pos_).squaredNorm()<=dist_next_wp_){
        // add end point as last landmark
        landmark_points_.push_back(LandmarkPoint(end_pos_,id));
        flag_landmark_search_done=true;
      }
      rou_last=rou_t;
      gradient_last=gradient_t;
    }

    // add end_pos to global path
    global_path_.push_back(end_pos_);
    // reset landmark counter
    id_last_landmark_=0;
  }

  Eigen::Vector2d getPosition(double t){
      return global_pos_traj_.evaluateDeBoor(t);
  }

  Eigen::Vector2d getVelocity(double t){
      return global_vel_traj_.evaluateDeBoor(t);
  }

  Eigen::Vector2d getAcceleration(double t){
      return global_acc_traj_.evaluateDeBoor(t);
  }

  double calculateCurveRadius(Eigen::Vector2d vel,Eigen::Vector2d acc){
    double rou=std::pow(vel.squaredNorm(),3)/(vel(0)*acc(1)-vel(1)*acc(0));
    return std::abs(rou);
  }

public:
  UniformBspline global_pos_traj_, global_vel_traj_, global_acc_traj_;
  double tm_start_, tm_end_;
  Eigen::Vector2d start_pos_, end_pos_;

  std::vector<Eigen::Vector2d> global_path_;
  std::vector<LandmarkPoint> landmark_points_;

  GlobalData(){};
  ~GlobalData(){};

  void setGlobalDataParam(double dist_next_wp, double dist_tolerance, double rou_thresh){
    dist_next_wp_=dist_next_wp;
    dist_tolerance_=dist_tolerance;
    rou_thresh_=rou_thresh;
  }

  void resetGlobalData(const UniformBspline & traj){
    global_pos_traj_= traj;
    global_vel_traj_=global_pos_traj_.getDerivative();
    global_acc_traj_=global_vel_traj_.getDerivative();
    global_pos_traj_.getTimeSpan(tm_start_, tm_end_);
    start_pos_=getPosition(tm_start_);
    end_pos_=getPosition(tm_end_);
    resetLandmarks();
  }

  void getGlobalPath(std::vector<Eigen::Vector2d> &global_path){
     global_path=global_path_;
  }

  void getLandmarks(std::vector<Eigen::Vector2d> & landmark_pts){

     for(int i=0;i<landmark_points_.size();i++){
       landmark_pts.push_back(landmark_points_[i].pos);
     }
  }

  Eigen::Vector2d getLocalTarget(Eigen::Vector2d &current_pt){
    Eigen::Vector2d target_pt;
    Eigen::Vector2d landmark_pt;

    double dist_to_last_landmark;
    dist_to_last_landmark=(landmark_points_[id_last_landmark_].pos-current_pt).squaredNorm();

    // check if landmark point is arrived, and update target landmark id
    if(dist_to_last_landmark<dist_tolerance_){
      id_last_landmark_++;

      if(id_last_landmark_>=landmark_points_.size()){
        id_last_landmark_=landmark_points_.size()-1;
      }
    }

    // set landmark pos
    landmark_pt=landmark_points_[id_last_landmark_].pos;

    // calculate the target_pt at the direction of landmark pos
    double a;
    a=dist_next_wp_/(current_pt-landmark_pt).squaredNorm();
    target_pt=current_pt+a*(landmark_pt-current_pt);

    return target_pt;
  }

};

class GlobalTrajData
{
  private:
  public:
    PolynomialTraj global_traj_;
    std::vector<UniformBspline> local_traj_;

    double global_duration_;
    ros::Time global_start_time_;

    double local_start_time_, local_end_time_;
    double time_increase_;
    double last_time_inc_;
    double last_progress_time_;

    GlobalTrajData(/* args */) {}
    ~GlobalTrajData() {}

    bool localTrajReachTarget() { return fabs(local_end_time_ - global_duration_) < 0.1; }

    void setGlobalTraj(const PolynomialTraj &traj, const ros::Time &time)
    {
      global_traj_ = traj;
      global_traj_.init();
      global_duration_ = global_traj_.getTimeSum();
      global_start_time_ = time;

      local_traj_.clear();
      local_start_time_ = -1;
      local_end_time_ = -1;
      time_increase_ = 0.0;
      last_time_inc_ = 0.0;
      last_progress_time_ = 0.0;
    }

    void setLocalTraj(UniformBspline traj, double local_ts, double local_te, double time_inc)
    {
      local_traj_.resize(3);
      local_traj_[0] = traj;
      local_traj_[1] = local_traj_[0].getDerivative();
      local_traj_[2] = local_traj_[1].getDerivative();

      local_start_time_ = local_ts;
      local_end_time_ = local_te;
      global_duration_ += time_inc;
      time_increase_ += time_inc;
      last_time_inc_ = time_inc;
    }

    Eigen::Vector2d getPosition(double t)
    {
      if (t >= -1e-3 && t <= local_start_time_)
      {
        return global_traj_.evaluate(t - time_increase_ + last_time_inc_);
      }
      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
      {
        return global_traj_.evaluate(t - time_increase_);
      }
      else
      {
        double tm, tmp;
        local_traj_[0].getTimeSpan(tm, tmp);
        return local_traj_[0].evaluateDeBoor(tm + t - local_start_time_);
      }
    }

    Eigen::Vector2d getVelocity(double t)
    {
      if (t >= -1e-3 && t <= local_start_time_)
      {
        return global_traj_.evaluateVel(t);
      }
      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
      {
        return global_traj_.evaluateVel(t - time_increase_);
      }
      else
      {
        double tm, tmp;
        local_traj_[0].getTimeSpan(tm, tmp);
        return local_traj_[1].evaluateDeBoor(tm + t - local_start_time_);
      }
    }

    Eigen::Vector2d getAcceleration(double t)
    {
      if (t >= -1e-3 && t <= local_start_time_)
      {
        return global_traj_.evaluateAcc(t);
      }
      else if (t >= local_end_time_ && t <= global_duration_ + 1e-3)
      {
        return global_traj_.evaluateAcc(t - time_increase_);
      }
      else
      {
        double tm, tmp;
        local_traj_[0].getTimeSpan(tm, tmp);
        return local_traj_[2].evaluateDeBoor(tm + t - local_start_time_);
      }
    }

    // get Bspline paramterization data of a local trajectory within a sphere
    // start_t: start time of the trajectory
    // dist_pt: distance between the discretized points
    void getTrajByRadius(const double &start_t, const double &des_radius, const double &dist_pt,
                         vector<Eigen::Vector2d> &point_set, vector<Eigen::Vector2d> &start_end_derivative,
                         double &dt, double &seg_duration)
    {
      double seg_length = 0.0; // length of the truncated segment
      double seg_time = 0.0;   // duration of the truncated segment
      double radius = 0.0;     // distance to the first point of the segment

      double delt = 0.2;
      Eigen::Vector2d first_pt = getPosition(start_t); // first point of the segment
      Eigen::Vector2d prev_pt = first_pt;              // previous point
      Eigen::Vector2d cur_pt;                          // current point

      // go forward until the traj exceed radius or global time
      while (radius < des_radius && seg_time < global_duration_ - start_t - 1e-3)
      {
        seg_time += delt;
        seg_time = min(seg_time, global_duration_ - start_t);

        cur_pt = getPosition(start_t + seg_time);
        seg_length += (cur_pt - prev_pt).norm();
        prev_pt = cur_pt;
        radius = (cur_pt - first_pt).norm();
      }

      // get parameterization dt by desired density of points
      int seg_num = floor(seg_length / dist_pt);

      // get outputs

      seg_duration = seg_time; // duration of the truncated segment
      dt = seg_time / seg_num; // time difference between two points

      for (double tp = 0.0; tp <= seg_time + 1e-4; tp += dt)
      {
        cur_pt = getPosition(start_t + tp);
        point_set.push_back(cur_pt);
      }

      start_end_derivative.push_back(getVelocity(start_t));
      start_end_derivative.push_back(getVelocity(start_t + seg_time));
      start_end_derivative.push_back(getAcceleration(start_t));
      start_end_derivative.push_back(getAcceleration(start_t + seg_time));
    }

    // get Bspline paramterization data of a fixed duration local trajectory
    // start_t: start time of the trajectory
    // duration: time length of the segment
    // seg_num: discretized the segment into *seg_num* parts
    void getTrajByDuration(double start_t, double duration, int seg_num,vector<Eigen::Vector2d> &point_set,vector<Eigen::Vector2d> &start_end_derivative, double &dt)
    {
      dt = duration / seg_num;
      Eigen::Vector2d cur_pt;
      for (double tp = 0.0; tp <= duration + 1e-4; tp += dt)
      {
        cur_pt = getPosition(start_t + tp);
        point_set.push_back(cur_pt);
      }

      start_end_derivative.push_back(getVelocity(start_t));
      start_end_derivative.push_back(getVelocity(start_t + duration));
      start_end_derivative.push_back(getAcceleration(start_t));
      start_end_derivative.push_back(getAcceleration(start_t + duration));
    }

};

struct PlanParameters
{
    /* planning algorithm parameters */
    double max_vel_, max_acc_, max_jerk_; // physical limits
    double ctrl_pt_dist_;                  // distance between adjacient B-spline control points
    double feasibility_tolerance_;        // permitted ratio of vel/acc exceeding limits
    double planning_horizen_;

    /* global data param */
    double dist_next_wp_;           // distance from current location to next waypoint
    double dist_tolerance_;         // distance tolerance for arriving
    double rou_thresh_;             // thresh hold of curature radius for determin landmark points

    /* flags */
    bool use_astar_, use_kino_astar_, use_oneshot_;
    bool use_optimization_esdf_, use_optimization_astar_;

    /* processing time */
    //double time_search_ = 0.0;
    //double time_optimize_ = 0.0;
    //double time_adjust_ = 0.0;
};

struct LocalTrajData
{
    /* info of generated traj */

    int traj_id_;
    double duration_;
    double global_time_offset; // This is because when the local traj finished and is going to switch back to the global traj, the global traj time is no longer matches the world time.
    ros::Time start_time_;
    Eigen::Vector2d start_pos_;
    UniformBspline position_traj_, velocity_traj_, acceleration_traj_;
};

#endif


/**
 * \ file TanFilter.cpp
 */

#include <array>
#include <fstream>

#include <ATK/config.h>

#include <ATK/Tools/TanFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#include <boost/math/constants/constants.hpp>

#include <gtest/gtest.h>

constexpr gsl::index PROCESSSIZE = 1000;
constexpr gsl::index SAMPLING_RATE = 1024*64;

TEST(TanFilter, const_sin1k)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(SAMPLING_RATE);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::TanFilter<double> filter;
  filter.set_input_sampling_rate(SAMPLING_RATE);
  filter.set_output_sampling_rate(SAMPLING_RATE);

  filter.set_input_port(0, generator, 0);
  filter.process(PROCESSSIZE);

  auto sin = generator.get_output_array(0);
  auto array = filter.get_output_array(0);

  for(size_t i = 0; i < PROCESSSIZE; ++i)
  {
    ASSERT_NEAR(array[i], std::tan(sin[i] * boost::math::constants::pi<double>() / SAMPLING_RATE), 0.00001);
  }
}


#include <dimensional/quantity.hpp>
#include <dimensional/arithmetic.hpp>
#include <dimensional/systems/si/all.hpp>
#include <dimensional/systems/nonsi/hour.hpp>
#include <dimensional/systems/si/derived_units/area.hpp>
#include <dimensional/systems/si/derived_units/angle.hpp>
#include <dimensional/io.hpp>

#include <dimensional/systems/si/prefix.hpp>
#include <dimensional/math/all.hpp>
#include <dimensional/systems/currency/jpy.hpp>
#include <dimensional/systems/nonsi/degree_celsius.hpp>
#include <dimensional/expr.hpp>
#include <dimensional/delta.hpp>
#include <boost/type_index.hpp>
#include <iostream>

#define REPL(...) \
    do { std::cout << "$ " << #__VA_ARGS__ << "\n=> " << (__VA_ARGS__) << std::endl; } while(false)
int main(){
    using namespace mitama;
    namespace si = mitama::systems::si;
    namespace nonsi = mitama::systems::nonsi;
    { // Homogeneous dimension examples
        std::cout << "--[Homogeneous dimension examples]--\n";

        // quantity<Dimension, Type = double>:
        // quantity is the type T that is meaningful by Dimension;
        // Dimension is a phantom template parameter.
        constexpr quantity<si::meter_t, int> a = 2;
        constexpr quantity<si::millimeter_t, int> b = 2;

        // `a + b` is valid operation if and only if `a` and `b` has same base dimensions.
        auto r1 = a + b;
        REPL(r1);

        // `a - b` is valid operation if and only if `a` and `b` has same base dimensions.
        auto r2 = a - b;
        REPL(r2);

        // `a * b` is always valid and:
        // - Result quantity has heterogeneous dimension inducted from `a` and `b`, and
        // - If both a and b values are of the same dimension, these values are automatically scaled to high precision units.
        auto r3 = a * b;
        REPL(r3);

        // a / b := a * b^{-1}
        auto r4 = a / b;
        REPL(r4);
        REPL(1 + r4);
        REPL(boost::typeindex::type_id<decltype(r4)>().pretty_name());

        quantity<si::millimeter_t, int> d(a);
        REPL(d);

        constexpr auto e = 3 | si::meter<2> ;
        constexpr auto f = 3 | si::millimeter<2> ;
        REPL(e);
        REPL(f);
        REPL(e + f);

        auto v = (1|si::meters) * (1|si::millimeters);
        auto u = (1|si::centimeters);
        REPL(v);
        REPL(u);
        REPL(v/u);
        quantity<si::millimeter_t, int> milli = u;
        REPL(milli);
        std::cout << "------------------------\n";
    }

    { // Heterogeneous dimesnsion examples
        std::cout << "--[Heterogeneous dimension examples]--\n";
        // speed := km/h
        using speed_t = decltype(si::kilometers/nonsi::hour_t{});

        constexpr quantity<si::meter_t> L(1.2);
        constexpr quantity<si::second_t> T(0.3);
        // unit is automatically convert from m/s to km/h
        quantity<speed_t> V = L/T;
        std::cout << boost::typeindex::type_id<decltype(L*T)>().pretty_name() << std::endl;

        std::cout << V.value() << "[ km/h ]" << std::endl;
        {
            auto w = 36 | si::kilogram<> * si::meter<2> * si::second<-2> * si::ampere<-1>;
            std::cout << boost::typeindex::type_id<decltype(w)>().pretty_name() << std::endl;
        }
        using newton_t = decltype(si::kilogram<> * si::meter<> * si::second<-2>);
        quantity<newton_t> N = (1.0|si::kilograms) * V / (2|si::seconds);
        REPL(N);
        std::cout << "------------------------\n";
    }

    { // compare examples
        std::cout << "--[dimension type comparisons examples]--\n";
        std::cout << std::boolalpha;
        REPL((1|si::meters) == (1|si::millimeters));
        REPL((1|si::meters) == (1000|si::millimeters));
        REPL((1|si::meters) != (1|si::millimeters));
        REPL((1|si::meters) != (1000|si::millimeters));
        REPL((1|si::meters) < (1|si::millimeters));
        REPL((1|si::meters) > (1|si::millimeters));
        REPL((1|si::meters) < (1000|si::millimeters));
        REPL((1|si::meters) > (1000|si::millimeters));
        REPL((1|si::meters) <= (1000|si::millimeters));
        REPL((1|si::meters) >= (1000|si::millimeters));
        std::cout << "------------------------\n";
    }

    { // math function examples
        std::cout << "--[dimension type math functions examples]--\n";
        quantity_t<std::decay_t<decltype(si::millimeter<2>)>, double> v = (1|si::meters) * (1|si::millimeters);
        REPL(sqrt(v));
        REPL(cbrt(v));

        REPL(mitama::min((1|si::meters),(1|si::millimeters),(1|si::centimeters)));
        REPL(mitama::max((1|si::meters),(1|si::millimeters),(1|si::centimeters)));
        std::cout << "------------------------\n";

        REPL(pow<5>(2|si::meters));
        REPL(square(2|si::meters));
        REPL(cubic(2|si::meters));

        REPL(hypot(2.|si::meters, 2.|si::meters));

        REPL(ceil(2.2|si::meters));
        REPL(floor(2.2|si::meters));
        REPL(trunc(2.2|si::meters));
        REPL(round(2.2|si::meters));
        REPL(lround(2.2|si::meters));
        REPL(llround(2.2|si::meters));
        REPL(nearbyint(2.2|si::meters));
        REPL(rint(2.2|si::meters));
        REPL(lrint(2.2|si::meters));
        REPL(llrint(2.2|si::milli*si::meters));
    }

    { // User defined dimension examples
        // currency units
        REPL(100|yen);
    }

    { // User defined dimension examples
        // currency units
        REPL(100|nonsi::degree_celsius);
        quantity<si::kelvin_t> s( 100.|nonsi::degree_celsius );
        REPL(s);
    }

    {
        quantity<si::kelvin_t> hoge = as_expr(1|si::kelvins) + (2|nonsi::degree_celsius);
        REPL(hoge);
    }

    {
        quantity_t a1 = accepts<si::area_r> |= (2|si::meters) * (7|si::meters);
        REPL(a1);

        quantity_t a2 = accepts<si::area_r> |= (2|si::millimeters) * (7|si::millimeters);
        REPL(a2);

        // error!
        // quantity_t a3 = accepts<area_r> |= (2|si::millimeters);

        quantity_t a3 = partial_accepts_for<sym::M<>> |= (2|si::meters) * (2|si::meters) * (2|si::kilograms) / (2|si::second<2>);
        REPL(a3);
    }

    {
        delta d = (2|si::kelvins) - (1|si::kelvins);
        REPL((1|nonsi::degree_celsius) + d);
    }

    {
        quantity_t m = 1.0 | si::meters * si::radian;
        quantity_for<double, si::meter_<>> x = m.into();
        REPL(x);
    }
}


#ifndef FDM_HH
#define FDM_HH

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include "PDE.hh"

typedef std::vector<Eigen::VectorXd> matrix;

class FDMBase {
 protected:
  std::shared_ptr<BlackScholesPDE> pde;

  //Space domain [Nminus*dx,Nplus*dx]
  int long Nminus;
  int long Nplus;
  int long N;
  double dx;
  std::vector<double> x_values;

  //Time domain [0, 1/2*simga^2*T]
  unsigned long M;
  double dt;
  std::vector<double> tau_values;

  //coefficients
  double alpha;

  //initial vector;
  Eigen::VectorXd u0;

  //matrices
  Eigen::SparseMatrix<double, Eigen::RowMajor> A, I;

  // Constructor
  FDMBase(int _N , double dx, unsigned long _M, std::shared_ptr<BlackScholesPDE> _pde);

  void calculate_step_sizes();
  void set_initial_conditions();
  void calculate_boundary_conditions(Eigen::VectorXd&, double);

 public:

  double get_dt()const{ return dt; }
  double get_dx()const{ return dx; }
  double get_alpha()const{ return alpha; }
  std::vector<double> get_x() const{ return x_values; }
  std::vector<double> get_tau() const{ return tau_values; }

  void BuildMatrix();
  void change_var(matrix&);
  virtual matrix solve() = 0;
};

class FDMEulerExplicit : public FDMBase {
 public:
  FDMEulerExplicit(int _N, double dx, unsigned long _M, std::shared_ptr<BlackScholesPDE> _pde):
    FDMBase(_N, dx, _M, _pde){}

  matrix solve();
};

class FDMEulerImplicit : public FDMBase{
  public:
    FDMEulerImplicit(int _N, double dx, unsigned long _M, std::shared_ptr<BlackScholesPDE> _pde):
      FDMBase(_N, dx, _M, _pde){}

    matrix solve();
};

class FDMCranckNicholson : public FDMBase{
public:
  FDMCranckNicholson(int _N, double dx, unsigned long _M, std::shared_ptr<BlackScholesPDE> _pde):
    FDMBase(_N, dx, _M, _pde){}

  matrix solve();
};

#endif


#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <Eigen/Core>
#include <vector>
#include <string>
#include <boost/format.hpp>
#include <iostream>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
// 内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 基线
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png"); // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// TODO implement this function
/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21,
    bool show = false);

// TODO implement this function
/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21,
    bool show = false);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y)
{
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]);
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 1000;
    int boarder = 40;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++)
    {
        int x = rng.uniform(boarder, left_img.cols - boarder); // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder); // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++)
    { // 1~10
        cout<<"Image "<<i<<"--------------------------------------"<<endl;
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref, true); // first you need to test single layer
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref, true);
    }
}

void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21,
    bool show)
{

    // parameters
    int half_patch_size = 4;
    int iterations = 100;

    double cost = 0, lastCost = 0;
    int nGood = 0; // good projections
    VecVector2d goodProjection;
    VecVector2d goodOriginal;

    int iter_cnt = 0;
    for (int iter = 0; iter < iterations; iter++)
    {
        iter_cnt = iter;
        nGood = 0;
        goodProjection.clear();
        goodOriginal.clear();

        // Define Hessian and bias
        Matrix6d H = Matrix6d::Zero(); // 6x6 Hessian
        Vector6d b = Vector6d::Zero(); // 6x1 bias

        for (size_t i = 0; i < px_ref.size(); i++)
        {

            // compute the projection in the second image
            // TODO START YOUR CODE HERE
            Eigen::Vector3d P((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
            P *= depth_ref[i];
            Eigen::Vector3d q = T21 * P;
            float u = q[0] / q[2] * fx + cx, v = q[1] / q[2] * fy + cy;

            if (u <= half_patch_size || u >= img2.cols - half_patch_size || v <= half_patch_size || v >= img2.rows - half_patch_size)
            {
                continue;
            }
            nGood++;
            goodProjection.push_back(Eigen::Vector2d(u, v));
            goodOriginal.push_back(px_ref[i]);

            // and compute error and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++)
                {

                    double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) - GetPixelValue(img2, u + x, v + y);

                    Matrix26d J_pixel_xi; // pixel to \xi in Lie algebra
                    J_pixel_xi(0, 0) = fx / q[2];
                    J_pixel_xi(0, 1) = 0;
                    J_pixel_xi(0, 2) = -fx * q[0] / (q[2] * q[2]);
                    J_pixel_xi(0, 3) = -fx * q[0] * q[1] / (q[2] * q[2]);
                    J_pixel_xi(0, 4) = fx + fx * q[0] * q[0] / (q[2] * q[2]);
                    J_pixel_xi(0, 5) = -fx * q[1] / q[2];
                    J_pixel_xi(1, 0) = 0;
                    J_pixel_xi(1, 1) = fy / q[2];
                    J_pixel_xi(1, 2) = -fy * q[1] / (q[2] * q[2]);
                    J_pixel_xi(1, 3) = -fy - fy * q[1] * q[1] / (q[2] * q[2]);
                    J_pixel_xi(1, 4) = fy * q[0] * q[1] / (q[2] * q[2]);
                    J_pixel_xi(1, 5) = fy * q[0] / q[2];
                    Eigen::Vector2d J_img_pixel; // image gradients
                    J_img_pixel[0] = 0.5 * (GetPixelValue(img2, u + x + 1, v + y) - GetPixelValue(img2, u + x - 1, v + y));
                    J_img_pixel[1] = 0.5 * (GetPixelValue(img2, u + x, v + y + 1) - GetPixelValue(img2, u + x, v + y - 1));

                    // total jacobian
                    Vector6d J = -J_pixel_xi.transpose() * J_img_pixel;

                    H += J * J.transpose();
                    b += -error * J;
                    cost += error * error;
                }
            // END YOUR CODE HERE
        }

        // solve update and put it into estimation
        // TODO START YOUR CODE HERE
        Vector6d update;
        update = H.ldlt().solve(b);
        T21 = Sophus::SE3d::exp(update) * T21;
        // END YOUR CODE HERE

        cost /= nGood;

        if (isnan(update[0]))
        {
            // sometimes occurred when we have a black or white patch and H is irreversible
            std::cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost)
        {
            // cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        lastCost = cost;
        if(show)
            std::cout << "iter = "<< iter <<", cost = " << cost << ", good = " << nGood <<"/"<< px_ref.size() << endl;
    }
    if (show)
    {
        std::cout << "end_iter = " << iter_cnt - 1 << ", lastcost = " << lastCost << ", good = " << nGood << "/" << px_ref.size() << endl;
        // std::cout << "good projection: " << nGood << endl;
        std::cout << "T21 = \n"
                  << T21.matrix() << endl;

        // in order to help you debug, we plot the projected pixels here
        cv::Mat img1_show, img2_show;
        cv::cvtColor(img1, img1_show, CV_GRAY2BGR);
        cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
        for (auto &px : px_ref)
        {
            cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
                          cv::Scalar(0, 250, 0));
        }
        // for (auto &px: goodProjection) {
        //     cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
        //                   cv::Scalar(0, 250, 0));
        // }
        for (int i = 0; i < goodProjection.size(); i++)
        {
            cv::circle(img2_show, cv::Point2f(goodProjection[i][0], goodProjection[i][1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(goodProjection[i][0], goodProjection[i][1]), cv::Point2f(goodOriginal[i][0], goodOriginal[i][1]), cv::Scalar(0, 250, 0));
        }
        cv::imshow("reference", img1_show);
        cv::imshow("current", img2_show);
        cv::waitKey(0);
    }
}

void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21,
    bool show)
{

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE
    for (int i = 0; i < pyramids; i++)
    {
        if (i == 0)
        {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
            continue;
        }
        cv::Mat dst1, dst2;
        cv::resize(img1, dst1, cv::Size(), scales[i], scales[i]);
        cv::resize(img2, dst2, cv::Size(), scales[i], scales[i]);
        pyr1.push_back(dst1);
        pyr2.push_back(dst2);
    }

    // END YOUR CODE HERE

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy; // backup the old values
    for (int level = pyramids - 1; level >= 0; level--)
    {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px : px_ref)
        {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // TODO START YOUR CODE HERE
        // scale fx, fy, cx, cy in different pyramid levels
        fx = fxG*scales[level];
        cx = cxG*scales[level];
        fy = fyG*scales[level];
        cy = cyG*scales[level];
        // END YOUR CODE HERE
        if (level==0)
            DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21, show);
        else{
            DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
        }
    }
}


// This file is part of Man, a robotic perception, locomotion, and
// team strategy application created by the Northern Bites RoboCup
// team of Bowdoin College in Brunswick, Maine, for the Aldebaran
// Nao robot.
//
// Man is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// Man 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 Lesser Public License for more details.
//
// You should have received a copy of the GNU General Public License
// and the GNU Lesser Public License along with Man.  If not, see
// <http://www.gnu.org/licenses/>.

#include <math.h>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>              // for lu_factorize
#include <boost/numeric/ublas/io.hpp>              // for cout

#include "nb/CoordFrame.h"

using namespace NBMath;
using namespace CoordFrame3D;

// -------------------- Helper matrix methods --------------------
const NBMath::ufmatrix3 CoordFrame3D::rotation3D(const Axis axis,
                                                 const double angle) {
    NBMath::ufmatrix3 rot =
        boost::numeric::ublas::identity_matrix <double>(3);

    if (angle == 0.0) { //OPTIMIZAION POINT
        return rot;
    }
    const double sinAngle = std::sin(angle);
    const double cosAngle = std::cos(angle);

    switch(axis) {
    case Z_AXIS:
        rot(X_AXIS, X_AXIS) =  cosAngle;
        rot(X_AXIS, Y_AXIS) = -sinAngle;
        rot(Y_AXIS, X_AXIS) =  sinAngle;
        rot(Y_AXIS, Y_AXIS) =  cosAngle;
        break;
    default:
        break;
    }
    return rot;
}

const NBMath::ufmatrix3 CoordFrame3D::translation3D(const double dx,
                                                    const double dy) {
    boost::numeric::ublas::matrix <double> trans =
        boost::numeric::ublas::identity_matrix <double>(3);
    trans(X_AXIS, Z_AXIS) = dx;
    trans(Y_AXIS, Z_AXIS) = dy;
    return trans;
}

const NBMath::ufvector3 CoordFrame3D::vector3D(const double x, const double y,
                                               const double z) {
    NBMath::ufvector3 p = boost::numeric::ublas::zero_vector <double> (3);
    p(0) = x;
    p(1) = y;
    p(2) = z;
    return p;
}

const NBMath::ufrowVector3 CoordFrame3D::rowVector3D(const double x,
                                                  const double y,
                                                  const double z) {

    NBMath::ufrowVector3 p(1, 3);

    p(0,0) = x;

    p(0,1) = y;

    p(0,2) = z;

    return p;
}

const NBMath::ufmatrix3 CoordFrame3D::identity3D(){
    return boost::numeric::ublas::identity_matrix <double> (3);
}


// bit_tuple_iterator.hpp
//
// Generate all n-tuples where each element is drawn
// from the set {0,1}. The algorithm is based on
// incrementing a binary counter. (Note the difference
// to e.g. Gray code).

#ifndef BIT_TUPLE_ITERATOR_HPP
#define BIT_TUPLE_ITERATOR_HPP

#include <cstdint>
#include <numeric>
#include <type_traits>
#include <cassert>

#include <boost/iterator/iterator_facade.hpp>

template <typename T>
class bit_tuple_iterator
	: public boost::iterator_facade <
	bit_tuple_iterator<T>,
	const T&,
	boost::forward_traversal_tag
	>
{
private:
	static_assert(std::is_integral<T>::value, "T must be integral");

public:
	bit_tuple_iterator() : end_(true), n_(0), p_(0) { }

	explicit bit_tuple_iterator(int p) : end_(false), n_(0), p_(std::pow(2, p) - 1)
	{
		assert(p_ == std::pow(2, p) - 1 && "T not large enough to hold 2^p");
		assert(p <= sizeof(T) * 8 && "T not large enough to hold n tuples");
		assert(n_ == 0);
	}

private:
	friend class boost::iterator_core_access;

	void increment()
	{
		if (p_ == n_)
		{
			end_ = true;
		}

		++n_;
	}

	bool equal(const bit_tuple_iterator& other) const
	{
		return end_ == other.end_;
	}

	const T& dereference() const
	{
		return n_;
	}

	bool end_;
	T n_;
	const T p_;
};

#endif


#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    // 3x3 C-style row major storage, base zero
    Array<int,2> A(3, 3);

    // 3x3 column major storage, base zero
    Array<int,2> B(3, 3, ColumnMajorArray<2>());

    // A custom storage format:
    // Indices have range 0..3, 0..3
    // Column major ordering
    // Rows are stored ascending, columns stored descending
    GeneralArrayStorage<2> storage;
    storage.ordering() = firstRank, secondRank;
    storage.base() = 0, 0;
    storage.ascendingFlag() = true, false;

    Array<int,2> C(3, 3, storage);

    // Set each array equal to
    // [ 1 2 3 ]
    // [ 4 5 6 ]
    // [ 7 8 9 ]

    A = 1, 2, 3,
        4, 5, 6,
        7, 8, 9;

    cout << "A = " << A << endl;

    // Comma-delimited lists initialize in memory-storage order only.
    // Hence we list the values in column-major order to initialize B:

    B = 1, 4, 7, 2, 5, 8, 3, 6, 9;

    cout << "B = " << B << endl;

    // Array C is stored in column major, plus the columns are stored
    // in descending order!

    C = 3, 6, 9, 2, 5, 8, 1, 4, 7;

    cout << "C = " << C << endl;

    Array<int,2> D(3,3);
    D = A + B + C;

#ifdef BZ_DEBUG
    A.dumpStructureInformation();
    B.dumpStructureInformation();
    C.dumpStructureInformation();
    D.dumpStructureInformation();
#endif

    cout << "D = " << D << endl;

    return 0;
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <vector>
#include <iostream>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/adjacency_list.hpp>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, directedS > Graph;
  const int N = 6;
  Graph G(N);
  add_edge(0, 1, G);
  add_edge(1, 1, G);
  add_edge(1, 3, G);
  add_edge(1, 4, G);
  add_edge(3, 4, G);
  add_edge(3, 0, G);
  add_edge(4, 3, G);
  add_edge(5, 2, G);

  std::vector<int> c(N);
  int num = strong_components
    (G, make_iterator_property_map(c.begin(), get(vertex_index, G), c[0]));

  std::cout << "Total number of components: " << num << std::endl;
  std::vector < int >::iterator i;
  for (i = c.begin(); i != c.end(); ++i)
    std::cout << "Vertex " << i - c.begin()
      << " is in component " << *i << std::endl;
  return EXIT_SUCCESS;
}


/*
 * Copyright Nick Thompson, 2017
 * 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)
 *
 * Use the adaptive trapezoidal rule to estimate the integral of periodic functions over a period,
 * or to integrate a function whose derivative vanishes at the endpoints.
 *
 * If your function does not satisfy these conditions, and instead is simply continuous and bounded
 * over the whole interval, then this routine will still converge, albeit slowly. However, there
 * are much more efficient methods in this case, including Romberg, Simpson, and double exponential quadrature.
 */

#ifndef BOOST_MATH_QUADRATURE_TRAPEZOIDAL_HPP
#define BOOST_MATH_QUADRATURE_TRAPEZOIDAL_HPP

#include <cmath>
#include <limits>
#include <stdexcept>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost{ namespace math{ namespace quadrature {

template<class F, class Real, class Policy>
auto trapezoidal(F f, Real a, Real b, Real tol, std::size_t max_refinements, Real* error_estimate, Real* L1, const Policy& pol)->decltype(std::declval<F>()(std::declval<Real>()))
{
    static const char* function = "boost::math::quadrature::trapezoidal<%1%>(F, %1%, %1%, %1%)";
    using std::abs;
    using boost::math::constants::half;
    // In many math texts, K represents the field of real or complex numbers.
    // Too bad we can't put blackboard bold into C++ source!
    typedef decltype(f(a)) K;
    if(a >= b)
    {
       return static_cast<K>(boost::math::policies::raise_domain_error(function, "a < b for integration over the region [a, b] is required, but got a = %1%.\n", a, pol));
    }
    if (!(boost::math::isfinite)(a))
    {
       return static_cast<K>(boost::math::policies::raise_domain_error(function, "Left endpoint of integration must be finite for adaptive trapezoidal integration but got a = %1%.\n", a, pol));
    }
    if (!(boost::math::isfinite)(b))
    {
       return static_cast<K>(boost::math::policies::raise_domain_error(function, "Right endpoint of integration must be finite for adaptive trapedzoidal integration but got b = %1%.\n", b, pol));
    }

    K ya = f(a);
    K yb = f(b);
    Real h = (b - a)*half<Real>();
    K I0 = (ya + yb)*h;
    Real IL0 = (abs(ya) + abs(yb))*h;

    K yh = f(a + h);
    K I1;
    I1 = I0*half<Real>() + yh*h;
    Real IL1 = IL0*half<Real>() + abs(yh)*h;

    // The recursion is:
    // I_k = 1/2 I_{k-1} + 1/2^k \sum_{j=1; j odd, j < 2^k} f(a + j(b-a)/2^k)
    std::size_t k = 2;
    // We want to go through at least 4 levels so we have sampled the function at least 10 times.
    // Otherwise, we could terminate prematurely and miss essential features.
    // This is of course possible anyway, but 10 samples seems to be a reasonable compromise.
    Real error = abs(I0 - I1);
    while (k < 4 || (k < max_refinements && error > tol*IL1) )
    {
        I0 = I1;
        IL0 = IL1;

        I1 = I0*half<Real>();
        IL1 = IL0*half<Real>();
        std::size_t p = static_cast<std::size_t>(1u) << k;
        h *= half<Real>();
        K sum = 0;
        Real absum = 0;

        for(std::size_t j = 1; j < p; j += 2)
        {
            K y = f(a + j*h);
            sum += y;
            absum += abs(y);
        }

        I1 += sum*h;
        IL1 += absum*h;
        ++k;
        error = abs(I0 - I1);
    }

    if (error_estimate)
    {
        *error_estimate = error;
    }

    if (L1)
    {
        *L1 = IL1;
    }

    return static_cast<K>(I1);
}
#if BOOST_WORKAROUND(BOOST_MSVC, < 1800)
// Template argument dedcution failure otherwise:
template<class F, class Real>
auto trapezoidal(F f, Real a, Real b, Real tol = 0, std::size_t max_refinements = 12, Real* error_estimate = 0, Real* L1 = 0)->decltype(std::declval<F>()(std::declval<Real>()))
#elif !defined(BOOST_NO_CXX11_NULLPTR)
template<class F, class Real>
auto trapezoidal(F f, Real a, Real b, Real tol = boost::math::tools::root_epsilon<Real>(), std::size_t max_refinements = 12, Real* error_estimate = nullptr, Real* L1 = nullptr)->decltype(std::declval<F>()(std::declval<Real>()))
#else
template<class F, class Real>
auto trapezoidal(F f, Real a, Real b, Real tol = boost::math::tools::root_epsilon<Real>(), std::size_t max_refinements = 12, Real* error_estimate = 0, Real* L1 = 0)->decltype(std::declval<F>()(std::declval<Real>()))
#endif
{
#if BOOST_WORKAROUND(BOOST_MSVC, <= 1600)
   if (tol == 0)
      tol = boost::math::tools::root_epsilon<Real>();
#endif
   return trapezoidal(f, a, b, tol, max_refinements, error_estimate, L1, boost::math::policies::policy<>());
}

}}}
#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "FullyConnected.hpp"

#include "RefWorkloadUtils.hpp"

#include <boost/assert.hpp>

namespace armnn
{

void FullyConnected(const TensorShape& rInputShape,
                    Decoder<float>& rInputDecoder,
                    const TensorShape& rOutputShape,
                    Encoder<float>& rOutputEncoder,
                    Decoder<float>& rWeightDecoder,
                    Decoder<float>& rBiasDecoder,
                    const bool biasEnabled,
                    const unsigned int K,
                    const bool transposeWeights)
{
    // Perform FullyConnected implementation
    unsigned int outputSize = rOutputShape[1];

    for (unsigned int n = 0; n < rInputShape[0]; n++)
    {
        for (unsigned int channelOutput = 0; channelOutput < outputSize; channelOutput++)
        {
            float outval = 0.f;

            for (unsigned int channelInput = 0; channelInput < K; channelInput++)
            {
                float weight;
                if (transposeWeights)
                {
                    rWeightDecoder[channelOutput * K + channelInput];
                    weight = rWeightDecoder.Get();
                }
                else
                {
                    rWeightDecoder[channelInput * outputSize + channelOutput];
                    weight = rWeightDecoder.Get();
                }

                rInputDecoder[n * K + channelInput];
                outval += weight * rInputDecoder.Get();
            }

            if (biasEnabled)
            {
                rBiasDecoder[channelOutput];
                outval += rBiasDecoder.Get();
            }

            rOutputEncoder[n * outputSize + channelOutput];
            rOutputEncoder.Set(outval);
        }
    }
}

} //namespace armnn


/* boost random/triangle_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: triangle_distribution.hpp 11696 2001-11-14 21:53:38Z jmaurer $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP
#define BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

// triangle distribution, with a smallest, b most probable, and c largest
// value.
template<class UniformRandomNumberGenerator, class RealType = double>
class triangle_distribution
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;
  triangle_distribution(base_type & rng, result_type a, result_type b,
                        result_type c)
    : _rng(rng), _a(a), _b(b), _c(c),
      d1(_b-_a), d2(_c-_a), d3(_c-_b), q1(d1/d2), p1(d1*d2)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    d3 = sqrt(d3);
    p1 = sqrt(p1);
    assert(_a <= _b && _b <= _c);
  }
  // compiler-generated copy ctor is fine
  // uniform_01 cannot be assigned, neither can this class
  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    result_type u = _rng();
    if( u <= q1 )
      return _a + p1*sqrt(u);
    else
      return _c - d3*sqrt(d2*u-d1);
  }
#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const triangle_distribution& x,
                         const triangle_distribution& y)
  { return x._a == y._a && x._b == y._b && x._c == y._c && x._rng == y._rng; }
#else
  // Use a member function
  bool operator==(const triangle_distribution& rhs) const
  { return _a == rhs._a && _b == rhs._b && _c == rhs._c && _rng == rhs._rng;  }
#endif
private:
  uniform_01<base_type, result_type> _rng;
  result_type _a, _b, _c;
  result_type d1, d2, d3, q1, p1;
};

} // namespace boost

#endif // BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP


/*
 * @brief Linear interpolation tools.
 * @author Jenna Reher (jreher@caltech.edu)
 */

#ifndef LINEAR_INTERPOLATION_HPP
#define LINEAR_INTERPOLATION_HPP

#include <Eigen/Dense>
#include <iostream>
#include <vector>
using namespace Eigen;

namespace cassie_common_toolbox {

int find_index(VectorXd &X, double &Xi);

void linear_interp(VectorXd &X, MatrixXd &Y, double Xi, VectorXd &Yi );

void bilinear_interp(VectorXd &X, VectorXd &Y, std::vector< std::vector<VectorXd> > Z, double Xi, double Yi, VectorXd &Zi );

} // namespace cassie_common_toolbox

#endif // LINEAR_INTERPOLATION_HPP


//==============================================================================
//          Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TENPOWER_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TENPOWER_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/tenpower.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_odd.hpp>
#include <boost/simd/include/functions/simd/is_ltz.hpp>
#include <boost/simd/include/functions/simd/shift_right.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/any.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/sqr.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/ten.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/mpl/equal_to.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF( tenpower_, tag::cpu_
                             , (A0)(X)
                             , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                , boost::simd::meta::cardinal_of<typename dispatch::meta::as_floating<A0>::type>
                                >)
                             , ((simd_<int_<A0>,X>))
                          )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type result = One<result_type>();
      result_type base = Ten<result_type>();
      A0 exp = boost::simd::abs(a0);
      while(any(exp))
      {
        result *= if_else(is_odd(exp), base, One<result_type>());
        exp >>= 1;
        base = sqr(base);
      }
      return if_else(is_ltz(a0), rec(result), result);
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF( tenpower_, tag::cpu_
                             , (A0)(X)
                             , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                , boost::simd::meta::cardinal_of<typename dispatch::meta::as_floating<A0>::type>
                                >)
                             , ((simd_<uint_<A0>,X>))
                          )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type result = One<result_type>();
      result_type base = Ten<result_type>();
      A0 exp = a0;
      while(any(exp))
      {
        result *= if_else(is_odd(exp), base, One<result_type>());
        exp >>= 1;
        base = sqr(base);
      }
      return result;
    }
  };
} } }

#endif


/*
 * Copyright 2012 Karsten Ahnert
 * Copyright 2013 Mario Mulansky
 *
 * 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)
 */

#include <iostream>
#include <vector>

#include <vexcl/vexcl.hpp>

#include <boost/numeric/odeint.hpp>
//[ vexcl_includes
#include <boost/numeric/odeint/external/vexcl/vexcl.hpp>
//]

namespace odeint = boost::numeric::odeint;

//[ vexcl_state_types
typedef vex::vector< double >    vector_type;
typedef vex::multivector< double, 3 > state_type;
//]

//[ vexcl_system
const double sigma = 10.0;
const double b = 8.0 / 3.0;

struct sys_func
{
    const vector_type &R;

    sys_func( const vector_type &_R ) : R( _R ) { }

    void operator()( const state_type &x , state_type &dxdt , double t ) const
    {
        dxdt(0) = -sigma * ( x(0) - x(1) );
        dxdt(1) = R * x(0) - x(1) - x(0) * x(2);
        dxdt(2) = - b * x(2) + x(0) * x(1);
    }
};
//]

int main( int argc , char **argv )
{
    using namespace std;
    using namespace odeint;

    //[ vexcl_main
    // setup the opencl context
    vex::Context ctx( vex::Filter::Type(CL_DEVICE_TYPE_GPU) );
    std::cout << ctx << std::endl;

    // set up number of system, time step and integration time
    const size_t n = 1024 * 1024;
    const double dt = 0.01;
    const double t_max = 1000.0;

    // initialize R
    double Rmin = 0.1 , Rmax = 50.0 , dR = ( Rmax - Rmin ) / double( n - 1 );
    std::vector<double> x( n * 3 ) , r( n );
    for( size_t i=0 ; i<n ; ++i ) r[i] = Rmin + dR * double( i );
    vector_type R( ctx.queue() , r );

    // initialize the state of the lorenz ensemble
    state_type X(ctx.queue(), n);
    X(0) = 10.0;
    X(1) = 10.0;
    X(2) = 10.0;

    // create a stepper
    runge_kutta4< state_type > stepper;

    // solve the system
    integrate_const( stepper , sys_func( R ) , X , 0.0 , t_max , dt );
    //]

    std::vector< double > res( 3 * n );
    vex::copy( X(0) , res );
    for( size_t i=0 ; i<n ; ++i )
        cout << r[i] << "\t" << res[i] << "\t" << "\n";
}


#include <math/transform.h>
#include <Eigen/Geometry>
#include "test_common.h"

namespace {

constexpr double epsilon = 1e-4;
std::vector<Eigen::Vector3f> test_vectors = {
    {0.f, 0.f, 0.f},
    {1.f, 0.f, 0.f},
    {0.f, 1.f, 0.f},
    {0.f, 0.f, 1.f},
    {-1.f, 5.f, 6.f}
};
}

TEST(Transform,TestIdentityAndReset)
{
    cpt::Transform xform;
    EXPECT_TRUE(xform.translation().isZero());
    EXPECT_TRUE(xform.scale().isOnes());
    EXPECT_TRUE(xform.rotation().isIdentity());

    xform.set_translation(Eigen::Vector3f::Constant(1.f));
    {
        Eigen::AngleAxisf base_ref_rotation(M_PI / 4.f, Eigen::Vector3f(1.f, 1.f, 1.f).normalized());
        Eigen::Matrix3f ref_rotation = base_ref_rotation.toRotationMatrix();
        xform.set_rotation(ref_rotation);
    }
    xform.set_scale(Eigen::Vector3f::Constant(0.5f));
    xform.reset();
    EXPECT_TRUE(xform.translation().isZero());
    EXPECT_TRUE(xform.scale().isOnes());
    EXPECT_TRUE(xform.rotation().isIdentity());

    for (const auto& test_vector : test_vectors) {
        EXPECT_TRUE((xform * test_vector - test_vector).isZero());
        EXPECT_TRUE((xform.homogeneous_mult(test_vector) - test_vector).isZero());
    }
}

TEST(Transform,TestTranslation)
{
    const Eigen::Vector3f ref_trans(0.1f, -0.5f, 2.f);
    cpt::Transform xform;
    xform.set_translation(ref_trans);
    EXPECT_TRUE(xform.translation().isApprox(ref_trans));

    {
        Eigen::Vector3f test_vector(0.f, 0.f, 0.f);
        Eigen::Vector3f ref_vector(0.1f, -0.5f, 2.f);
        EXPECT_TRUE((xform * test_vector - test_vector).isZero());
        EXPECT_TRUE((xform.homogeneous_mult(test_vector) - ref_vector).isZero());
    }

    {
        Eigen::Vector3f test_vector(8.f, -3.f, 0.3f);
        Eigen::Vector3f ref_vector(8.1f, -3.5f, 2.3f);
        EXPECT_TRUE((xform * test_vector - test_vector).isZero());
        EXPECT_TRUE((xform.homogeneous_mult(test_vector) - ref_vector).isZero());
    }
}

TEST(Transform,TestRotation)
{
    Eigen::AngleAxisf base_ref_rotation(M_PI / 4.f, Eigen::Vector3f(1.f, 1.f, 1.f).normalized());
    Eigen::Matrix3f ref_rotation = base_ref_rotation.toRotationMatrix();

    cpt::Transform xform;
    xform.set_rotation(ref_rotation);
    EXPECT_TRUE(xform.rotation().isApprox(ref_rotation));

    for (const auto& test_vector : test_vectors) {
        const Eigen::Vector3f ref_vector = ref_rotation * test_vector;
        EXPECT_TRUE((xform * test_vector - ref_vector).isZero());
        EXPECT_TRUE((xform.homogeneous_mult(test_vector) - ref_vector).isZero());
    }
}

TEST(Transform,TestInvalidRotation)
{
    Eigen::Matrix3f invalid_rotation;
    invalid_rotation <<
        1.f, 2.f, 3.f,
        4.f, 5.f, 6.f,
        7.f, 8.f, 9.f;

    cpt::Transform xform;
    EXPECT_THROW(xform.set_rotation(invalid_rotation), std::runtime_error);
}

TEST(Transform,TestScale)
{
    const Eigen::Vector3f ref_scale(0.1f, -0.5f, 2.f);
    cpt::Transform xform;
    xform.set_scale(ref_scale);
    EXPECT_TRUE(xform.scale().isApprox(ref_scale));

    {
        Eigen::Vector3f test_vector(0.f, 0.f, 0.f);
        EXPECT_TRUE((xform * test_vector - test_vector).isZero());
        EXPECT_TRUE((xform.homogeneous_mult(test_vector) - test_vector).isZero());
    }

    {
        Eigen::Vector3f test_vector(8.f, -3.f, 0.3f);
        Eigen::Vector3f ref_vector(0.8f, 1.5f, 0.6f);
        EXPECT_TRUE((xform * test_vector - ref_vector).isZero());
        EXPECT_TRUE((xform.homogeneous_mult(test_vector) - ref_vector).isZero());
    }
}

TEST(Transform,TestAllApply)
{
    // This just ensures we're doing the order of operations correctly (scale, rotate, translate).
    const Eigen::Vector3f ref_trans(0.1f, -0.5f, 2.f);
    const Eigen::Vector3f ref_scale(0.5f, -0.1f, 3.f);
    Eigen::AngleAxisf base_ref_rotation(M_PI / 4.f, Eigen::Vector3f(1.f, 1.f, 1.f).normalized());
    Eigen::Matrix3f ref_rotation = base_ref_rotation.toRotationMatrix();

    cpt::Transform xform;
    xform.set_translation(ref_trans);
    xform.set_scale(ref_scale);
    xform.set_rotation(ref_rotation);

    EXPECT_TRUE(xform.translation().isApprox(ref_trans));
    EXPECT_TRUE(xform.rotation().isApprox(ref_rotation));
    EXPECT_TRUE(xform.scale().isApprox(ref_scale));

    {
        Eigen::Vector3f test_vector(0.f, 0.f, 0.f);
        Eigen::Vector3f ref_vector(0.1f, -0.5f, 2.f);
        EXPECT_TRUE((xform * test_vector - test_vector).isZero());
        EXPECT_TRUE((xform.homogeneous_mult(test_vector) - ref_vector).isZero());
    }

    {
        Eigen::Vector3f test_vector(1.f, 1.f, 1.f);
        Eigen::Vector3f ref_vector_no_trans(1.9510677f, -0.7593853, 2.2083176f);
        Eigen::Vector3f ref_vector_with_trans(2.0510677f, -1.2593853f, 4.2083176f);
        EXPECT_TRUE((xform * test_vector - ref_vector_no_trans).isZero());
        EXPECT_TRUE((xform.homogeneous_mult(test_vector) - ref_vector_with_trans).isZero());
    }
}

TEST(Transform,TestCombineTranslation)
{
    cpt::Transform xform1;
    xform1.set_translation(Eigen::Vector3f::UnitX());

    cpt::Transform xform2;
    xform2.set_translation(Eigen::Vector3f::UnitY());

    {
        cpt::Transform test = xform1 * xform2;
        EXPECT_TRUE(test.translation().isApprox(Eigen::Vector3f(1.f, 1.f, 0.f)));
    }

    {
        cpt::Transform test = xform2 * xform1;
        EXPECT_TRUE(test.translation().isApprox(Eigen::Vector3f(1.f, 1.f, 0.f)));
    }
}

TEST(Transform,TestCombineScale)
{
    cpt::Transform xform1;
    xform1.set_scale(Eigen::Vector3f::Constant(0.5f));

    cpt::Transform xform2;
    xform2.set_scale(Eigen::Vector3f::Constant(0.1f));

    {
        cpt::Transform test = xform1 * xform2;
        EXPECT_TRUE(test.scale().isApproxToConstant(0.05f));
    }

    {
        cpt::Transform test = xform2 * xform1;
        EXPECT_TRUE(test.scale().isApproxToConstant(0.05f));
    }
}

TEST(Transform,TestCombineRotation)
{
    Eigen::AngleAxisf base_ref_rotation1(M_PI / 4.f, Eigen::Vector3f(1.f, 1.f, 1.f).normalized());
    Eigen::Matrix3f ref_rotation1 = base_ref_rotation1.toRotationMatrix();

    Eigen::AngleAxisf base_ref_rotation2(M_PI / 3.f, Eigen::Vector3f(1.f, 0.f, 0.f).normalized());
    Eigen::Matrix3f ref_rotation2 = base_ref_rotation2.toRotationMatrix();

    cpt::Transform xform1, xform2;
    xform1.set_rotation(ref_rotation1);
    xform2.set_rotation(ref_rotation2);

    {
        cpt::Transform test = xform1 * xform2;
        EXPECT_TRUE(test.rotation().isApprox(ref_rotation1 * ref_rotation2));
    }

    {
        cpt::Transform test = xform2 * xform1;
        EXPECT_TRUE(test.rotation().isApprox(ref_rotation2 * ref_rotation1));
    }
}

TEST(Transform,TestFromTransformMatrix)
{
    const Eigen::Vector3f ref_translation(0.1f, 0.2f, 0.3f);
    const Eigen::Vector3f ref_scale(1.5f, -2.f, 0.75f);
    const Eigen::AngleAxisf ref_rotation(M_PI / 4.f, Eigen::Vector3f(1.f, 1.f, 1.f).normalized());

    Eigen::Affine3f ref_xform;
    ref_xform.setIdentity();
    ref_xform.prescale(ref_scale);
    ref_xform.prerotate(ref_rotation);
    ref_xform.pretranslate(ref_translation);

    cpt::Transform xform = cpt::Transform::from_transform_matrix(ref_xform.matrix());
    EXPECT_TRUE(xform.translation().isApprox(ref_translation));
    // Can't guarantee that we decompose the scale exactly but rot * scale should be the same.
    EXPECT_TRUE((xform.rotation() * xform.scale().asDiagonal()).isApprox(ref_xform.matrix().block(0,0,3,3)));
}

TEST(Transform,TestCombineTransform)
{
    const Eigen::Vector3f ref_translation1(0.1f, 0.2f, 0.3f);
    const Eigen::Vector3f ref_scale1(1.5f, -2.f, 0.75f);
    const Eigen::AngleAxisf ref_rotation1(M_PI / 4.f, Eigen::Vector3f(1.f, 1.f, 1.f).normalized());

    const Eigen::Vector3f ref_translation2(1.1f, -0.25, -0.1f);
    const Eigen::Vector3f ref_scale2(0.2f, 0.1f, 0.83f);
    const Eigen::AngleAxisf ref_rotation2(M_PI / 3.f, Eigen::Vector3f(0.f, 1.f, 0.5f).normalized());

    Eigen::Affine3f ref_xform;
    ref_xform.setIdentity();
    ref_xform.prescale(ref_scale1);
    ref_xform.prerotate(ref_rotation1);
    ref_xform.pretranslate(ref_translation1);
    ref_xform.prescale(ref_scale2);
    ref_xform.prerotate(ref_rotation2);
    ref_xform.pretranslate(ref_translation2);

    cpt::Transform xform1;
    xform1.set_translation(ref_translation1);
    xform1.set_rotation(ref_rotation1.toRotationMatrix());
    xform1.set_scale(ref_scale1);

    cpt::Transform xform2;
    xform2.set_translation(ref_translation2);
    xform2.set_rotation(ref_rotation2.toRotationMatrix());
    xform2.set_scale(ref_scale2);

    cpt::Transform test_xform = xform2 * xform1;
    EXPECT_TRUE(test_xform.translation().isApprox(ref_xform.translation()));
    EXPECT_TRUE((test_xform.rotation() * test_xform.scale().asDiagonal()).isApprox(ref_xform.matrix().block(0,0,3,3)));
}

CREATE_GENERIC_TEST_MAIN


//
// Underlying iLQRNode for Tree-iLQR.
//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

#pragma once

#include <ilqr/ilqr_taylor_expansions.hh>

#include <Eigen/Dense>

#include <functional>
#include <memory>
#include <ostream>
#include <vector>

namespace ilqr
{

// Each plan node represents a timestep.
class iLQRNode
{
public:
    iLQRNode(const int state_dim,
             const int control_dim,
             const DynamicsFunc &dynamics_func,
             const CostFunc &cost_func,
             const double probablity);

    iLQRNode(const Eigen::VectorXd &x_star,
             const Eigen::VectorXd &u_star,
             const DynamicsFunc &dynamics_func,
             const CostFunc &cost_func,
             const double probablity);

    // Compute the control policy and quadratic value of the node given the next
    // timestep value. The policy and value are set directly in the node.
    //void bellman_backup(const QuadraticValue& Jt1);
    void bellman_backup(const std::vector<std::shared_ptr<iLQRNode>> &children);

    // Computes a feedback control from state xt.
    Eigen::VectorXd compute_control(const Eigen::VectorXd &xt) const;
    // Computes a feedback control from state xt except moving only "alpha" step-size away from the
    // expansion point u().
    Eigen::VectorXd compute_control(const Eigen::VectorXd &xt, const double alpha) const;

    // Get and set the probability
    double probability() const { return probability_; }
    void set_probability(double p) { probability_ = p; }

    DynamicsFunc& dynamics_func() { return dynamics_func_; }
    CostFunc& cost_func() { return cost_func_; }
    const DynamicsFunc& dynamics_func() const { return dynamics_func_; }
    const CostFunc& cost_func() const { return cost_func_; }

    // Current Taylor expansion points x and u.
    Eigen::VectorXd& x() { return x_; }
    Eigen::VectorXd& u() { return u_; }
    const Eigen::VectorXd& x() const { return x_; }
    const Eigen::VectorXd& u() const { return u_; }

    // Original Taylor expansion points x and u.
    Eigen::VectorXd& orig_xstar() { return orig_xstar_; }
    Eigen::VectorXd& orig_ustar() { return orig_ustar_; }
    const Eigen::VectorXd& orig_xstar() const { return orig_xstar_; }
    const Eigen::VectorXd& orig_ustar() const { return orig_ustar_; }

    const QuadraticValue& value() const { return J_; };
    QuadraticValue& value() { return J_; };

    const Eigen::MatrixXd& K() const { return K_; };
    Eigen::MatrixXd& K() { return K_; };

    const Eigen::VectorXd& k() const { return k_; };
    Eigen::VectorXd& k() { return k_; };

private:
    DynamicsFunc dynamics_func_;
    CostFunc cost_func_;

    // Set point used for linearization of THIS node's cost and
    // all CHILD node dynamics.
    Eigen::VectorXd x_;
    Eigen::VectorXd u_;

    // Probability of transitioning to this node from the parent.
    double probability_;

    // The terms of the quadratic value function, 1/2 * x^T V x + Gx + W.
    QuadraticValue J_;

    // Feedback gain matrix on the extended-state, [dim(u)] x [dim(x) + 1]
    Eigen::MatrixXd K_;
    // Feed-forward control matrix, [dim(u)] x [dim(1)].
    Eigen::VectorXd k_;

    // Original nominal state specified at the beginning of iLQR. [dim(x)] x [1]
    Eigen::VectorXd orig_xstar_;
    // Original nominal control specified at the beginning of iLQR. [dim(u)] x [1]
    Eigen::VectorXd orig_ustar_;
};

// Allows the iLQRNode to be printed.
std::ostream& operator<<(std::ostream& os, const ilqr::iLQRNode& node);

} // namespace ilqr


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra elementary tgamma
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/algorithm/algebra/elementary/tgamma.h"

namespace fa = fern::algorithm;

template<
    class Value>
using OutOfDomainPolicy = fa::tgamma::OutOfDomainPolicy<Value>;

BOOST_AUTO_TEST_CASE(out_of_domain_policy)
{
    {
        OutOfDomainPolicy<double> policy;
        BOOST_CHECK( policy.within_domain(5));
        BOOST_CHECK(!policy.within_domain(-5));
        BOOST_CHECK( policy.within_domain(-5.01));
        BOOST_CHECK( policy.within_domain(-4.99));
        BOOST_CHECK(policy.within_domain(0));
        BOOST_CHECK(policy.within_domain(-0));
    }
}

template<
    class Value,
    class Result>
using OutOfRangePolicy = fa::tgamma::OutOfRangePolicy<Value, Result>;

template<
    class Value,
    class Result>
struct VerifyWithinRange
{
    bool operator()(
        Value const& value)
    {
        fa::SequentialExecutionPolicy sequential;

        OutOfRangePolicy<Value, Result> policy;
        Result result;

        fa::algebra::tgamma(sequential, value, result);

        return policy.within_range(value, result);
    }
};

BOOST_AUTO_TEST_CASE(out_of_range_policy)
{
    {
        VerifyWithinRange<double, double> verify;
        BOOST_CHECK_EQUAL(verify(-1.0), false);
        BOOST_CHECK_EQUAL(verify(fern::infinity<double>()), false);
    }
}

template<
    class Value,
    class Result>
void verify_value(
    Value const& value,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::algebra::tgamma(sequential, value, result_we_get);
    BOOST_CHECK_CLOSE(result_we_get, result_we_want, 1e-6);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    verify_value<double, double>(10.0, 362880.0);
    verify_value<double, double>(0.5, std::sqrt(fern::pi<double>()));
    verify_value<double, double>(1.0, 1.0);
}