#include "cutting_plane.hpp"
#include "ell.hpp"
#include "profit_oracle.hpp"
#include "ldlt_ext.hpp"
//#include <boost/numeric/ublas/symmetric.hpp>
#include <xtensor/xarray.hpp>
#include <xtensor-blas/xlinalg.hpp>
//#include <fmt/format.h>
#include <iostream>

//#include <boost/numeric/ublas/io.hpp>

// Versions: (latest first)
// http://melpon.org/wandbox/permlink/CcvL0BHfVJhHZH4M
// http://melpon.org/wandbox/permlink/ExQoromITFQ7WOxO
// http://melpon.org/wandbox/permlink/YiLtKIriWtkigZs8

int test1() {
  //namespace bnu = boost::numeric::ublas;
  //using Vec = bnu::vector<double>;
  //using Mat = xt::xarray<double, xt::layout_type::row_major>;
  using Vec = xt::xarray<double, xt::layout_type::row_major>;

  double p = 20, A = 40, alpha = 0.1, beta = 0.4;
  double v1 = 10, v2 = 35, k = 30.5;
  double fb;
  int niter, status;
  bool feasible;

  {
    ell E(100., Vec{0., 0.});
    profit_oracle P(p, A, alpha, beta, v1, v2, k);
    std::tie(std::ignore, fb, niter, feasible, status) =
        cutting_plane_dc(P, E, 0.);
    // fmt::print("{:f} {} {} {} \n", fb, niter, feasible, status);
    std::cout << fb << ", " << niter << ", " << feasible << ", " << status << "\n";
  }

  double ui = 1., e1 = 0.003, e2 = 0.007, e3 = 1.;

  {
    ell E1(100., Vec{0., 0.});
    profit_rb_oracle P1(p, A, alpha, beta, v1, v2, k, ui, e1, e2, e3);
    std::tie(std::ignore, fb, niter, feasible, status) =
        cutting_plane_dc(P1, E1, 0.);
    // fmt::print("{:f} {} {} {} \n", fb, niter, feasible, status);
    std::cout << fb << ", " << niter << ", " << feasible << ", " << status
              << "\n";
  }

  {
    ell E2(100., Vec{2., 0.});
    profit_q_oracle P2(p, A, alpha, beta, v1, v2, k);
    std::tie(std::ignore, fb, niter, feasible, status) =
        cutting_plane_q(P2, E2, 0.);
    // fmt::print("{:f} {} {} {} \n", fb, niter, feasible, status);
    std::cout << fb << ", " << niter << ", " << feasible << ", " << status
             << "\n";
  }

  return 0;
}

int main() {
  using Arr = xt::xarray<double, xt::layout_type::row_major>;
  using xt::placeholders::_;
  using xt::linalg::dot;

  auto m1 = Arr({{25., 15., -5.},
                {15., 18.,  0.},
                {-5.,  0., 11.}});
  std::cout << m1.shape()[0] << "\n";
  auto Q1 = ldlt_ext(m1.shape()[0]);
  Q1.factorize(m1);
  if (!Q1.is_sd()) {
    auto v = Q1.witness();
    auto p = v.size();
    auto sub = xt::range(0, p);
    Arr App = xt::view(m1, sub, sub);
    Arr Appv = dot(App, v);
    auto fj = -dot(v, Appv)();
    std::cout << fj << std::endl;
  }

  auto m2 = Arr({{18., 22.,  54.,  42.},
                {22., -70.,  86.,  62.},
                {54., 86., -174., 134.},
                {42., 62., 134., -106.}});
  std::cout << m2.shape()[0] << "\n";
  auto Q2 = ldlt_ext(m2.shape()[0]);
  Q2.factorize(m2);
  if (!Q2.is_sd()) {
    auto v = Q2.witness();
    auto p = v.size();
    auto sub = xt::range(0, p);
    Arr App = xt::view(m2, sub, sub);
    Arr Appv = dot(App, v);
    auto fj = -dot(v, Appv)();
    std::cout << fj << std::endl;
  }

  return 0;
}


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

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

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

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

// Critical temperature of hexagonal lattice Ising model

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/differentiation/autodiff.hpp>
#include <standards/newton.hpp>

namespace {

template<typename T>
struct func {
  func(T Ja, T Jb, T Jc) : Ja_(Ja), Jb_(Jb), Jc_(Jc) {}
  auto operator()(T beta) const -> decltype(boost::math::differentiation::make_fvar<T, 2>(beta)) {
    using std::tanh;
    auto beta_fvar = boost::math::differentiation::make_fvar<T, 2>(beta);
    auto za = tanh(beta_fvar * Ja_);
    auto zb = tanh(beta_fvar * Jb_);
    auto zc = tanh(beta_fvar * Jc_);
    return za * zb + zb * zc + zc * za - 1;
  }
  T Ja_, Jb_, Jc_;
};

}

namespace ising {
namespace tc {

template<typename T>
inline T hexagonal(T Ja, T Jb, T Jc) {
  Ja = abs(Ja);
  Jb = abs(Jb);
  Jc = abs(Jc);
  if (Ja * Jb * Jc <= 0) throw(std::invalid_argument("Ja * Jb * Jc should be non-zero"));
  auto result = standards::newton_1d(func<T>(Ja, Jb, Jc), 1 / (Ja + Jb + Jc));
  if (!result.second) throw(std::runtime_error("convergence error"));
  return 1 / result.first;
}

} // end namespace tc
} // end namespace ising


#include <boost/numeric/odeint.hpp>
#include "stochastic_euler.hpp"
#include "cahnhilliard.h"
#include "cahnhilliard_thermal.h"
#include "cahnhilliard_thermal_nodiffusion.h"
#include "run_ch_solver.h"

void run_ch_solver_non_thermal( CHparamsVector& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS rhs = CahnHilliard2DRHS( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_thermal_no_diffusion( CHparamsVector& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS_thermal_nodiffusion rhs = CahnHilliard2DRHS_thermal_nodiffusion( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_thermal_with_diffusion( CHparamsVector& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS_thermal rhs = CahnHilliard2DRHS_thermal( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_non_thermal( CHparamsScalar& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS rhs = CahnHilliard2DRHS( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_thermal_no_diffusion( CHparamsScalar& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS_thermal_nodiffusion rhs = CahnHilliard2DRHS_thermal_nodiffusion( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver_thermal_with_diffusion( CHparamsScalar& chparams , SimInfo& info )
{

  // Instantiate rhs
  CahnHilliard2DRHS_thermal rhs = CahnHilliard2DRHS_thermal( chparams , info );

  std::vector<double> x;
  if (info.t0 == 0) {
    rhs.setInitialConditions(x);
    int iter = 0;
  }
  else {
    x        = info.x;
    int iter = info.iter;
  }

  // define adaptive stepper
  typedef boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double>> error_stepper_type;

  // define runge kutta
  typedef boost::numeric::odeint::controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

  controlled_stepper_type controlled_stepper;

  const double stability_limit = chparams.compute_stability_limit(info.dx , info.dy); // just an estimate
  const double res0            = rhs.l2residual(x);

  std::cout << "residual at initial condition: " << res0 << std::endl;
  if (info.iter == 0)
    rhs.write_state(x,0,info.nx,info.ny,info.outdir);

  if (chparams.sigma_noise < 1e-2) {
    std::cout << "Solving deterministic (noise-free) CH" << std::endl;
    integrate_adaptive(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
    //boost::numeric::odeint::integrate_const(controlled_stepper, rhs, x, info.t0, info.tf, stability_limit/2.);
  }
  else {
    std::cout << "Solving stochastic CH" << std::endl;
    boost::mt19937 rng;
    boost::numeric::odeint::integrate_const( stochastic_euler() ,
		     std::make_pair( rhs , ornstein_stoch( rng , chparams.sigma_noise ) ),
		     x , info.t0 , info.tf , stability_limit/40. );
  }
  info.iter += 1;
  std::cout << "iter: " << info.iter << " , t = " << info.tf << ", relative residual: " << rhs.l2residual(x) / res0 << std::endl;
  rhs.write_state(x,info.iter,info.nx,info.ny,info.outdir);
  info.x = x;

};

void run_ch_solver( CHparamsVector& chparams , SimInfo& info )
{

  if      (info.rhs_type.compare("ch_non_thermal") == 0) {
    run_ch_solver_non_thermal( chparams , info );
  }
  else if (info.rhs_type.compare("ch_thermal_no_diffusion") == 0) {
    run_ch_solver_thermal_no_diffusion( chparams , info );
  }
  else if (info.rhs_type.compare("ch_thermal_with_diffusion") == 0) {
    run_ch_solver_thermal_with_diffusion( chparams , info );
  }

};

void run_ch_solver( CHparamsScalar& chparams , SimInfo& info )
{

  if      (info.rhs_type.compare("ch_non_thermal") == 0) {
    run_ch_solver_non_thermal( chparams , info );
  }
  else if (info.rhs_type.compare("ch_thermal_no_diffusion") == 0) {
    run_ch_solver_thermal_no_diffusion( chparams , info );
  }
  else if (info.rhs_type.compare("ch_thermal_with_diffusion") == 0) {
    run_ch_solver_thermal_with_diffusion( chparams , info );
  }

};


/*    Copyright (c) 2010-2016, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120203    B. Tong Minh      Copied RungeKutta4Stepsize unit test.
 *      120207    K. Kumar          Adapted to use modified benchmark functions in Tudat Core.
 *      120213    K. Kumar          Modified getCurrentInterval( ) to getIndependentVariable( );
 *                                  transferred to Boost unit test framework.
 *      120321    K. Kumar          Updated (Burden and Faires, 2011) benchmark function call.
 *      120323    K. Kumar          Rewrote unit tests to use benchmark data from
 *                                  (Burden and Faires, 2011); removed test against benchmark
 *                                  functions; renamed file to RFK45 unit test. Other unit tests
 *                                  for other Runge-Kutta methods will appear in other dedicated
 *                                  unit test files.
 *      120327    K. Kumar          Added missing comments; added unit test based on output data
 *                                  generated by (The Mathworks, 2012).
 *      120328    K. Kumar          Moved (Burden and Faires, 2011) test class to its own file;
 *                                  modified MATLAB unit tests to test forward and backwards in
 *                                  time and "forced" and "free" adaptive step size adjustment and
 *                                  moved to separate file (added call to function to run Matlab
 *                                  tests); added rollback tests for all cases.
 *      120404    K. Kumar          Updated Matlab unit test by adding discrete-event data file.
 *      130118    K. Kumar          Rewrote unit test to make use of testing code for numerical
 *                                  integrators migrated to Tudat Core.
 *      130906    K. Kumar          Updated error tolerances for MuPAD-based tests.
 *
 *      160321    R. Hoogendoorn    Created Runge Kutta 56 test
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *      Montenbruck, O., Gill, E. Satellite Orbits: Models, Methods, Applications, Springer, 2005.
 *      The MathWorks, Inc. RKF54b, Symbolic Math Toolbox, 2012.
 *
 *    Notes
 *      For the tests using data from the Symbolic Math Toolbox (MathWorks, 2012), the single step
 *      and full integration error tolerances were picked to be as small as possible, without
 *      causing the tests to fail. These values are not deemed to indicate any bugs in the code;
 *      however, it is important to take these discrepancies into account when using this numerical
 *      integrator.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <string>

#include <cmath>

#include <Eigen/Core>

#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_runge_kutta_fehlberg_56_integrator )

using numerical_integrator_test_functions::computeNonAutonomousModelStateDerivative;
using numerical_integrator_test_functions::computeVanDerPolStateDerivative;
using numerical_integrator_test_functions::computeFehlbergLogirithmicTestODEStateDerivative;
using numerical_integrator_test_functions::computeAnalyticalStateFehlbergODE;

using namespace numerical_integrators;

//! Compare with analytical solution of Fehlberg
BOOST_AUTO_TEST_CASE( test_RungeKuttaFehlberg56_Integrator_Fehlberg_Benchmark )
{
    RungeKuttaCoefficients coeff56 =
        RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg56);

    // Integrator settings
    double minimumStepSize   = std::numeric_limits< double >::epsilon( );
    double maximumStepSize   = std::numeric_limits< double >::infinity( );
    double initialStepSize   = 1E-6; // Don't make this too small
    double relativeTolerance = 1E-16;
    double absoluteTolerance = 1E-16;

    // Initial conditions
    double initialTime = 0.0;
    double finalTime   = 5.0;
    Eigen::Vector2d initialState( exp( 1.0 ), 1.0);

    // Setup integrator
    RungeKuttaVariableStepSizeIntegratorXd integrator56(
                coeff56, computeFehlbergLogirithmicTestODEStateDerivative,
                initialTime, initialState, minimumStepSize,
                maximumStepSize, relativeTolerance, absoluteTolerance );

    // Obtain numerical solution
    Eigen::Vector2d numericalSolution = integrator56.integrateTo( finalTime, initialStepSize );

    // Analytical solution
    // (page 30, Fehlberg, E. (1968). Classical Fifth-, Sixth-, Seventh- and Eigth-Order Runge-Kutta
    // Formulas with Stepsize Control)
    Eigen::Vector2d analyticalSolution =
        computeAnalyticalStateFehlbergODE( finalTime, initialState );

    Eigen::Vector2d computedError = numericalSolution - analyticalSolution;
    BOOST_CHECK_SMALL( std::fabs( computedError( 0 ) ), 1E-12 );
    BOOST_CHECK_SMALL( std::fabs( computedError( 1 ) ), 1E-12 );

    // Error calculated by -> Fehlberg, E. (1968) page 30
    // Initial stepsize unknown..
    Eigen::VectorXd fehlbergError( 2 );
    fehlbergError << 0.1072E-12, -0.2190E-12;

    // Sign check
    // Not always same sign -> initial step size = 1 or 1E-2, failure: computedError( 1 ),
    // fehlbergError( 1 ) not same sign
    BOOST_CHECK_GE( computedError( 0 ) / fehlbergError( 0 ), 0.0 );
    BOOST_CHECK_GE( computedError( 1 ) / fehlbergError( 1 ), 0.0 );

    // Check error is similar in magnitude
    BOOST_CHECK_SMALL( std::fabs( computedError( 0 ) / fehlbergError( 0 ) ), 3.0);
    BOOST_CHECK_SMALL( std::fabs( computedError( 1 ) / fehlbergError( 1 ) ), 1.0);
}

//! Test Compare with Runge Kutta 78
BOOST_AUTO_TEST_CASE( test_RungeKuttaFehlberg56_Integrator_Compare78 )
{
    // Setup integrator
    RungeKuttaCoefficients coeff56 =
            RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg56);

    RungeKuttaCoefficients coeff78 =
            RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78);

    // Integrator settings
    double minimumStepSize = std::numeric_limits< double >::epsilon( );
    double maximumStepSize = std::numeric_limits< double >::infinity( );
    double initialStepSize = 1E-4; // Don't make this too small
    double relativeTolerance = 1E-15;
    double absoluteTolerance = 1E-15;

    // Initial conditions
    double initialTime = 0.5;
    Eigen::VectorXd InitialState( 1 );
    InitialState << 0.5; // 1 large error

    // Setup integrator
    RungeKuttaVariableStepSizeIntegratorXd integrator56(
                coeff56, computeNonAutonomousModelStateDerivative, initialTime, InitialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    RungeKuttaVariableStepSizeIntegratorXd integrator78(
                coeff78, computeNonAutonomousModelStateDerivative, initialTime, InitialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    double endTime = 1.5;
    Eigen::VectorXd solution56 = integrator56.integrateTo( endTime, initialStepSize );
    Eigen::VectorXd solution78 = integrator78.integrateTo( endTime, initialStepSize );

    Eigen::VectorXd difference = solution78 - solution56;

    BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 1E-13 );
}

//! Test Compare with Runge Kutta 78
BOOST_AUTO_TEST_CASE( test_RungeKuttaFehlberg56_Integrator_Compare78_v2 )
{
    // Setup integrator
    RungeKuttaCoefficients coeff56 =
            RungeKuttaCoefficients::get(
                RungeKuttaCoefficients::rungeKuttaFehlberg56 );

    RungeKuttaCoefficients coeff78 =
            RungeKuttaCoefficients::get(
                RungeKuttaCoefficients::rungeKuttaFehlberg78 );

    // Integrator settings
    double minimumStepSize = std::numeric_limits< double >::epsilon( );
    double maximumStepSize = std::numeric_limits< double >::infinity( );
    double initialStepSize = 1.0; // Don't make this too small
    double relativeTolerance = 1E-10;
    double absoluteTolerance = 1E-10;

    // Initial conditions
    double initialTime = 0.2;
    Eigen::VectorXd InitialState( 1 );
    InitialState << -1.0;

    // Setup integrator
    RungeKuttaVariableStepSizeIntegratorXd integrator56(
                coeff56, computeNonAutonomousModelStateDerivative, initialTime, InitialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    RungeKuttaVariableStepSizeIntegratorXd integrator78(
                coeff78, computeNonAutonomousModelStateDerivative, initialTime, InitialState,
                minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance );

    double endTime = 2.0;
    Eigen::VectorXd solution56 = integrator56.integrateTo( endTime, initialStepSize );
    Eigen::VectorXd solution78 = integrator78.integrateTo( endTime, initialStepSize );

    Eigen::VectorXd difference = solution78 - solution56;

    BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 1E-8 );
}

//! Test Compare with Runge Kutta 78
BOOST_AUTO_TEST_CASE( test_RungeKuttaFehlberg56_Integrator_Compare78_VanDerPol )
{
    // Setup integrator
    RungeKuttaCoefficients coeff56 =
            RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg56 );

    RungeKuttaCoefficients coeff78 =
            RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 );

    // Integrator settings
    double minimumStepSize = std::numeric_limits< double >::epsilon( );
    double maximumStepSize = std::numeric_limits< double >::infinity( );
    double initialStepSize = 1; // Don't make this too small
    double relativeTolerance = 1E-15;
    double absoluteTolerance = 1E-15;

    // Initial conditions
    double initialTime = 0.2;
    Eigen::VectorXd InitialState( 2 );
    InitialState << -1.0, 1.0;

    // Setup integrator
    RungeKuttaVariableStepSizeIntegratorXd integrator56(
                coeff56, computeVanDerPolStateDerivative, initialTime, InitialState, minimumStepSize,
                maximumStepSize, relativeTolerance, absoluteTolerance );

    RungeKuttaVariableStepSizeIntegratorXd integrator78(
                coeff78, computeVanDerPolStateDerivative, initialTime, InitialState, minimumStepSize,
                maximumStepSize, relativeTolerance, absoluteTolerance );

    double endTime = 1.4;
    Eigen::VectorXd solution56 = integrator56.integrateTo(endTime,initialStepSize);
    Eigen::VectorXd solution78 = integrator78.integrateTo(endTime,initialStepSize);

    Eigen::VectorXd difference = solution78 - solution56;

    BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 1E-13 );
    BOOST_CHECK_SMALL( std::fabs( difference( 1 ) ), 1E-13 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


//
//  eigen_utils.hpp
//
//  Created By Davis Blalock on 3/2/16.
//  Copyright (c) 2016 Davis Blalock. All rights reserved.
//

#ifndef __EIGEN_UTILS_HPP
#define __EIGEN_UTILS_HPP

#define EIGEN_DONT_PARALLELIZE // ensure no multithreading

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

// ================================================================
// typealiases
// ================================================================

template <class T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
using RowMatrix = Eigen::Matrix<T, Rows, Cols, Eigen::RowMajor>;

template <class T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
using ColMatrix = Eigen::Matrix<T, Rows, Cols, Eigen::ColMajor>;

template <class T> using ColVector = Eigen::Matrix<T, Eigen::Dynamic, 1>;

template <class T>
using RowVector = Eigen::Matrix<T, 1, Eigen::Dynamic, Eigen::RowMajor>;

#endif


/**
 * \file boost/numeric/ublasx/operation/rcond.hpp
 *
 * \brief Matrix reciprocal condition number estimate.
 *
 * The condition number of a regular (square) matrix is the product
 * of the \e norm of the matrix and the norm of its inverse (or
 * pseudo-inverse), and hence depends on the kind of matrix-norm.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_RCOND_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_RCOND_HPP

#include <algorithm>
#include <boost/numeric/bindings/lapack/auxiliary/lange.hpp>
#include <boost/numeric/bindings/lapack/computational/gbcon.hpp>
#include <boost/numeric/bindings/lapack/computational/gbtrf.hpp>
#include <boost/numeric/bindings/lapack/computational/gecon.hpp>
#include <boost/numeric/bindings/lapack/computational/getrf.hpp>
#include <boost/numeric/bindings/lapack/computational/hecon.hpp>
#include <boost/numeric/bindings/lapack/computational/hetrf.hpp>
#include <boost/numeric/bindings/lapack/computational/sycon.hpp>
#include <boost/numeric/bindings/lapack/computational/sytrf.hpp>
#include <boost/numeric/bindings/lapack/computational/trcon.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/qr.hpp>
#include <stdexcept>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

enum matrix_norm_category
{
	matrix_norm_1,
	matrix_norm_2,
	matrix_norm_frobenius,
	matrix_norm_inf
};

//template <
//	typename ValueT,
//	typename LayoutT,
//	typename StorageT
//>
//ValueT* band_mat_to_lapack_vec(banded_matrix<ValueT,LayoutT,StorageT> const& A, ::fortran_int_t& n)
//{
//	typedef banded_matrix<ValueT,LayoutT,StorageT> matrix_type;
//	typedef typename matrix_traits<matrix_type>::size_type size_type;
//
//	const size_type kl = A.lower();
//	const size_type ku = A.upper();
//	const size_type nr = num_rows(A);
//	const size_type nc = num_columns(A);
//
//	n = (2*kl+ku+1)*nc;
//
//	ValueT* v = new ValueT[n];
//
//	for (size_type r = 0; r < nr; ++r)
//	{
//		for (size_type c = 0; c < nc; ++c)
//		{
//			size_type rr = kl+ku+r-c;
//			size_type k = (2*kl+ku+1)*c+rr;
//
//			if ((r == c) || (c <= (ku+r) && r <= (c+kl)))
//			{
//				// In band
//				v[k] = A(r,c);
//			}
//			else
//			{
//				// Out of band
//				v[k] = ValueT();
//			}
//		}
//	}
//
//	return v;
//}

//template <typename T>
//void print_vector(std::string const& desc, T const* a, std::size_t n)
//{
//	std::cout << std::endl << desc << std::endl << "  [";
//	for(std::size_t i = 0; i < n; ++i)
//	{
//		std::cout << " " << std::fixed << a[i];
//	}
//	std::cout << "]" << std::endl;
//}

template <typename MatrixT>
typename type_traits<
	typename matrix_traits<MatrixT>::value_type
>::real_type rcond_impl(MatrixT const& A, matrix_norm_category norm_category, column_major_tag)
{
    typedef typename matrix_traits<MatrixT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef typename matrix_traits<MatrixT>::size_type size_type;
	typedef matrix<value_type, column_major> work_matrix_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);
	size_type k = ::std::min(nr,nc);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(false), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(false), norm_category, column_major_tag());
		}
	}

	char what_norm;
	result_type norm;
	result_type res;

	//FIXME: actually, in bindings this function is broken
//	switch (norm_category)
//	{
//		case matrix_norm_1:
//			what_norm = 'O';
//			break;
//		case matrix_norm_inf:
//			what_norm = 'I';
//			break;
////		case matrix_norm_frobenius:
////			what_norm = 'F';
////			break;
//		default:
//			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
//	}

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			what_norm = 'O';
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			what_norm = 'I';
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
	work_matrix_type tmp_LU(A);
	vector< ::fortran_int_t > dummy_ipiv(k);
	::boost::numeric::bindings::lapack::getrf(
		tmp_LU,
		dummy_ipiv
	);
	dummy_ipiv.resize(0, false); // free memory

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::gecon(
		what_norm,
		tmp_LU,
		norm,
		res
	);

	return res;
}

template <typename MatrixT>
typename type_traits<
	typename matrix_traits<MatrixT>::value_type
>::real_type rcond_impl(MatrixT const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef matrix<
				typename matrix_traits<MatrixT>::value_type,
				column_major
			> work_matrix_type;

	work_matrix_type tmp_A(A);

	return rcond_impl(tmp_A, norm_category, column_major_tag());
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(triangular_matrix<ValueT,TriangularT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef triangular_matrix<ValueT,TriangularT,column_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef matrix<value_type,column_major> auxiliary_matrix_type;
	typedef triangular_adaptor<auxiliary_matrix_type, TriangularT> work_matrix_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	char what_norm;
	result_type res;

	switch (norm_category)
	{
		case matrix_norm_1:
			what_norm = 'O';
			break;
		case matrix_norm_inf:
			what_norm = 'I';
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	auxiliary_matrix_type aux_A(A);
	work_matrix_type tmp_A(aux_A);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::trcon(
		what_norm,
		tmp_A,
		res
	);

	return res;
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(triangular_matrix<ValueT,TriangularT,row_major,StorageT> const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef triangular_matrix<ValueT,TriangularT,column_major,StorageT> work_matrix_type;

	work_matrix_type tmp_A(A);

	return rcond_impl(tmp_A, norm_category, column_major_tag());
}

/*
template <
	typename ValueT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(banded_matrix<ValueT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef banded_matrix<ValueT,column_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	//typedef matrix<value_type,LayoutT> auxiliary_matrix_type;
	//typedef banded_adaptor<auxiliary_matrix_type> work_matrix_type;
	typedef banded_matrix<ValueT,row_major,StorageT> work_matrix_type;
	typedef vector< ::fortran_int_t > vector_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	char what_norm;
	result_type norm;
	result_type res;
	size_type k = ::std::min(nr,nc);
	size_type kl = A.lower();
	size_type ku = A.upper();
//	size_type ldab = 2*kl+ku+1;

	switch (norm_category)
	{
		case matrix_norm_1:
			what_norm = 'O';
			break;
		case matrix_norm_inf:
			what_norm = 'I';
			break;
//		case matrix_norm_frobenius:
//			what_norm = 'F';
//			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
//	::fortran_int_t* aux_ab = band_mat_to_lapack_vec(A, n);
//	vector< ::fortran_int_t> tmp_ipiv(k);
//	::std::ptrdiff_t info;
//	info = ::boost::numeric::bindings::lapack::detail::gbtrf(
//		nr,
//		nc,
//		kl,
//		ku,
//		aux_ab,
//		ldab,
//		tmp_ipiv.data().begin()
//	);
	work_matrix_type AB(A, kl, kl+ku); //NOTE: "kl+ku" is not a typo
	vector_type ipiv(k);
	::boost::numeric::bindings::lapack::gbtrf(AB, ipiv);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::gbcon(
		what_norm,
		AB,
		ipiv,
		norm,
		res
	);

	return res;
}
*/

template <
	typename ValueT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(banded_matrix<ValueT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef banded_matrix<ValueT,row_major,StorageT> work_matrix_type;

	work_matrix_type tmp_A(A, A.lower(), A.upper());

	return rcond_impl(tmp_A, norm_category, row_major_tag());
}

template <
	typename ValueT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(banded_matrix<ValueT,row_major,StorageT> const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef banded_matrix<ValueT,row_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef banded_matrix<ValueT,row_major,StorageT> work_matrix_type;
	typedef vector< ::fortran_int_t > vector_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	char what_norm;
	result_type norm;
	result_type res;
	size_type k = ::std::min(nr,nc);
	size_type kl = A.lower();
	size_type ku = A.upper();
//	size_type ldab = 2*kl+ku+1;

	switch (norm_category)
	{
		case matrix_norm_1:
			what_norm = 'O';
			break;
		case matrix_norm_inf:
			what_norm = 'I';
			break;
//		case matrix_norm_frobenius:
//			what_norm = 'F';
//			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
//	::fortran_int_t* aux_ab = band_mat_to_lapack_vec(A, n);
//	vector< ::fortran_int_t> tmp_ipiv(k);
//	::std::ptrdiff_t info;
//	info = ::boost::numeric::bindings::lapack::detail::gbtrf(
//		nr,
//		nc,
//		kl,
//		ku,
//		aux_ab,
//		ldab,
//		tmp_ipiv.data().begin()
//	);
	work_matrix_type AB(A, kl, kl+ku); //NOTE: "kl+ku" is not a typo
	vector_type ipiv(k);
	::boost::numeric::bindings::lapack::gbtrf(AB, ipiv);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::gbcon(
		what_norm,
		AB,
		ipiv,
		norm,
		res
	);

	return res;
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(symmetric_matrix<ValueT,TriangularT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef symmetric_matrix<ValueT,TriangularT,column_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef matrix<ValueT,column_major> auxiliary_matrix_type;
	typedef symmetric_adaptor<auxiliary_matrix_type,TriangularT> work_matrix_type;
	typedef vector< ::fortran_int_t > vector_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);
	size_type n = ::std::min(nr, nc);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	result_type norm;
	result_type res;

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
	//work_matrix_type AB(A);
	auxiliary_matrix_type aux_A(A);
	work_matrix_type AB(aux_A);
	vector_type ipiv(n);
	::boost::numeric::bindings::lapack::sytrf(AB, ipiv);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::sycon(
		AB,
		ipiv,
		norm,
		res
	);

	return res;
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(symmetric_matrix<ValueT,TriangularT,row_major,StorageT> const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef symmetric_matrix<ValueT,TriangularT,column_major,StorageT> work_matrix_type;

	work_matrix_type tmp_A(A);

	return rcond_impl(tmp_A, norm_category, column_major_tag());
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(hermitian_matrix<ValueT,TriangularT,column_major,StorageT> const& A, matrix_norm_category norm_category, column_major_tag)
{
	typedef hermitian_matrix<ValueT,TriangularT,column_major,StorageT> matrix_type;
    typedef typename matrix_traits<matrix_type>::value_type value_type;
	typedef typename matrix_traits<matrix_type>::size_type size_type;
	typedef typename type_traits<value_type>::real_type result_type;
	typedef matrix<ValueT,column_major> auxiliary_matrix_type;
	typedef hermitian_adaptor<auxiliary_matrix_type,TriangularT> work_matrix_type;
	typedef vector< ::fortran_int_t > vector_type;

	size_type nr = num_rows(A);
	size_type nc = num_columns(A);
	size_type n = ::std::min(nr, nc);

	// Check if A is a square matrix
	if (nr != nc)
	{
		// Non-square matrix -> Use QR decomposition
		if (nr < nc)
		{
			return rcond_impl(qr_decompose(trans(A)).R(), norm_category, column_major_tag());
		}
		else
		{
			return rcond_impl(qr_decompose(A).R(), norm_category, column_major_tag());
		}
	}

	result_type norm;
	result_type res;

	// Compute the norm of A
	//FIXME: actually, in bindings this function is broken
//	::boost::numeric::bindings::lapack::lange(
//		what_norm,
//		A
//	);
	switch (norm_category)
	{
		case matrix_norm_1:
			norm = norm_1(A);
			break;
		case matrix_norm_inf:
			norm = norm_inf(A);
			break;
		default:
			throw std::runtime_error("[rcond::detail::rcond_impl] Unsupported norm category.");
	}

	// Compute the LUP factorization of A
	auxiliary_matrix_type aux_A(A);
	work_matrix_type AB(aux_A);
	vector_type ipiv(n);
	::boost::numeric::bindings::lapack::hetrf(AB, ipiv);

	// Finally, compute the reciprocal condition number
	::boost::numeric::bindings::lapack::hecon(
		AB,
		ipiv,
		norm,
		res
	);

	return res;
}

template <
	typename ValueT,
	typename TriangularT,
	typename StorageT
>
typename type_traits<ValueT>::real_type rcond_impl(hermitian_matrix<ValueT,TriangularT,row_major,StorageT> const& A, matrix_norm_category norm_category, row_major_tag)
{
	typedef hermitian_matrix<ValueT,TriangularT,column_major,StorageT> work_matrix_type;

	work_matrix_type tmp_A(A);

	return rcond_impl(tmp_A, norm_category, column_major_tag());
}

} // Namespace detail

/**
 * \brief Matrix reciprocal condition number estimate based on 1-norm.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param A The input \e square matrix expression.
 * \return The estimate of the reciprocal condition number of \a A.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
typename type_traits<
	typename matrix_traits<MatrixExprT>::value_type
>::real_type rcond(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category;

	return detail::rcond_impl(A(), detail::matrix_norm_1, orientation_category());
}

//FIXME: Does we also need this?
///**
// * \brief Matrix reciprocal condition number estimate based on the matrix norm
// *  defined by parameter \a what_norm.
// *
// * \tparam MatrixExprT The type of the input matrix expression.
// * \tparam NormTagT The type of the matrix norm category.
// *
// * \param A The input \e square matrix expression.
// * \param what_norm The matrix norm category.
// * \return The estimate of the reciprocal condition number of \a A.
// *
// * \author Marco Guazzone, marco.guazzone@gmail.com
// */
//template <typename MatrixExprT, typename NormTagT>
//typename type_traits<
//	typename matrix_traits<MatrixExprT>::value_type
//>::real_type rcond(matrix_expression<MatrixExprT> const& A, NormTagT what_norm)
//{
//	typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category;
//
//	return detail::rcond_impl(A(), detail::matrix_norm_1, orientation_category());
//}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_RCOND_HPP


/**
 * @file distribution_test.cpp
 * @author Ryan Curtin
 * @author Yannis Mentekidis
 *
 * Tests for the classes:
 *  * mlpack::distribution::DiscreteDistribution
 *  * mlpack::distribution::GaussianDistribution
 *  * mlpack::distribution::GammaDistribution
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::distribution;
using namespace mlpack::math;

BOOST_AUTO_TEST_SUITE(DistributionTest);

/*********************************/
/** Discrete Distribution Tests **/
/*********************************/

/**
 * Make sure we initialize correctly.
 */
BOOST_AUTO_TEST_CASE(DiscreteDistributionConstructorTest)
{
  DiscreteDistribution d(5);

  BOOST_REQUIRE_EQUAL(d.Probabilities().n_elem, 5);
  BOOST_REQUIRE_CLOSE(d.Probability("0"), 0.2, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("1"), 0.2, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("2"), 0.2, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("3"), 0.2, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("4"), 0.2, 1e-5);
}

/**
 * Make sure we get the probabilities of observations right.
 */
BOOST_AUTO_TEST_CASE(DiscreteDistributionProbabilityTest)
{
  DiscreteDistribution d(5);

  d.Probabilities() = "0.2 0.4 0.1 0.1 0.2";

  BOOST_REQUIRE_CLOSE(d.Probability("0"), 0.2, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("1"), 0.4, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("2"), 0.1, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("3"), 0.1, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("4"), 0.2, 1e-5);
}

/**
 * Make sure we get random observations correct.
 */
BOOST_AUTO_TEST_CASE(DiscreteDistributionRandomTest)
{
  DiscreteDistribution d(arma::Col<size_t>("3"));

  d.Probabilities() = "0.3 0.6 0.1";

  arma::vec actualProb(3);

  actualProb.zeros();

  for (size_t i = 0; i < 50000; i++)
    actualProb((size_t) (d.Random()[0] + 0.5))++;

  // Normalize.
  actualProb /= accu(actualProb);

  // 8% tolerance, because this can be a noisy process.
  BOOST_REQUIRE_CLOSE(actualProb(0), 0.3, 8.0);
  BOOST_REQUIRE_CLOSE(actualProb(1), 0.6, 8.0);
  BOOST_REQUIRE_CLOSE(actualProb(2), 0.1, 8.0);
}

/**
 * Make sure we can estimate from observations correctly.
 */
BOOST_AUTO_TEST_CASE(DiscreteDistributionTrainTest)
{
  DiscreteDistribution d(4);

  arma::mat obs("0 0 1 1 2 2 2 3");

  d.Train(obs);

  BOOST_REQUIRE_CLOSE(d.Probability("0"), 0.25, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("1"), 0.25, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("2"), 0.375, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("3"), 0.125, 1e-5);
}

/**
 * Estimate from observations with probabilities.
 */
BOOST_AUTO_TEST_CASE(DiscreteDistributionTrainProbTest)
{
  DiscreteDistribution d(3);

  arma::mat obs("0 0 1 2");

  arma::vec prob("0.25 0.25 0.5 1.0");

  d.Train(obs, prob);

  BOOST_REQUIRE_CLOSE(d.Probability("0"), 0.25, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("1"), 0.25, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("2"), 0.5, 1e-5);
}

/**
 * Achieve multidimensional probability distribution.
 */
BOOST_AUTO_TEST_CASE(MultiDiscreteDistributionTrainProbTest)
{
  DiscreteDistribution d("10 10 10");

  arma::mat obs("0 1 1 1 2 2 2 2 2 2;"
                "0 0 0 1 1 1 2 2 2 2;"
                "0 0 0 1 1 2 2 2 2 2;");

  d.Train(obs);
  BOOST_REQUIRE_CLOSE(d.Probability("0 0 0"), 0.009, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("0 1 2"), 0.015, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("2 1 0"), 0.054, 1e-5);
}

/**
 * Make sure we initialize multidimensional probability distribution
 * correctly.
 */
BOOST_AUTO_TEST_CASE(MultiDiscreteDistributionConstructorTest)
{
  DiscreteDistribution d("4 4 4 4");

  BOOST_REQUIRE_EQUAL(d.Probabilities(0).size(), 4);
  BOOST_REQUIRE_EQUAL(d.Dimensionality(), 4);
  BOOST_REQUIRE_CLOSE(d.Probability("0 0 0 0"), 0.00390625, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("0 1 2 3"), 0.00390625, 1e-5);
}

/**
 * Achieve multidimensional probability distribution.
 */
BOOST_AUTO_TEST_CASE(MultiDiscreteDistributionTrainTest)
{
  std::vector<arma::vec> pro;
  pro.push_back(arma::vec("0.1, 0.3, 0.6"));
  pro.push_back(arma::vec("0.3, 0.3, 0.3"));
  pro.push_back(arma::vec("0.25, 0.25, 0.5"));

  DiscreteDistribution d(pro);

  BOOST_REQUIRE_CLOSE(d.Probability("0 0 0"), 0.0083333, 1e-3);
  BOOST_REQUIRE_CLOSE(d.Probability("0 1 2"), 0.0166666, 1e-3);
  BOOST_REQUIRE_CLOSE(d.Probability("2 1 0"), 0.05, 1e-5);
}

/**
 * Estimate multidimensional probability distribution from observations with
 * probabilities.
 */
BOOST_AUTO_TEST_CASE(MultiDiscreteDistributionTrainProTest)
{
  DiscreteDistribution d("5 5 5");

  arma::mat obs("0 0 1 1 2;"
                "0 1 1 2 2;"
                "0 1 1 2 2");

  arma::vec prob("0.25 0.25 0.25 0.25 1");

  d.Train(obs, prob);

  BOOST_REQUIRE_CLOSE(d.Probability("0 0 0"), 0.00390625, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("1 0 1"), 0.0078125, 1e-5);
  BOOST_REQUIRE_CLOSE(d.Probability("2 1 0"), 0.015625, 1e-5);
}

/*********************************/
/** Gaussian Distribution Tests **/
/*********************************/

/**
 * Make sure Gaussian distributions are initialized correctly.
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionEmptyConstructor)
{
  GaussianDistribution d;

  BOOST_REQUIRE_EQUAL(d.Mean().n_elem, 0);
  BOOST_REQUIRE_EQUAL(d.Covariance().n_elem, 0);
}

/**
 * Make sure Gaussian distributions are initialized to the correct
 * dimensionality.
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionDimensionalityConstructor)
{
  GaussianDistribution d(4);

  BOOST_REQUIRE_EQUAL(d.Mean().n_elem, 4);
  BOOST_REQUIRE_EQUAL(d.Covariance().n_rows, 4);
  BOOST_REQUIRE_EQUAL(d.Covariance().n_cols, 4);
}

/**
 * Make sure Gaussian distributions are initialized correctly when we give a
 * mean and covariance.
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionDistributionConstructor)
{
  arma::vec mean(3);
  arma::mat covariance(3, 3);

  mean.randu();
  covariance.randu();
  covariance *= covariance.t();
  covariance += arma::eye<arma::mat>(3, 3);

  GaussianDistribution d(mean, covariance);

  for (size_t i = 0; i < 3; i++)
    BOOST_REQUIRE_CLOSE(d.Mean()[i], mean[i], 1e-5);

  for (size_t i = 0; i < 3; i++)
    for (size_t j = 0; j < 3; j++)
      BOOST_REQUIRE_CLOSE(d.Covariance()(i, j), covariance(i, j), 1e-5);
}

/**
 * Make sure the probability of observations is correct.
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionProbabilityTest)
{
  arma::vec mean("5 6 3 3 2");
  arma::mat cov("6 1 1 1 2;"
                "1 7 1 0 0;"
                "1 1 4 1 1;"
                "1 0 1 7 0;"
                "2 0 1 0 6");

  GaussianDistribution d(mean, cov);

  BOOST_REQUIRE_CLOSE(d.LogProbability("0 1 2 3 4"), -13.432076798791542, 1e-5);
  BOOST_REQUIRE_CLOSE(d.LogProbability("3 2 3 7 8"), -15.814880322345738, 1e-5);
  BOOST_REQUIRE_CLOSE(d.LogProbability("2 2 0 8 1"), -13.754462857772776, 1e-5);
  BOOST_REQUIRE_CLOSE(d.LogProbability("2 1 5 0 1"), -13.283283233107898, 1e-5);
  BOOST_REQUIRE_CLOSE(d.LogProbability("3 0 5 1 0"), -13.800326511545279, 1e-5);
  BOOST_REQUIRE_CLOSE(d.LogProbability("4 0 6 1 0"), -14.900192463287908, 1e-5);
}

/**
 * Test GaussianDistribution::Probability() in the univariate case.
 */
BOOST_AUTO_TEST_CASE(GaussianUnivariateProbabilityTest)
{
  GaussianDistribution g(arma::vec("0.0"), arma::mat("1.0"));

  // Simple case.
  BOOST_REQUIRE_CLOSE(g.Probability(arma::vec("0.0")), 0.398942280401433, 1e-5);
  BOOST_REQUIRE_CLOSE(g.Probability(arma::vec("1.0")), 0.241970724519143, 1e-5);
  BOOST_REQUIRE_CLOSE(g.Probability(arma::vec("-1.0")), 0.241970724519143,
      1e-5);

  // A few more cases...
  arma::mat covariance;

  covariance = 2.0;
  g.Covariance(std::move(covariance));
  BOOST_REQUIRE_CLOSE(g.Probability(arma::vec("0.0")), 0.282094791773878, 1e-5);
  BOOST_REQUIRE_CLOSE(g.Probability(arma::vec("1.0")), 0.219695644733861, 1e-5);
  BOOST_REQUIRE_CLOSE(g.Probability(arma::vec("-1.0")), 0.219695644733861,
      1e-5);

  g.Mean().fill(1.0);
  covariance = 1.0;
  g.Covariance(std::move(covariance));
  BOOST_REQUIRE_CLOSE(g.Probability(arma::vec("1.0")), 0.398942280401433, 1e-5);

  covariance = 2.0;
  g.Covariance(std::move(covariance));
  BOOST_REQUIRE_CLOSE(g.Probability(arma::vec("-1.0")), 0.103776874355149,
      1e-5);
}

/**
 * Test GaussianDistribution::Probability() in the multivariate case.
 */
BOOST_AUTO_TEST_CASE(GaussianMultivariateProbabilityTest)
{
  // Simple case.
  arma::vec mean = "0 0";
  arma::mat cov = "1 0; 0 1";
  arma::vec x = "0 0";

  GaussianDistribution g(mean, cov);

  BOOST_REQUIRE_CLOSE(g.Probability(x), 0.159154943091895, 1e-5);

  arma::mat covariance;
  covariance = "2 0; 0 2";
  g.Covariance(std::move(covariance));

  BOOST_REQUIRE_CLOSE(g.Probability(x), 0.0795774715459477, 1e-5);

  x = "1 1";

  BOOST_REQUIRE_CLOSE(g.Probability(x), 0.0482661763150270, 1e-5);
  BOOST_REQUIRE_CLOSE(g.Probability(-x), 0.0482661763150270, 1e-5);

  g.Mean() = "1 1";
  BOOST_REQUIRE_CLOSE(g.Probability(x), 0.0795774715459477, 1e-5);
  g.Mean() *= -1;
  BOOST_REQUIRE_CLOSE(g.Probability(-x), 0.0795774715459477, 1e-5);

  g.Mean() = "1 1";
  covariance = "2 1.5; 1.5 4";
  g.Covariance(std::move(covariance));

  BOOST_REQUIRE_CLOSE(g.Probability(x), 0.066372199406187285, 1e-5);
  g.Mean() *= -1;
  BOOST_REQUIRE_CLOSE(g.Probability(-x), 0.066372199406187285, 1e-5);

  g.Mean() = "1 1";
  x = "-1 4";

  BOOST_REQUIRE_CLOSE(g.Probability(x), 0.00072147262356379415, 1e-5);
  BOOST_REQUIRE_CLOSE(g.Probability(-x), 0.00085851785428674523, 1e-5);

  // Higher-dimensional case.
  x = "0 1 2 3 4";
  g.Mean() = "5 6 3 3 2";

  covariance = "6 1 1 1 2;"
               "1 7 1 0 0;"
               "1 1 4 1 1;"
               "1 0 1 7 0;"
               "2 0 1 0 6";
  g.Covariance(std::move(covariance));

  BOOST_REQUIRE_CLOSE(g.Probability(x), 1.4673143531128877e-06, 1e-5);
  BOOST_REQUIRE_CLOSE(g.Probability(-x), 7.7404143494891786e-09, 1e-8);

  g.Mean() *= -1;
  BOOST_REQUIRE_CLOSE(g.Probability(-x), 1.4673143531128877e-06, 1e-5);
  BOOST_REQUIRE_CLOSE(g.Probability(x), 7.7404143494891786e-09, 1e-8);
}

/**
 * Test the phi() function, for multiple points in the multivariate Gaussian
 * case.
 */
BOOST_AUTO_TEST_CASE(GaussianMultipointMultivariateProbabilityTest)
{
  // Same case as before.
  arma::vec mean = "5 6 3 3 2";
  arma::mat cov("6 1 1 1 2;"
                "1 7 1 0 0;"
                "1 1 4 1 1;"
                "1 0 1 7 0;"
                "2 0 1 0 6");

  arma::mat points = "0 3 2 2 3 4;"
                     "1 2 2 1 0 0;"
                     "2 3 0 5 5 6;"
                     "3 7 8 0 1 1;"
                     "4 8 1 1 0 0;";

  arma::vec phis;
  GaussianDistribution g(mean, cov);
  g.LogProbability(points, phis);

  BOOST_REQUIRE_EQUAL(phis.n_elem, 6);

  BOOST_REQUIRE_CLOSE(phis(0), -13.432076798791542, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(1), -15.814880322345738, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(2), -13.754462857772776, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(3), -13.283283233107898, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(4), -13.800326511545279, 1e-5);
  BOOST_REQUIRE_CLOSE(phis(5), -14.900192463287908, 1e-5);
}

/**
 * Make sure random observations follow the probability distribution correctly.
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionRandomTest)
{
  arma::vec mean("1.0 2.25");
  arma::mat cov("0.85 0.60;"
                "0.60 1.45");

  GaussianDistribution d(mean, cov);

  arma::mat obs(2, 5000);

  for (size_t i = 0; i < 5000; i++)
    obs.col(i) = d.Random();

  // Now make sure that reflects the actual distribution.
  arma::vec obsMean = arma::mean(obs, 1);
  arma::mat obsCov = ccov(obs);

  // 10% tolerance because this can be noisy.
  BOOST_REQUIRE_CLOSE(obsMean[0], mean[0], 10.0);
  BOOST_REQUIRE_CLOSE(obsMean[1], mean[1], 10.0);

  BOOST_REQUIRE_CLOSE(obsCov(0, 0), cov(0, 0), 10.0);
  BOOST_REQUIRE_CLOSE(obsCov(0, 1), cov(0, 1), 10.0);
  BOOST_REQUIRE_CLOSE(obsCov(1, 0), cov(1, 0), 10.0);
  BOOST_REQUIRE_CLOSE(obsCov(1, 1), cov(1, 1), 10.0);
}

/**
 * Make sure that we can properly estimate from given observations.
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionTrainTest)
{
  arma::vec mean("1.0 3.0 0.0 2.5");
  arma::mat cov("3.0 0.0 1.0 4.0;"
                "0.0 2.4 0.5 0.1;"
                "1.0 0.5 6.3 0.0;"
                "4.0 0.1 0.0 9.1");

  // Now generate the observations.
  arma::mat observations(4, 10000);

  arma::mat transChol = trans(chol(cov));
  for (size_t i = 0; i < 10000; i++)
    observations.col(i) = transChol * arma::randn<arma::vec>(4) + mean;

  // Now estimate.
  GaussianDistribution d;

  // Find actual mean and covariance of data.
  arma::vec actualMean = arma::mean(observations, 1);
  arma::mat actualCov = ccov(observations);

  d.Train(observations);

  // Check that everything is estimated right.
  for (size_t i = 0; i < 4; i++)
    BOOST_REQUIRE_SMALL(d.Mean()[i] - actualMean[i], 1e-5);

  for (size_t i = 0; i < 4; i++)
    for (size_t j = 0; j < 4; j++)
      BOOST_REQUIRE_SMALL(d.Covariance()(i, j) - actualCov(i, j), 1e-5);
}

/**
 * This test verifies the fitting of GaussianDistribution works properly when
 * probabilities for each sample is given.
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionTrainWithProbabilitiesTest)
{
  arma::vec mean = ("5.0");
  arma::vec cov = ("2.0");

  GaussianDistribution dist(mean, cov);
  size_t N = 5000;
  size_t d = 1;

  arma::mat rdata(d, N);
  for (size_t i = 0; i < N; i++)
    rdata.col(i) = dist.Random();

  arma::vec probabilities(N);
  for (size_t i = 0; i < N; i++)
    probabilities(i) = Random();

  // Fit distribution with probabilities and data.
  GaussianDistribution guDist;
  guDist.Train(rdata, probabilities);

  // Fit distribution only with data.
  GaussianDistribution guDist2;
  guDist2.Train(rdata);

  BOOST_REQUIRE_CLOSE(guDist.Mean()[0], guDist2.Mean()[0], 6);
  BOOST_REQUIRE_CLOSE(guDist.Covariance()[0], guDist2.Covariance()[0], 6);

  BOOST_REQUIRE_CLOSE(guDist.Mean()[0], mean[0], 6);
  BOOST_REQUIRE_CLOSE(guDist.Covariance()[0], cov[0], 6);
}

/**
 * This test ensures that the same result is obtained when trained with
 * probabilities all set to 1 and with no probabilities at all.
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionWithProbabilties1Test)
{
  arma::vec mean = ("5.0");
  arma::vec cov  = ("4.0");

  GaussianDistribution dist(mean, cov);
  size_t N = 50000;
  size_t d = 1;

  arma::mat rdata(d, N);

  for (size_t i = 0; i < N; i++)
      rdata.col(i) = Random();

  arma::vec probabilities(N, arma::fill::ones);

  // Fit the distribution with only data.
  GaussianDistribution guDist;
  guDist.Train(rdata);

  // Fit the distribution with data and each probability as 1.
  GaussianDistribution guDist2;
  guDist2.Train(rdata, probabilities);

  BOOST_REQUIRE_CLOSE(guDist.Mean()[0], guDist2.Mean()[0], 1e-15);
  BOOST_REQUIRE_CLOSE(guDist.Covariance()[0], guDist2.Covariance()[0], 1e-2);
}

/**
 * This test draws points from two different normal distributions, sets the
 * probabilities for points from the first distribution to something small and
 * the probabilities for the second to something large.
 *
 * We expect that the distribution we recover after training to be the same as
 * the second normal distribution (the one with high probabilities).
 */
BOOST_AUTO_TEST_CASE(GaussianDistributionTrainWithTwoDistProbabilitiesTest)
{
  arma::vec mean1 = ("5.0");
  arma::vec cov1 = ("4.0");

  arma::vec mean2 = ("3.0");
  arma::vec cov2 = ("1.0");

  // Create two GaussianDistributions with different parameters.
  GaussianDistribution dist1(mean1, cov1);
  GaussianDistribution dist2(mean2, cov2);

  size_t N = 50000;
  size_t d = 1;

  arma::mat rdata(d, N);
  arma::vec probabilities(N);

  // Fill even numbered columns with random points from dist1 and odd numbered
  // columns with random points from dist2.
  for (size_t j = 0; j < N; j++)
  {
    if (j % 2 == 0)
      rdata.col(j) = dist1.Random();
    else
      rdata.col(j) = dist2.Random();
  }

  // Assign high probabilities to points drawn from dist1 and low probabilities
  // to numbers drawn from dist2.
  for (size_t i = 0 ; i < N ; i++)
  {
    if (i % 2 == 0)
      probabilities(i) = Random(0.98, 1);
    else
      probabilities(i) = Random(0, 0.02);
  }

  GaussianDistribution guDist;
  guDist.Train(rdata, probabilities);

  BOOST_REQUIRE_CLOSE(guDist.Mean()[0], mean1[0], 5);
  BOOST_REQUIRE_CLOSE(guDist.Covariance()[0], cov1[0], 5);
}

/******************************/
/** Gamma Distribution Tests **/
/******************************/
/**
 * Make sure that using an object to fit one reference set and then asking
 * to fit another works properly.
 */
BOOST_AUTO_TEST_CASE(GammaDistributionTrainTest)
{
  // Create a gamma distribution random generator.
  double alphaReal = 5.3;
  double betaReal = 1.5;
  std::gamma_distribution<double> dist(alphaReal, betaReal);

  // Create a N x d gamma distribution data and fit the results.
  size_t N = 200;
  size_t d = 2;
  arma::mat rdata(d, N);

  // Random generation of gamma-like points.
  for (size_t j = 0; j < d; ++j)
    for (size_t i = 0; i < N; ++i)
      rdata(j, i) = dist(math::randGen);

  // Create Gamma object and call Train() on reference set.
  GammaDistribution gDist;
  gDist.Train(rdata);

  // Training must estimate d pairs of alpha and beta parameters.
  BOOST_REQUIRE_EQUAL(gDist.Dimensionality(), d);
  BOOST_REQUIRE_EQUAL(gDist.Dimensionality(), d);

  // Create a N' x d' gamma distribution, fit results without new object.
  size_t N2 = 350;
  size_t d2 = 4;
  arma::mat rdata2(d2, N2);

  // Random generation of gamma-like points.
  for (size_t j = 0; j < d2; ++j)
    for (size_t i = 0; i < N2; ++i)
      rdata2(j, i) = dist(math::randGen);

  // Fit results using old object.
  gDist.Train(rdata2);

  // Training must estimate d' pairs of alpha and beta parameters.
  BOOST_REQUIRE_EQUAL(gDist.Dimensionality(), d2);
  BOOST_REQUIRE_EQUAL(gDist.Dimensionality(), d2);
}

/**
 * This test verifies that the fitting procedure for GammaDistribution works
 * properly when probabilities for each sample is given.
 */
BOOST_AUTO_TEST_CASE(GammaDistributionTrainWithProbabilitiesTest)
{
  double alphaReal = 5.4;
  double betaReal = 6.7;

  // Create a gamma distribution random generator.
  std::gamma_distribution<double> dist(alphaReal, betaReal);

  size_t N = 50000;
  size_t d = 2;
  arma::mat rdata(d, N);

  for (size_t j = 0; j < d; j++)
    for (size_t i = 0; i < N; i++)
      rdata(j, i) = dist(math::randGen);

  // Fill the probabilities randomly.
  arma::vec probabilities(N, arma::fill::randu);

  // Fit results with probabilities and data.
  GammaDistribution gDist;
  gDist.Train(rdata, probabilities);

  // Fit results with only data.
  GammaDistribution gDist2;
  gDist2.Train(rdata);

  BOOST_REQUIRE_CLOSE(gDist2.Alpha(0), gDist.Alpha(0), 1.5);
  BOOST_REQUIRE_CLOSE(gDist2.Beta(0), gDist.Beta(0), 1.5);

  BOOST_REQUIRE_CLOSE(gDist2.Alpha(1), gDist.Alpha(1), 1.5);
  BOOST_REQUIRE_CLOSE(gDist2.Beta(1), gDist.Beta(1), 1.5);

  BOOST_REQUIRE_CLOSE(alphaReal, gDist.Alpha(0), 3.0);
  BOOST_REQUIRE_CLOSE(betaReal, gDist.Beta(0), 3.0);

  BOOST_REQUIRE_CLOSE(alphaReal, gDist.Alpha(1), 3.0);
  BOOST_REQUIRE_CLOSE(betaReal, gDist.Beta(1), 3.0);
}

/**
 * This test ensures that the same result is obtained when trained with
 * probabilities all set to 1 and with no probabilities at all.
 */
BOOST_AUTO_TEST_CASE(GammaDistributionTrainAllProbabilities1Test)
{
  double alphaReal = 5.4;
  double betaReal = 6.7;

  // Create a gamma distribution random generator.
  std::gamma_distribution<double> dist(alphaReal, betaReal);

  size_t N = 1000;
  size_t d = 2;
  arma::mat rdata(d, N);

  for (size_t j = 0; j < d; j++)
    for (size_t i = 0; i < N; i++)
      rdata(j, i) = dist(math::randGen);

  // Fit results with only data.
  GammaDistribution gDist;
  gDist.Train(rdata);

  // Fit results with data and each probability as 1.
  GammaDistribution gDist2;
  arma::vec allProbabilities1(N, arma::fill::ones);
  gDist2.Train(rdata, allProbabilities1);

  BOOST_REQUIRE_CLOSE(gDist2.Alpha(0), gDist.Alpha(0), 1e-5);
  BOOST_REQUIRE_CLOSE(gDist2.Beta(0), gDist.Beta(0), 1e-5);

  BOOST_REQUIRE_CLOSE(gDist2.Alpha(1), gDist.Alpha(1), 1e-5);
  BOOST_REQUIRE_CLOSE(gDist2.Beta(1), gDist.Beta(1), 1e-5);
}

/**
 * This test draws points from two different gamma distributions, sets the
 * probabilities for the points from the first distribution to something small
 * and the probabilities for the second to something large.  It ensures that the
 * gamma distribution recovered has the same parameters as the second gamma
 * distribution with high probabilities.
 */
BOOST_AUTO_TEST_CASE(GammaDistributionTrainTwoDistProbabilities1Test)
{
  double alphaReal = 5.4;
  double betaReal = 6.7;

  double alphaReal2 = 1.9;
  double betaReal2 = 8.4;

  // Create two gamma distribution random generators.
  std::gamma_distribution<double> dist(alphaReal, betaReal);
  std::gamma_distribution<double> dist2(alphaReal2, betaReal2);

  size_t N = 50000;
  size_t d = 2;
  arma::mat rdata(d, N);
  arma::vec probabilities(N);

  // Draw points alternately from the two different distributions.
  for (size_t j = 0; j < d; j++)
  {
    for (size_t i = 0; i < N; i++)
    {
      if (i % 2 == 0)
        rdata(j, i) = dist(math::randGen);
      else
        rdata(j, i) = dist2(math::randGen);
    }
  }

  for (size_t i = 0; i < N; i++)
  {
    if (i % 2 == 0)
      probabilities(i) = 0.02 * math::Random();
    else
      probabilities(i) = 0.98 + 0.02 * math::Random();
  }

  GammaDistribution gDist;
  gDist.Train(rdata, probabilities);

  BOOST_REQUIRE_CLOSE(alphaReal2, gDist.Alpha(0), 5);
  BOOST_REQUIRE_CLOSE(betaReal2, gDist.Beta(0), 5);

  BOOST_REQUIRE_CLOSE(alphaReal2, gDist.Alpha(1), 5);
  BOOST_REQUIRE_CLOSE(betaReal2, gDist.Beta(1), 5);
}

/**
 * This test verifies that the fitting procedure for GammaDistribution works
 * properly and converges near the actual gamma parameters. We do this twice
 * with different alpha/beta parameters so we make sure we don't have some weird
 * bug that always converges to the same number.
 */
BOOST_AUTO_TEST_CASE(GammaDistributionFittingTest)
{
  // Offset from the actual alpha/beta. 10% is quite a relaxed tolerance since
  // the random points we generate are few (for test speed) and might be fitted
  // better by a similar distribution.
  double errorTolerance = 10;

  size_t N = 5000;
  size_t d = 1; // Only 1 dimension is required for this.

  /** Iteration 1 (first parameter set) **/

  // Create a gamma-random generator and data.
  double alphaReal = 5.3;
  double betaReal = 1.5;
  std::gamma_distribution<double> dist(alphaReal, betaReal);

  // Random generation of gamma-like points.
  arma::mat rdata(d, N);
  for (size_t j = 0; j < d; ++j)
    for (size_t i = 0; i < N; ++i)
      rdata(j, i) = dist(math::randGen);

  // Create Gamma object and call Train() on reference set.
  GammaDistribution gDist;
  gDist.Train(rdata);

  // Estimated parameter must be close to real.
  BOOST_REQUIRE_CLOSE(gDist.Alpha(0), alphaReal, errorTolerance);
  BOOST_REQUIRE_CLOSE(gDist.Beta(0), betaReal, errorTolerance);

  /** Iteration 2 (different parameter set) **/

  // Create a gamma-random generator and data.
  double alphaReal2 = 7.2;
  double betaReal2 = 0.9;
  std::gamma_distribution<double> dist2(alphaReal2, betaReal2);

  // Random generation of gamma-like points.
  arma::mat rdata2(d, N);
  for (size_t j = 0; j < d; ++j)
    for (size_t i = 0; i < N; ++i)
      rdata2(j, i) = dist2(math::randGen);

  // Create Gamma object and call Train() on reference set.
  GammaDistribution gDist2;
  gDist2.Train(rdata2);

  // Estimated parameter must be close to real.
  BOOST_REQUIRE_CLOSE(gDist2.Alpha(0), alphaReal2, errorTolerance);
  BOOST_REQUIRE_CLOSE(gDist2.Beta(0), betaReal2, errorTolerance);
}

/**
 * Test that Train() and the constructor that takes data give the same resulting
 * distribution.
 */
BOOST_AUTO_TEST_CASE(GammaDistributionTrainConstructorTest)
{
  const arma::mat data = arma::randu<arma::mat>(10, 500);

  GammaDistribution d1(data);
  GammaDistribution d2;
  d2.Train(data);

  for (size_t i = 0; i < 10; ++i)
  {
    BOOST_REQUIRE_CLOSE(d1.Alpha(i), d2.Alpha(i), 1e-5);
    BOOST_REQUIRE_CLOSE(d1.Beta(i), d2.Beta(i), 1e-5);
  }
}

/**
 * Test that Train() with a dataset and Train() with dataset statistics return
 * the same results.
 */
BOOST_AUTO_TEST_CASE(GammaDistributionTrainStatisticsTest)
{
  const arma::mat data = arma::randu<arma::mat>(1, 500);

  // Train object d1 with the data.
  GammaDistribution d1(data);

  // Train object d2 with the data's statistics.
  GammaDistribution d2;
  const arma::vec meanLogx = arma::mean(arma::log(data), 1);
  const arma::vec meanx = arma::mean(data, 1);
  const arma::vec logMeanx = arma::log(meanx);
  d2.Train(logMeanx, meanLogx, meanx);

  BOOST_REQUIRE_CLOSE(d1.Alpha(0), d2.Alpha(0), 1e-5);
  BOOST_REQUIRE_CLOSE(d1.Beta(0), d2.Beta(0), 1e-5);
}

/**
 * Tests that Random() generates points that can be reasonably well fit by the
 * distribution that generated them.
 */
BOOST_AUTO_TEST_CASE(GammaDistributionRandomTest)
{
  const arma::vec a("2.0 2.5 3.0"), b("0.4 0.6 1.3");
  const size_t numPoints = 2000;

  // Distribution to generate points.
  GammaDistribution d1(a, b);
  arma::mat data(3, numPoints); // 3-d points.

  for (size_t i = 0; i < numPoints; ++i)
    data.col(i) = d1.Random();

  // Distribution to fit points.
  GammaDistribution d2(data);
  for (size_t i = 0; i < 3; ++i)
  {
    BOOST_REQUIRE_CLOSE(d2.Alpha(i), a(i), 10); // Within 10%
    BOOST_REQUIRE_CLOSE(d2.Beta(i), b(i), 10);
  }
}

BOOST_AUTO_TEST_CASE(GammaDistributionProbabilityTest)
{
  // Train two 1-dimensional distributions.
  const arma::vec a1("2.0"), b1("0.9"), a2("3.1"), b2("1.4");
  arma::mat x1("2.0"), x2("2.94");
  arma::vec prob1, prob2;

  // Evaluated at wolfram|alpha
  GammaDistribution d1(a1, b1);
  d1.Probability(x1, prob1);
  BOOST_REQUIRE_CLOSE(prob1(0), 0.267575, 1e-3);

  // Evaluated at wolfram|alpha
  GammaDistribution d2(a2, b2);
  d2.Probability(x2, prob2);
  BOOST_REQUIRE_CLOSE(prob2(0), 0.189043, 1e-3);

  // Check that the overload that returns the probability for 1 dimension
  // agrees.
  BOOST_REQUIRE_CLOSE(prob2(0), d2.Probability(2.94, 0), 1e-5);

  // Combine into one 2-dimensional distribution.
  const arma::vec a3("2.0 3.1"), b3("0.9 1.4");
  arma::mat x3(2, 2);
  x3 << 2.0 << 2.94 << arma::endr
     << 2.0 << 2.94;
  arma::vec prob3;

  // Expect that the 2-dimensional distribution returns the product of the
  // 1-dimensional distributions (evaluated at wolfram|alpha).
  GammaDistribution d3(a3, b3);
  d3.Probability(x3, prob3);
  BOOST_REQUIRE_CLOSE(prob3(0), 0.04408, 1e-2);
  BOOST_REQUIRE_CLOSE(prob3(1), 0.026165, 1e-2);
}

BOOST_AUTO_TEST_CASE(GammaDistributionLogProbabilityTest)
{
  // Train two 1-dimensional distributions.
  const arma::vec a1("2.0"), b1("0.9"), a2("3.1"), b2("1.4");
  arma::mat x1("2.0"), x2("2.94");
  arma::vec prob1, prob2;

  // Evaluated at wolfram|alpha
  GammaDistribution d1(a1, b1);
  d1.LogProbability(x1, prob1);
  BOOST_REQUIRE_CLOSE(prob1(0), std::log(0.267575), 1e-3);

  // Evaluated at wolfram|alpha
  GammaDistribution d2(a2, b2);
  d2.LogProbability(x2, prob2);
  BOOST_REQUIRE_CLOSE(prob2(0), std::log(0.189043), 1e-3);

  // Combine into one 2-dimensional distribution.
  const arma::vec a3("2.0 3.1"), b3("0.9 1.4");
  arma::mat x3(2, 2);
  x3
    << 2.0 << 2.94 << arma::endr
    << 2.0 << 2.94;
  arma::vec prob3;

  // Expect that the 2-dimensional distribution returns the product of the
  // 1-dimensional distributions (evaluated at wolfram|alpha).
  GammaDistribution d3(a3, b3);
  d3.LogProbability(x3, prob3);
  BOOST_REQUIRE_CLOSE(prob3(0), std::log(0.04408), 1e-3);
  BOOST_REQUIRE_CLOSE(prob3(1), std::log(0.026165), 1e-3);
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

#include <boost/gil/gil_all.hpp>

template<typename Pixel, typename View>
void clip_pixel(std::int64_t const& x, std::int64_t const& y, Pixel const& pixel, View& view)
{
	if(x < 0 || y < 0)
	{
		return;
	}
	else if(x < view.width() && y < view.height())
	{
		view(x, y) = pixel;
	}
}

template<typename Pixel, typename View>
void bresenham_line(std::int64_t const& x1, std::int64_t const& y1, std::int64_t const& x2, std::int64_t const& y2, Pixel const& pixel, View& view)
{
	// Adapted from http://www.etechplanet.com/codesnippets/computer-graphics-draw-a-line-using-bresenham-algorithm.aspx

	std::int64_t const dx(x2 - x1);
	std::int64_t const dy(y2 - y1);
	std::int64_t const adx(std::abs(dx));
	std::int64_t const ady(std::abs(dy));
	std::int64_t px(2 * ady - adx);
	std::int64_t py(2 * adx - ady);
	std::int64_t x, y, xe, ye;
	if(adx < ady)
	{
		if(dy < 0)
		{
			x = x2;
			y = y2;
			ye = y1;
		}
		else
		{
			x = x1;
			y = y1;
			ye = y2;
		}
		clip_pixel(x, y, pixel, view);

		for(std::int64_t i(0); y < ye; ++ i)
		{
			y = y + 1;
			if(py > 0)
			{
				if((dx < 0 && dy < 0) || (dx > 0 && dy > 0))
				{
					x = x + 1;
				}
				else
				{
					x = x - 1;
				}
				py = py + 2 * (adx - ady);
			}
			else
			{
				py = py + 2 * adx;
			}
			clip_pixel(x, y, pixel, view);
		}
	}
	else
	{
		if(dx < 0)
		{
			x = x2;
			y = y2;
			xe = x1;
		}
		else
		{
			x = x1;
			y = y1;
			xe = x2;
		}
		clip_pixel(x, y, pixel, view);

		for(std::int64_t i(0); x < xe; ++ i)
		{
			x = x + 1;
			if(px < 0)
			{
				px = px + 2 * ady;
			}
			else
			{
				if((dx < 0 && dy < 0) || (dx > 0 && dy > 0))
				{
					y = y + 1;
				}
				else
				{
					y = y - 1;
				}
				px = px + 2 * (ady - adx);
			}
			clip_pixel(x, y, pixel, view);
		}
	}
}


//=======================================================================
// 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 <vector>
#include <string>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/leda_graph.hpp>
// Undefine macros from LEDA that conflict with the C++ Standard Library.
#undef string
#undef vector

int
main()
{
  using namespace boost;
  typedef GRAPH < std::string, char >graph_t;
  graph_t leda_g;
  typedef graph_traits < graph_t >::vertex_descriptor vertex_t;
  std::vector < vertex_t > vert(7);
  vert[0] = add_vertex(std::string("pick up kids from school"), leda_g);
  vert[1] = add_vertex(std::string("buy groceries (and snacks)"), leda_g);
  vert[2] = add_vertex(std::string("get cash at ATM"), leda_g);
  vert[3] =
    add_vertex(std::string("drop off kids at soccer practice"), leda_g);
  vert[4] = add_vertex(std::string("cook dinner"), leda_g);
  vert[5] = add_vertex(std::string("pick up kids from soccer"), leda_g);
  vert[6] = add_vertex(std::string("eat dinner"), leda_g);

  add_edge(vert[0], vert[3], leda_g);
  add_edge(vert[1], vert[3], leda_g);
  add_edge(vert[1], vert[4], leda_g);
  add_edge(vert[2], vert[1], leda_g);
  add_edge(vert[3], vert[5], leda_g);
  add_edge(vert[4], vert[6], leda_g);
  add_edge(vert[5], vert[6], leda_g);

  std::vector < vertex_t > topo_order;
  node_array < default_color_type > color_array(leda_g);

  topological_sort(leda_g, std::back_inserter(topo_order),
                   color_map(make_leda_node_property_map(color_array)));

  std::reverse(topo_order.begin(), topo_order.end());
  int n = 1;
  for (std::vector < vertex_t >::iterator i = topo_order.begin();
       i != topo_order.end(); ++i, ++n)
    std::cout << n << ": " << leda_g[*i] << std::endl;

  return EXIT_SUCCESS;
}


// Copyright 2017 Nest Labs, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "graph.hpp"
#include "detector.hpp"
#include "graphanalyzer.hpp"
#include "processorcontainer.hpp"
#include "lag.hpp"

#include <iostream>
#include <cmath>
#include <algorithm>
#include <sstream>
#include <string>
#include <Eigen/Dense>

using namespace std;
using namespace DetectorGraph;
using namespace Eigen;

/**
 * @file robotlocalization.cpp
 * @brief A localization system for a mobile robot with Lag-based feedback loop.
 *
 * @section ex-rl-intro Introduction
 * Back in the 2000s all [_citation needed_] robots were _differential wheeled
 * robots_ [1] and localization with sensor fusion was a popular problem.
 * This example shows a simple solution to that problem using DetectorGraph.
 * It uses an Extended Kalman Filter [2] to continuously predict and correct
 * the robot's Pose (and associated uncertainty) from two information sources:
 * the input to the wheels and a GPS-like system.
 *
 * @section ex-rl-fpvslag FuturePublisher vs. Lag\<T\>
 * This localization algorithm depends on a feedback-loop in that the output of
 * one graph evaluation (i.e. `LocalizationBelief`) is used as an input for the
 * next one (i.e. `Lagged<LocalizationBelief>`):
 @snippetlineno robotlocalization.cpp KalmanPoseCorrector Feedback Loop
 * Note that in this graph the TopicState where the feedback loop closes is a
 * legitimate output in itself and it's very likely that new Detectors in the
 * graph would subscribe to it. In cases like this the use of
 * DetectorGraph::Lag is preferred as it is more extensible and it preserves
 * the normal TopicState guarantee for the TopicState in question as well as it
 * makes the Lagged version of the TopicState clearly documented and also
 * available. Lag allows even for detectors that subscribes to both the immediate
 * and the Lagged version of a TopicState - this could be useful for things like
 * differentiation etc.
 *
 * @section ex-rl-conf-topicstates Configuration TopicStates
 * It also shows how TopicStates can be used for static/configuration data
 * (e.g. `RobotConfig`). This allows for easy dependency tracking, visualization
 * and testing at pretty much no runtime cost.
 *
 * @section ex-rl-localization Localization Algorithm
 * Regarding the Localization algorithm itself, it's mostly taken from [3]
 * except for the correction model where this example uses a different input
 * (i.e. `GPSPosition`) with much simpler transfer function to the Pose vector.
 *
 * @section ex-rl-arch Architecture
 * The graph below shows the relationships between the topics (rectangles) and
 * detectors (ellipses). Note that this graph can be automatically generated
 * for any instance of DetectorGraph::Graph using DetectorGraph::GraphAnalyzer.
 *  @dot "RobotBrain"
digraph GraphAnalyzer {
    rankdir = "LR";
    node[fontname=Helvetica];
    size="12,5";

    "RobotConfig" [label="0:RobotConfig",style=filled, shape=box, color=lightblue];
        "RobotConfig" -> "ExtendedKalmanPosePredictor";
    "InitialPose" [label="1:InitialPose",style=filled, shape=box, color=lightblue];
        "InitialPose" -> "ExtendedKalmanPosePredictor";
    "WheelSpeeds" [label="2:WheelSpeeds",style=filled, shape=box, color=lightblue];
        "WheelSpeeds" -> "ExtendedKalmanPosePredictor";
    "GPSPosition" [label="3:GPSPosition",style=filled, shape=box, color=lightblue];
        "GPSPosition" -> "KalmanPoseCorrector";
    "LaggedLocalizationBelief" [label="4:Lagged<LocalizationBelief>",style=filled, shape=box, color=lightblue];
        "LaggedLocalizationBelief" -> "ExtendedKalmanPosePredictor";
        "LaggedLocalizationBelief" -> "KalmanPoseCorrector";
    "KalmanPoseCorrector" [label="6:KalmanPoseCorrector", color=blue];
        "KalmanPoseCorrector" -> "LocalizationBelief";
    "ExtendedKalmanPosePredictor" [label="5:ExtendedKalmanPosePredictor", color=blue];
        "ExtendedKalmanPosePredictor" -> "LocalizationBelief";
    "LocalizationBelief" [label="7:LocalizationBelief",style=filled, shape=box, color=red];
        "LocalizationBelief" -> "LagLocalizationBelief";
    "LagLocalizationBelief" [label="8:Lag<LocalizationBelief>", color=blue];
        "LagLocalizationBelief" -> "LaggedLocalizationBelief" [style=dotted, color=red, constraint=false];
}
 *  @enddot
 *
 *
 * @section ex-rl-other-notes Other Notes
 * This example also uses the Eigen library [4] for matrix/vector arithmetic
 * and linear-algebraic operations.
 *
 * Note that this entire algorithm is contained in a single file for the sake
 * of unity as an example. In real-world scenarios the suggested pattern is to
 * split the code into:
 *
 @verbatim
   detectorgraph/
        include/
            robotbrain.hpp (RobotBrain header)
        src/
            robotbrain.hpp (RobotBrain implementation)
        detectors/
            include/
                ExtendedKalmanPosePredictor.hpp
                KalmanPoseCorrector.hpp
            src/
                ExtendedKalmanPosePredictor.cpp
                KalmanPoseCorrector.cpp
        topicstates/
            include/
                RobotConfig.hpp
                InitialPose.hpp
                WheelSpeeds.hpp
                LocalizationBelief.hpp
                GPSPosition.hpp
@endverbatim
 *
 * @section ex-rl-refs References
 *  - [1] Differential Wheeled Robots - https://en.wikipedia.org/wiki/Differential_wheeled_robot
 *  - [2] Extended Kalman Filter - https://en.wikipedia.org/wiki/Extended_Kalman_filter
 *  - [3] EKF applied to Mobile Robot’s Localization (Section 2.4.4, page 17) - http://cpscotti.com/pdf/pfc_scotti.pdf
 *  - [4] Eigen - http://eigen.tuxfamily.org/dox/index.html
 */

/// @cond DO_NOT_DOCUMENT

// Contains the configuration parameters of this differential mobile Robot
// or vehicle.
struct RobotConfig : public TopicState
{
    RobotConfig() : r(), b(), e() {}
    RobotConfig(double aR, double aB, double aE)
        : r(aR), b(aB), e(aE) {}
    double r; // wheels r
    double b; // displacement between wheels 2*l (l=d(wheel,center))

    double e; // relative wheel slippage/error
};

struct InitialPose : public TopicState
{
    InitialPose() : timestampMs(0), pose(Vector3d::Zero()) {}
    InitialPose(Vector3d aPose) : timestampMs(0), pose(aPose) {}
    InitialPose(uint64_t aTimestampMs, Vector3d aPose) : timestampMs(aTimestampMs), pose(aPose) {}
    uint64_t timestampMs;
    Vector3d pose;
};

// The measured (via encoders or other odometry method) wheel speeds
struct WheelSpeeds : public TopicState
{
    WheelSpeeds() : timestampMs(), phi(Vector2d::Zero()) {}
    WheelSpeeds(uint64_t ts, Vector2d aPhi) : timestampMs(ts), phi(aPhi) {}
    WheelSpeeds(uint64_t ts, double r, double l) : timestampMs(ts), phi(r, l) {}
    uint64_t timestampMs;

    // phi_r, phi_l in radians
    Vector2d phi;
};

//! [TopicStates Composition Example]
struct KalmanState
{
    KalmanState() : pose(Vector3d::Zero()), error(Matrix3d::Zero()) {}
    KalmanState(Vector3d aPose, Matrix3d aError) : pose(aPose), error(aError) {}
    Vector3d pose;
    Matrix3d error;
};

//! [Mutually Atomic Variables]
struct LocalizationBelief : public TopicState
{
    LocalizationBelief() : timestampMs(), state() {}
    LocalizationBelief(uint64_t ts, const KalmanState& aState) : timestampMs(ts), state(aState) {}
    LocalizationBelief(uint64_t ts, const Vector3d& aPose, const Matrix3d& aError) : timestampMs(ts), state(aPose, aError) {}
    uint64_t timestampMs;
    KalmanState state;
};
//! [Mutually Atomic Variables]

struct GPSPosition : public TopicState
{
    GPSPosition() : timestampMs(), state() {}
    GPSPosition(uint64_t ts, const KalmanState& aState) : timestampMs(ts), state(aState) {}
    uint64_t timestampMs;
    KalmanState state;
};
//! [TopicStates Composition Example]

double WrapAngle(double th)
{
    // remainder requires C++11
    return std::remainder(th, 2. * M_PI);
}

class ExtendedKalmanPosePredictor : public Detector,
    public SubscriberInterface<RobotConfig>,
    public SubscriberInterface<InitialPose>,
    public SubscriberInterface< Lagged<LocalizationBelief> >,
    public SubscriberInterface<WheelSpeeds>,
    public Publisher<LocalizationBelief>
{
public:
    ExtendedKalmanPosePredictor(Graph* graph) : Detector(graph), mCurrentBelief(), mConfig()
    {
        Subscribe<RobotConfig>(this);
        Subscribe<InitialPose>(this);
        Subscribe< Lagged<LocalizationBelief> >(this);
        Subscribe<WheelSpeeds>(this);
        SetupPublishing<LocalizationBelief>(this);
    }

    virtual void Evaluate(const RobotConfig& aConfig)
    {
        mConfig = aConfig;
    }

    virtual void Evaluate(const InitialPose& aInitialPose)
    {
        mCurrentBelief.state = KalmanState(aInitialPose.pose, Matrix3d::Zero());
        mCurrentBelief.timestampMs = aInitialPose.timestampMs;
    }

    virtual void Evaluate(const Lagged<LocalizationBelief>& aLaggedBelief)
    {
        mCurrentBelief.state = aLaggedBelief.data.state;
        mCurrentBelief.timestampMs = aLaggedBelief.data.timestampMs;
    }

    virtual void Evaluate(const WheelSpeeds& aWheelSpeeds)
    {
        // [\Delta S_{r}, \Delta S_{l}] from phi
        Vector2d wheelLinearSpeed = aWheelSpeeds.phi * mConfig.r;

        // Eq. 2.36
        double d_th = (wheelLinearSpeed.x() - wheelLinearSpeed.y()) / mConfig.b;
        // Eq. 2.37
        double d_travel = (wheelLinearSpeed.x() + wheelLinearSpeed.y()) / 2.;

        double theta = mCurrentBelief.state.pose.z();

        // Used on most equations below
        double th_dth2 = theta + d_th / 2.;

        // Eq. 2.33 on [3]
        Vector3d cartesianSpeed;
        cartesianSpeed << d_travel * cos(th_dth2),
                          d_travel * sin(th_dth2),
                                             d_th;

        // Compute Time Delta
        double tDelta = (aWheelSpeeds.timestampMs - mCurrentBelief.timestampMs) * 1e-3;

        // Eq. 2.33 (i.e. 2.23) on [3]
        mCurrentBelief.state.pose = mCurrentBelief.state.pose + (cartesianSpeed * tDelta);
        mCurrentBelief.state.pose.z() = WrapAngle(mCurrentBelief.state.pose.z());

        // Eq. 2.34 on [3]
        Matrix3d G;
        G << 1, 0, -d_travel * sin(th_dth2),
             0, 1,  d_travel * cos(th_dth2),
             0, 0,                        1;

        // Eq. 2.35 on [3]
        Matrix<double, 3, 2> V;
        V << 0.5 * cos(th_dth2) - (d_travel / (2. * mConfig.b)) * sin(th_dth2), 0.5 * cos(th_dth2) + (d_travel / (2. * mConfig.b)) * sin(th_dth2),
             0.5 * sin(th_dth2) + (d_travel / (2. * mConfig.b)) * cos(th_dth2), 0.5 * sin(th_dth2) - (d_travel / (2. * mConfig.b)) * cos(th_dth2),
                                                                  1./mConfig.b,                                                     -1./mConfig.b;

        // Eq. 2.38 on [3]
        Matrix2d M;
        M << mConfig.e * abs(wheelLinearSpeed.x()),                                     0,
                                                 0, mConfig.e * abs(wheelLinearSpeed.y());

        // Eq. 2.24 on [3]
        mCurrentBelief.state.error = G * mCurrentBelief.state.error * G.transpose() + V * M * V.transpose();

        mCurrentBelief.timestampMs = aWheelSpeeds.timestampMs;
        Publish(mCurrentBelief);
    }

    LocalizationBelief mCurrentBelief;
    RobotConfig mConfig;
};

///! [KalmanPoseCorrector Feedback Loop]
class KalmanPoseCorrector : public Detector,
    public SubscriberInterface< Lagged<LocalizationBelief> >,
    public SubscriberInterface<GPSPosition>,
    public Publisher<LocalizationBelief>
{
public:
    KalmanPoseCorrector(Graph* graph) : Detector(graph), mCurrentBelief()
    {
        Subscribe< Lagged<LocalizationBelief> >(this);
        Subscribe<GPSPosition>(this);
        SetupPublishing<LocalizationBelief>(this);
    }
///! [KalmanPoseCorrector Feedback Loop]

    virtual void Evaluate(const Lagged<LocalizationBelief>& aLaggedBelief)
    {
        mCurrentBelief.state = aLaggedBelief.data.state;
        mCurrentBelief.timestampMs = aLaggedBelief.data.timestampMs;
    }

    virtual void Evaluate(const GPSPosition& aCorrectionInput)
    {
        // C = Identity(3)

        // Eq. 2.15 on [3]
        Matrix3d K = mCurrentBelief.state.error * (mCurrentBelief.state.error + aCorrectionInput.state.error).inverse();

        // Eq. 2.13 on [3]
        mCurrentBelief.state.pose = mCurrentBelief.state.pose + K * (aCorrectionInput.state.pose - mCurrentBelief.state.pose);

        // Eq. 2.14 on [3]
        mCurrentBelief.state.error = (Matrix3d::Identity() - K) * mCurrentBelief.state.error;

        Publish(mCurrentBelief);
    }

    LocalizationBelief mCurrentBelief;
};

//![RobotBrain Static]
class RobotBrain : public ProcessorContainer
{
public:
    RobotBrain()
    : mPosePredictor(&mGraph)
    , mKalmanPoseCorrector(&mGraph)
    , mBeliefFeedback(&mGraph)
    {
    }

    ExtendedKalmanPosePredictor mPosePredictor;
    KalmanPoseCorrector mKalmanPoseCorrector;
    Lag<LocalizationBelief> mBeliefFeedback;
    //![RobotBrain Static]

    virtual void ProcessOutput()
    {
        Topic<LocalizationBelief>* correctedStateTopic = mGraph.ResolveTopic<LocalizationBelief>();
        if (correctedStateTopic->HasNewValue())
        {
            const LocalizationBelief& belief = correctedStateTopic->GetNewValue();
            const auto& pose = belief.state.pose;
            cout << "Pose = [" << pose.x() << ", " << pose.y() << ", " << pose.z() << "], ";
            const auto& error = belief.state.error.diagonal();
            cout << "Error = [" << error.x() << ", " << error.y() << ", " << error.z() << "]";
            cout << endl;
        }
    }
};

int main()
{
    RobotBrain robot;

    uint64_t kSimStep = 1000;
    uint64_t dataIndex = 0;

    robot.ProcessData(RobotConfig(1, 1, 0.1));
    robot.ProcessData(InitialPose(Vector3d(10.0, 10.0, 0.0)));

    // Circular Movement
    // for (int i=0;i<100;i++) robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1., 0.5));

    // Fwd, Rotate ~180, Fwd, GPS Update
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, -1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, -1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(WheelSpeeds((dataIndex++)*kSimStep, 1, 1));
    robot.ProcessData(GPSPosition((dataIndex++)*kSimStep,
        KalmanState(Vector3d(10., 10., 4),  // Position
                    Vector3d(0.1,0.1,300000).asDiagonal()  // Error
            )));

    cout << "---- Done ----" << endl;

    GraphAnalyzer analyzer(robot.mGraph);
    analyzer.GenerateDotFile("robot_localization.dot");

    return 0;
}

/// @endcond DO_NOT_DOCUMENT


/**
 * @file utils.cpp
 * @brief Usefult utilities
 * @author Parker Lusk <plusk@mit.edu>
 * @date 12 October 2020
 */

#include <functional>
#include <queue>
#include <random>
#include <utility>
#include <vector>

#include <Eigen/Dense>

#include "clipper/utils.h"

namespace clipper {
namespace utils {

Eigen::VectorXd randvec(size_t n)
{
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<double> dis(0, 1);

  return Eigen::VectorXd::NullaryExpr(n, 1, [&](){ return dis(gen); });
}

// ----------------------------------------------------------------------------

std::vector<int> findIndicesOfkLargest(const Eigen::VectorXd& x, int k)
{
  using T = std::pair<double, int>; // pair value to be compared and index
  if (k < 1) return {}; // invalid input
  // n.b., the top of this queue is smallest element
  std::priority_queue<T, std::vector<T>, std::greater<T>> q;
  for (size_t i=0; i<x.rows(); ++i) {
    if (q.size() < k) {
      q.push({x(i), i});
    } else if (q.top().first < x(i)) {
      q.pop();
      q.push({x(i), i});
    }
  }

  std::vector<int> indices(k);
  for (size_t i=0; i<k; ++i) {
    indices[k - i - 1] = q.top().second;
    q.pop();
  }

  return indices;
}

} // ns utils
} // ns clipper