/*
* This file is part of Teseo Android HAL
*
* Copyright (c) 2016-2017, STMicroelectronics - All Rights Reserved
* Author(s): Baudouin Feildel <baudouin.feildel@st.com> for STMicroelectronics.
*
* License terms: Apache 2.0.
*
* 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.
*
*/
/**
 * @file model.cpp
 * @author Baudouin Feildel <baudouin.feildel@st.com>
 * @copyright 2016, STMicroelectronics, All rights reserved.
 */

#include <teseo/geofencing/model.h>

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

namespace stm {
namespace geofencing {
namespace model {

double degree_to_rad(double d)
{
    return d * boost::math::double_constants::degree;
}

bool transitionFlagsIsValid(TransitionFlags flags)
{
    constexpr TransitionFlags ALL_FLAGS_INVERTED = ~(
        static_cast<int32_t>(Transition::Entered) &
        static_cast<int32_t>(Transition::Exited)  &
        static_cast<int32_t>(Transition::Uncertain));

    // force all valid values in flags to zero
    // if others bit are enabled transition flags is not valie
    return !(flags & ALL_FLAGS_INVERTED);
}

constexpr double WGS84_A = 6378137.0;
constexpr double WGS84_E = 0.0818191908426;
constexpr double WGS84_E2 = WGS84_E * WGS84_E;

Point::Point()
{ }

Point::Point(const Location & loc) :
    latitude(loc.latitude()),
    longitude(loc.longitude())
{ }

Point::Point(const ICoordinate & lat, const ICoordinate & lon) :
    latitude(lat.asDecimalDegree()),
    longitude(lon.asDecimalDegree())
{ }

std::pair<double, double> Point::to_rad() const
{
    return std::make_pair(
        degree_to_rad(latitude.value()),
        degree_to_rad(longitude.value())
    );
}

double Point::distanceFrom(const Point & p)
{
    const auto this_rad = to_rad();
    const auto p_rad = p.to_rad();

    const double sin_lat = sin(this_rad.first);

    /* Earth local radius @ given latitude */
    const double N = WGS84_A / sqrt(1.0 - (WGS84_E2 * sin_lat * sin_lat));

    return sqrt(
        ((N * (p_rad.first - this_rad.first)) * (N * (p_rad.first - this_rad.first))) +
        ((N * (p_rad.second - this_rad.second) * sin_lat) * (N * (p_rad.second - this_rad.second) * sin_lat))
    );
}

} // namespace model
} // namespace geofencing
} // namespace stm


//
// Created by kellerberrin on 5/08/18.
//

#include "kel_exec_env.h"
#include "kel_distribution.h"

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/negative_binomial.hpp>

namespace bm = boost::math;
namespace kel = kellerberrin;

/////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Distribution functions use boost special functions.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////

double kel::NormalDistribution::pdf(double x, double mean, double std_dev) {

  static const double inv_sqrt_2pi = 0.3989422804014327;

  double a = (x - mean) / std_dev;

  return (inv_sqrt_2pi / std_dev) * std::exp(-0.5 * a * a);

}

double kel::GammaDistribution::pdf(double x) const {

  return bm::pdf(bm::gamma_distribution<>(_a, _b), x);

}

double kel::GammaDistribution::cdf(double x) const {

  return bm::cdf(bm::gamma_distribution<>(_a, _b), x);

}

double kel::GammaDistribution::quantile(double p) const {

  return bm::quantile(bm::gamma_distribution<>(_a, _b), p);

}

double kel::BetaDistribution::logInverseBetaFunction(double a, double b) {

  assert(a > 0.0);
  assert(b > 0.0);

  double log_gamma = bm::lgamma<double>(a + b) - bm::lgamma<double>(a) - bm::lgamma<double>(b);

  return log_gamma;

}

double kel::BetaDistribution::logPartialPdf(double x, double a, double b) {

  assert(x >= 0 && x <= 1);
  assert(a >= 0);
  assert(b >= 0);

  double ret =  (b - 1) * std::log(1 - x) + (a - 1) * std::log(x);

  return ret;

}

double kel::BetaDistribution::logPdf(double x, double a, double b) {

  assert(x >= 0 && x <= 1);
  assert(a > 0);
  assert(b > 0);

  double ret = logInverseBetaFunction(a, b) + logPartialPdf(x, a, b);

  return ret;

}

double kel::BetaDistribution::pdf(double x, double a, double b) {

  assert(x >= 0 && x <= 1);
  assert(a > 0);
  assert(b > 0);

  double p = bm::tgamma<double>(a + b) / (bm::tgamma<double>(a) * bm::tgamma<double>(b));
  double q = std::pow(1 - x, b - 1) * std::pow(x, a - 1);
  return p * q;

}

double kel::BetaDistribution::mean(double a, double b) {

  assert(a > 0);
  assert(b > 0);

  return a / (a + b);

}

double kel::BetaDistribution::var(double a, double b) {

  assert(a > 0);
  assert(b > 0);

  return (a * b) / ((a + b) * (a + b) * (a + b + 1.0));

}

double kel::BetaDistribution::mode(double a, double b) {

  assert(a > 1);
  assert(b > 1);

  return (a - 1.0) / (a + b - 2.0);

}

double kel::BetaBinomialDistribution::pdf(size_t n, size_t k, double alpha, double beta) {

  assert(k <= n);
  assert(alpha > 0);
  assert(beta > 0);

  double coeff = bm::binomial_coefficient<double>(n, k);

  double a1 = static_cast<double>(k) + alpha;
  double b1 = static_cast<double>(n-k) + beta;

  double p = bm::beta<double>(a1, b1);
  double q = bm::beta<double>(alpha, beta);

  return coeff * (p / q);

}

double kel::BetaBinomialDistribution::partialPdf(double n, double k, double alpha, double beta) {

  assert(k <= n);
  assert(alpha > 0);
  assert(beta > 0);

  double a1 = k + alpha;
  double b1 = n - k + beta;

  double p = bm::beta<double>(a1, b1);
  double q = bm::beta<double>(alpha, beta);

  return (p / q);

}

double kel::BetaBinomialDistribution::logPartialPdf(double n, double k, double alpha, double beta) {

  assert(k <= n);
  assert(alpha > 0.0);
  assert(beta > 0.0);

  double r = n - k;
  double a1 = k + alpha;
  double b1 = r + beta;

  double beta_n = bm::lgamma<double>(a1) + bm::lgamma<double>(b1) - bm::lgamma<double>(a1 + b1);
  double beta_d = bm::lgamma<double>(alpha) + bm::lgamma<double>(beta) - bm::lgamma<double>(alpha + beta);
  double prob = beta_n - beta_d;

  return prob;

}

double kel::BetaBinomialDistribution::logPdf(double n, double k, double alpha, double beta) {

  assert(k <= n);
  assert(alpha > 0.0);
  assert(beta > 0.0);

  double r = n - k;
  double a1 = k + alpha;
  double b1 = r + beta;

  double beta_coeff = bm::lgamma<double>(r + 1.0) + bm::lgamma<double>(k + 1.0) - bm::lgamma<double>(n + 2.0);
  double inverse_log_binonimal_coeff = std::log(n + 1.0) + beta_coeff;
  double beta_n = bm::lgamma<double>(a1) + bm::lgamma<double>(b1) - bm::lgamma<double>(a1 + b1);
  double beta_d = bm::lgamma<double>(alpha) + bm::lgamma<double>(beta) - bm::lgamma<double>(alpha + beta);
  double prob = beta_n - beta_d - inverse_log_binonimal_coeff;

  return prob;

}

// .first is alpha, .second is beta. The raw moments are calculated from the observations and used to calculate alpha and beta.
[[nodiscard]] std::pair<double, double> kel::BetaBinomialDistribution::methodOfMoments(const std::vector<size_t>& observations, size_t n_trials) {

  // calculate the first moment
  size_t obs_sum = std::accumulate(observations.begin(), observations.end(), static_cast<size_t>(0));
  const double m1 = static_cast<double>(obs_sum) / static_cast<double>(observations.size());

  // calculate the 2nd moment
  auto sqr_lambda = [](size_t accumulate, size_t obs)->size_t { return accumulate + (obs * obs); };
  size_t sqr_sum = std::accumulate(observations.begin(), observations.end(), static_cast<size_t>(0), sqr_lambda);
  const double m2 = static_cast<double>(sqr_sum) / static_cast<double>(observations.size());

  const double n = static_cast<double>(n_trials);

  const double a_numer = (n * m1) - m2;
  const double ab_denom = n * ((m2 / m1) - m1 - 1) + m1;
  const double a = a_numer / ab_denom;

  const double b_numer = (n - m1) * (n - (m2/m1));
  const double b = b_numer / ab_denom;

  return {a, b};

}

double kel::BinomialDistribution::pdf(size_t n, size_t k, double prob_success) {

  assert(k <= n and k >= 0);
  assert(prob_success >= 0 and prob_success <= 1.0);

  double coeff = bm::binomial_coefficient<double>(n, k);

  double p = std::pow(prob_success, static_cast<double>(k));

  double q = std::pow((1.0 - prob_success), static_cast<double>(n - k));

  return coeff * p * q;

}

double kel::BinomialDistribution::cdf(size_t n, double k, double prob_success) {

  assert(k <= n and k >= 0);
  assert(prob_success >= 0 and prob_success <= 1.0);

  size_t integer_k = std::floor(k);
  double sum_pdf = 0.0;

  for (size_t index = 0; index <= integer_k; ++index) {

    sum_pdf += cdf(n, index, prob_success);

  }

  return sum_pdf;

}

kel::HypergeometricDistribution::HypergeometricDistribution(size_t pop_successes_K, size_t sample_size_n, size_t population_N) {

  if (pop_successes_K > population_N) {

    ExecEnv::log().warn("HypergeometricDistribution::HypergeometricDistribution; Population Successes K:{} exceeds population size :{}",
                         pop_successes_K, population_N);
    pop_successes_K = population_N;
  }

  if (sample_size_n > population_N) {

    ExecEnv::log().warn("HypergeometricDistribution::HypergeometricDistribution; Sample size n:{} exceeds population size :{}",
                         sample_size_n, population_N);
    sample_size_n = population_N;
  }

  pop_successes_K_ = pop_successes_K;
  sample_size_n_ = sample_size_n;
  population_N_ = population_N;

}

double kel::HypergeometricDistribution::pdf(size_t successes_k) const {

  if (successes_k > upperSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution::pdf; r_successes k:{} exceeds upper limit :{}",
                         successes_k, upperSuccesses_k());
    successes_k = upperSuccesses_k();

  }

  if (successes_k < lowerSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution::pdf; r_successes k:{} below lower limit :{}",
                         successes_k, lowerSuccesses_k());
    successes_k = lowerSuccesses_k();

  }

  bm::hypergeometric_distribution hypergeometric(pop_successes_K_, sample_size_n_, population_N_);

  return bm::pdf(hypergeometric, successes_k);

}

double kel::HypergeometricDistribution::cdf(size_t successes_k) const {

  if (successes_k > upperSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution(N:{},K:{},n:{},k:{})::cdf; r_successes k exceeds upper limit :{}",
                        population_N_, pop_successes_K_, sample_size_n_, successes_k, upperSuccesses_k());
    successes_k = upperSuccesses_k();

  }

  if (successes_k < lowerSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution(N:{},K:{},n:{},k:{})::cdf; r_successes k exceeds upper limit :{}",
                        population_N_, pop_successes_K_, sample_size_n_, successes_k, lowerSuccesses_k());
    successes_k = lowerSuccesses_k();

  }

  bm::hypergeometric_distribution hypergeometric(pop_successes_K_, sample_size_n_, population_N_);

  return bm::cdf(hypergeometric, successes_k);

}

double kel::HypergeometricDistribution::quantile(size_t successes_k) const {

  if (successes_k > upperSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution::quantile; r_successes k:{} exceeds upper limit :{}",
                         successes_k, upperSuccesses_k());
    successes_k = upperSuccesses_k();

  }

  if (successes_k < lowerSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution::quantile; r_successes k:{} below lower limit :{}",
                         successes_k, lowerSuccesses_k());
    successes_k = lowerSuccesses_k();

  }

  bm::hypergeometric_distribution hypergeometric(pop_successes_K_, sample_size_n_, population_N_);

  return bm::quantile(hypergeometric, successes_k);

}

size_t kel::HypergeometricDistribution::lowerSuccesses_k() const {

  int64_t lower_success = static_cast<int64_t>(sample_size_n_ + pop_successes_K_) - static_cast<int64_t>(population_N_);
  return static_cast<size_t>(std::max<int64_t>(0, lower_success));

}

double kel::HypergeometricDistribution::upperSingleTailTest(size_t test_value_k) const {

  if (test_value_k > 0) {

    test_value_k = test_value_k - 1;

  }

  return 1.0 - cdf(test_value_k);

}

double kel::HypergeometricDistribution::lowerSingleTailTest(size_t test_value_k) const {

  return cdf(test_value_k);

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// The Poisson distribution. Uses boost for implementation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////

double kel::Poisson::pdf(size_t count) const {

  return bm::pdf(bm::poisson_distribution<>(lambda_), count);

}

double kel::Poisson::cdf(size_t count) const  {

  return bm::cdf(bm::poisson_distribution<>(lambda_), count);

}

size_t kel::Poisson::quantile(double quantile) const {

  return bm::quantile(bm::poisson_distribution<>(lambda_), quantile);

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// The Negative Binomial distribution. Uses boost for implementation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////

double kel::NegativeBinomial::pdf(size_t count) const {

  return bm::pdf(bm::negative_binomial_distribution<>(r_successes_, p_prob_success_), count);

}

double kel::NegativeBinomial::cdf(size_t count) const {

  return bm::cdf(bm::negative_binomial_distribution<>(r_successes_, p_prob_success_), count);

}

size_t kel::NegativeBinomial::quantile(double quantile) const {

  return bm::quantile(bm::negative_binomial_distribution<>(r_successes_, p_prob_success_), quantile);

}

double kel::NegativeBinomial::mean() const {

  return bm::mean(bm::negative_binomial_distribution<>(r_successes_, p_prob_success_));

}


#include <iostream>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>
using namespace Eigen;
using namespace std;

template<typename Q>
EIGEN_DONT_INLINE Q nlerp(const Q& a, const Q& b, typename Q::Scalar t)
{
  return Q((a.coeffs() * (1.0-t) + b.coeffs() * t).normalized());
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t)
{
  return a.slerp(t,b);
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_legacy(const Q& a, const Q& b, typename Q::Scalar t)
{
  typedef typename Q::Scalar Scalar;
  static const Scalar one = Scalar(1) - dummy_precision<Scalar>();
  Scalar d = a.dot(b);
  Scalar absD = internal::abs(d);
  if (absD>=one)
    return a;

  // theta is the angle between the 2 quaternions
  Scalar theta = std::acos(absD);
  Scalar sinTheta = internal::sin(theta);

  Scalar scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
  Scalar scale1 = internal::sin( ( t * theta) ) / sinTheta;
  if (d<0)
    scale1 = -scale1;

  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_legacy_nlerp(const Q& a, const Q& b, typename Q::Scalar t)
{
  typedef typename Q::Scalar Scalar;
  static const Scalar one = Scalar(1) - epsilon<Scalar>();
  Scalar d = a.dot(b);
  Scalar absD = internal::abs(d);

  Scalar scale0;
  Scalar scale1;

  if (absD>=one)
  {
    scale0 = Scalar(1) - t;
    scale1 = t;
  }
  else
  {
    // theta is the angle between the 2 quaternions
    Scalar theta = std::acos(absD);
    Scalar sinTheta = internal::sin(theta);

    scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
    scale1 = internal::sin( ( t * theta) ) / sinTheta;
    if (d<0)
      scale1 = -scale1;
  }

  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
}

template<typename T>
inline T sin_over_x(T x)
{
  if (T(1) + x*x == T(1))
    return T(1);
  else
    return std::sin(x)/x;
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_rw(const Q& a, const Q& b, typename Q::Scalar t)
{
  typedef typename Q::Scalar Scalar;

  Scalar d = a.dot(b);
  Scalar theta;
  if (d<0.0)
    theta = /*M_PI -*/ Scalar(2)*std::asin( (a.coeffs()+b.coeffs()).norm()/2 );
  else
    theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );

  // theta is the angle between the 2 quaternions
//   Scalar theta = std::acos(absD);
  Scalar sinOverTheta = sin_over_x(theta);

  Scalar scale0 = (Scalar(1)-t)*sin_over_x( ( Scalar(1) - t ) * theta) / sinOverTheta;
  Scalar scale1 = t * sin_over_x( ( t * theta) ) / sinOverTheta;
  if (d<0)
    scale1 = -scale1;

  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_gael(const Q& a, const Q& b, typename Q::Scalar t)
{
  typedef typename Q::Scalar Scalar;

  Scalar d = a.dot(b);
  Scalar theta;
//   theta = Scalar(2) * atan2((a.coeffs()-b.coeffs()).norm(),(a.coeffs()+b.coeffs()).norm());
//   if (d<0.0)
//     theta = M_PI-theta;

  if (d<0.0)
    theta = /*M_PI -*/ Scalar(2)*std::asin( (-a.coeffs()-b.coeffs()).norm()/2 );
  else
    theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );

  Scalar scale0;
  Scalar scale1;
  if(theta*theta-Scalar(6)==-Scalar(6))
  {
    scale0 = Scalar(1) - t;
    scale1 = t;
  }
  else
  {
    Scalar sinTheta = std::sin(theta);
    scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
    scale1 = internal::sin( ( t * theta) ) / sinTheta;
    if (d<0)
      scale1 = -scale1;
  }

  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
}

int main()
{
  typedef double RefScalar;
  typedef float TestScalar;

  typedef Quaternion<RefScalar>  Qd;
  typedef Quaternion<TestScalar> Qf;

  unsigned int g_seed = (unsigned int) time(NULL);
  std::cout << g_seed << "\n";
//   g_seed = 1259932496;
  srand(g_seed);

  Matrix<RefScalar,Dynamic,1> maxerr(7);
  maxerr.setZero();

  Matrix<RefScalar,Dynamic,1> avgerr(7);
  avgerr.setZero();

  cout << "double=>float=>double       nlerp        eigen        legacy(snap)         legacy(nlerp)        rightway         gael's criteria\n";

  int rep = 100;
  int iters = 40;
  for (int w=0; w<rep; ++w)
  {
    Qf a, b;
    a.coeffs().setRandom();
    a.normalize();
    b.coeffs().setRandom();
    b.normalize();

    Qf c[6];

    Qd ar(a.cast<RefScalar>());
    Qd br(b.cast<RefScalar>());
    Qd cr;

    cout.precision(8);
    cout << std::scientific;
    for (int i=0; i<iters; ++i)
    {
      RefScalar t = 0.65;
      cr = slerp_rw(ar,br,t);

      Qf refc = cr.cast<TestScalar>();
      c[0] = nlerp(a,b,t);
      c[1] = slerp_eigen(a,b,t);
      c[2] = slerp_legacy(a,b,t);
      c[3] = slerp_legacy_nlerp(a,b,t);
      c[4] = slerp_rw(a,b,t);
      c[5] = slerp_gael(a,b,t);

      VectorXd err(7);
      err[0] = (cr.coeffs()-refc.cast<RefScalar>().coeffs()).norm();
//       std::cout << err[0] << "    ";
      for (int k=0; k<6; ++k)
      {
        err[k+1] = (c[k].coeffs()-refc.coeffs()).norm();
//         std::cout << err[k+1] << "    ";
      }
      maxerr = maxerr.cwise().max(err);
      avgerr += err;
//       std::cout << "\n";
      b = cr.cast<TestScalar>();
      br = cr;
    }
//     std::cout << "\n";
  }
  avgerr /= RefScalar(rep*iters);
  cout << "\n\nAccuracy:\n"
       << "  max: " << maxerr.transpose() << "\n";
  cout << "  avg: " << avgerr.transpose() << "\n";

  // perf bench
  Quaternionf a,b;
  a.coeffs().setRandom();
  a.normalize();
  b.coeffs().setRandom();
  b.normalize();
  //b = a;
  float s = 0.65;

  #define BENCH(FUNC) {\
    BenchTimer t; \
    for(int k=0; k<2; ++k) {\
      t.start(); \
      for(int i=0; i<1000000; ++i) \
        FUNC(a,b,s); \
      t.stop(); \
    } \
    cout << "  " << #FUNC << " => \t " << t.value() << "s\n"; \
  }

  cout << "\nSpeed:\n" << std::fixed;
  BENCH(nlerp);
  BENCH(slerp_eigen);
  BENCH(slerp_legacy);
  BENCH(slerp_legacy_nlerp);
  BENCH(slerp_rw);
  BENCH(slerp_gael);
}


#ifndef DEFINITIONS_CPP
#define DEFINITIONS_CPP

#include  <Eigen/Core>
#include <bits/stdc++.h>
#include <Eigen/SVD>
#include  <iostream>
#include <math.h>
#include <assert.h>
#include <time.h>
#include <fstream>
#include <random>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include <iterator>
#include <cstdlib>
#include <unistd.h>

using  namespace  std;
using  namespace  Eigen;
typedef complex<double> cd;

#define mat MatrixXd
#define MIN 0.00001
#define vec VectorXd

// #define CROSSCHECK true
#define CROSSCHECK false
// #define DEBUG true
#define DEBUG false

#endif // DEFINITIONS_CPP


//
// Created by hhy on 2022/3/11.
//
#include "OsqpEigen/OsqpEigen.h"
#include <Eigen/Dense>
#include <iostream>

using namespace std;

int main() {
  OsqpEigen::Solver solver;

  // assert(InitMat(solver));
  int numOfVar = 3;
  int numOfCons = 4;

  Eigen::SparseMatrix<double> hessian(numOfVar, numOfVar);
  hessian.insert(0, 0) = 1;
  hessian.insert(0, 1) = -1;
  hessian.insert(0, 2) = 1;
  hessian.insert(1, 0) = -1;
  hessian.insert(1, 1) = 2;
  hessian.insert(1, 2) = -2;
  hessian.insert(2, 0) = 1;
  hessian.insert(2, 1) = -2;
  hessian.insert(2, 2) = 4;
  std::cout << "hessian:" << hessian << std::endl;
  /* hessian << 1, -1, 1,
           -1, 2, -2,
            1, -2, 4;*/

  Eigen::SparseMatrix<double> linearMatrix(numOfCons, numOfVar);
  linearMatrix.insert(0, 0) = 1;
  linearMatrix.insert(1, 1) = 1;
  linearMatrix.insert(2, 2) = 1;
  linearMatrix.insert(3, 0) = 1;
  linearMatrix.insert(3, 1) = 1;
  linearMatrix.insert(3, 2) = 1;
  std::cout << " linearMatrix:" << linearMatrix << std::endl;
  /*linearMatrix << 1, 0, 0,
                0, 1, 0,
                0, 0, 1,
                1, 1, 1;*/

  Eigen::Vector3d gradient;
  gradient << 2, -3, 1;
  std::cout << "gradient:\n" << gradient << std::endl;

  Eigen::VectorXd lowerBound;
  lowerBound.resize(4, 1);
  lowerBound << 0, 0, 0, 0.4;
  std::cout << "lowerBound:\n" << lowerBound << std::endl;

  Eigen::VectorXd upperBound;
  upperBound.resize(4, 1);
  upperBound << 1, 1, 1, 0.5;

  solver.settings()->setVerbosity(true);
  solver.settings()->setAlpha(1.0);

  //  assert(solver.data()->setHessianMatrix(H_s)== true);
  solver.data()->setNumberOfVariables(numOfVar);
  solver.data()->setNumberOfConstraints(numOfCons);

  assert(solver.data()->setHessianMatrix(hessian));
  assert(solver.data()->setGradient(gradient));
  assert(solver.data()->setLinearConstraintsMatrix(linearMatrix));
  assert(solver.data()->setLowerBound(lowerBound));
  assert(solver.data()->setUpperBound(upperBound));

  assert(solver.initSolver());

  // assert(solver.solve() == true);
  assert(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError);

  std::cout << solver.getSolution() << std::endl;
}


/**
 * Copyright (C) Omar Thor <omarthoro@gmail.com> - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 *
 * Written by Omar Thor <omarthoro@gmail.com>, 2017
 */
#ifndef SP_ALGO_NN_LOSS_MEAN_SQUARE_ERROR_HPP
#define SP_ALGO_NN_LOSS_MEAN_SQUARE_ERROR_HPP

#include <boost/assert.hpp>
#include "../config.hpp"
#include "../matrix.hpp"
#include "../types.hpp"
#include "sp/util/hints.hpp"

SP_ALGO_NN_NAMESPACE_BEGIN

/**
 * \brief Mean Square Error Derivative
 */
struct mean_square_error_derivative {

    sp_hot void operator()(         const size_t& si,
                                    const tensor_4& predicted,
                                    const tensor_4& observed,
                                    tensor_4& result) {

        BOOST_ASSERT(predicted.dimensions() == observed.dimensions());

        const size_t m = predicted.dimension(1) * predicted.dimension(2) * predicted.dimension(3);

        float_t factor = float_t(2.0f) / static_cast<float_t> (m);

        result.chip(si, 0) = factor * (predicted.chip(si, 0) - observed.chip(si, 0));
    }
};

/**
 * \brief Mean Square Error
 */
struct mean_square_error {

    using derivative_type = mean_square_error_derivative;

    auto operator()(                    const size_t& si,
                                        const tensor_4& predicted,
                                        const tensor_4& observed) {
        BOOST_ASSERT(predicted.dimensions() == observed.dimensions());

        tensor_0 d = (
            (predicted.chip(si, 0) - observed.chip(si, 0))
                *
            (predicted.chip(si, 0) - observed.chip(si, 0))
        ).sum();

        return d(0) / static_cast<float_t> (predicted.size());
    }

    derivative_type derivative;
};

SP_ALGO_NN_NAMESPACE_END

#endif	/* SP_ALGO_NN_LOSS_MEAN_SQUARE_ERROR_HPP */


/**
 * @file gmm_test.cpp
 * @author Ryan Curtin
 * @author Michael Fox
 *
 * Test for the Gaussian Mixture Model class.
 *
 * 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 <mlpack/methods/gmm/gmm.hpp>

#include <mlpack/methods/gmm/no_constraint.hpp>
#include <mlpack/methods/gmm/positive_definite_constraint.hpp>
#include <mlpack/methods/gmm/diagonal_constraint.hpp>
#include <mlpack/methods/gmm/eigenvalue_ratio_constraint.hpp>

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

using namespace mlpack;
using namespace mlpack::gmm;

BOOST_AUTO_TEST_SUITE(GMMTest);
/**
 * Test GMM::Probability() for a single observation for a few cases.
 */
BOOST_AUTO_TEST_CASE(GMMProbabilityTest)
{
  // Create a GMM.
  GMM gmm(2, 2);
  gmm.Component(0) = distribution::GaussianDistribution("0 0", "1 0; 0 1");
  gmm.Component(1) = distribution::GaussianDistribution("3 3", "2 1; 1 2");
  gmm.Weights() = "0.3 0.7";

  // Now test a couple observations.  These comparisons are calculated by hand.
  BOOST_REQUIRE_CLOSE(gmm.Probability("0 0"), 0.05094887202, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("1 1"), 0.03451996667, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("2 2"), 0.04696302254, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("3 3"), 0.06432759685, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("-1 5.3"), 2.503171278804e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("1.4 0"), 0.024676682176, 1e-5);
}

/**
 * Test GMM::Probability() for a single observation being from a particular
 * component.
 */
BOOST_AUTO_TEST_CASE(GMMProbabilityComponentTest)
{
  // Create a GMM (same as the last test).
  GMM gmm(2, 2);
  gmm.Component(0) = distribution::GaussianDistribution("0 0", "1 0; 0 1");
  gmm.Component(1) = distribution::GaussianDistribution("3 3", "2 1; 1 2");
  gmm.Weights() = "0.3 0.7";

  // Now test a couple observations.  These comparisons are calculated by hand.
  BOOST_REQUIRE_CLOSE(gmm.Probability("0 0", 0), 0.0477464829276, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("0 0", 1), 0.0032023890978, 1e-5);

  BOOST_REQUIRE_CLOSE(gmm.Probability("1 1", 0), 0.0175649494573, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("1 1", 1), 0.0169550172159, 1e-5);

  BOOST_REQUIRE_CLOSE(gmm.Probability("2 2", 0), 8.7450733951e-4, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("2 2", 1), 0.0460885151993, 1e-5);

  BOOST_REQUIRE_CLOSE(gmm.Probability("3 3", 0), 5.8923841039e-6, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("3 3", 1), 0.0643217044658, 1e-5);

  BOOST_REQUIRE_CLOSE(gmm.Probability("-1 5.3", 0), 2.30212100302e-8, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("-1 5.3", 1), 2.48015006877e-6, 1e-5);

  BOOST_REQUIRE_CLOSE(gmm.Probability("1.4 0", 0), 0.0179197849738, 1e-5);
  BOOST_REQUIRE_CLOSE(gmm.Probability("1.4 0", 1), 0.0067568972024, 1e-5);
}

/**
 * Test training a model on only one Gaussian (randomly generated) in two
 * dimensions.  We will vary the dataset size from small to large.  The EM
 * algorithm is used for training the GMM.
 */
BOOST_AUTO_TEST_CASE(GMMTrainEMOneGaussian)
{
  for (size_t iterations = 0; iterations < 4; iterations++)
  {
    // Determine random covariance and mean.
    arma::vec mean;
    mean.randu(2);
    arma::vec covar;
    covar.randu(2);

    arma::mat data;
    data.randn(2 /* dimension */, 150 * pow(10, (iterations / 3.0)));

    // Now apply mean and covariance.
    data.row(0) *= covar(0);
    data.row(1) *= covar(1);

    data.row(0) += mean(0);
    data.row(1) += mean(1);

    // Now, train the model.
    GMM gmm(1, 2);
    gmm.Train(data, 10);

    arma::vec actualMean = arma::mean(data, 1);
    arma::mat actualCovar = ccov(data, 1 /* biased estimator */);

    // Check the model to see that it is correct.
    CheckMatrices(gmm.Component(0).Mean(), actualMean);
    CheckMatrices(gmm.Component(0).Covariance(), actualCovar);

    BOOST_REQUIRE_CLOSE(gmm.Weights()[0], 1.0, 1e-5);
  }
}

/**
 * Test a training model on multiple Gaussians in higher dimensionality than
 * two.  We will hold the dataset size constant at 10k points.  The EM algorithm
 * is used for training the GMM.
 */
BOOST_AUTO_TEST_CASE(GMMTrainEMMultipleGaussians)
{
  // Higher dimensionality gives us a greater chance of having separated
  // Gaussians.
  size_t dims = 8;
  size_t gaussians = 3;

  // Generate dataset.
  arma::mat data;
  data.zeros(dims, 500);

  std::vector<arma::vec> means(gaussians);
  std::vector<arma::mat> covars(gaussians);
  arma::vec weights(gaussians);
  arma::Col<size_t> counts(gaussians);

  // Choose weights randomly.
  weights.zeros();
  while (weights.min() < 0.02)
  {
    weights.randu(gaussians);
    weights /= accu(weights);
  }

  for (size_t i = 0; i < gaussians; i++)
    counts[i] = round(weights[i] * (data.n_cols - gaussians));
  // Ensure one point minimum in each.
  counts += 1;

  // Account for rounding errors (possibly necessary).
  counts[gaussians - 1] += (data.n_cols - arma::accu(counts));

  // Build each Gaussian individually.
  size_t point = 0;
  for (size_t i = 0; i < gaussians; i++)
  {
    arma::mat gaussian;
    gaussian.randn(dims, counts[i]);

    // Randomly generate mean and covariance.
    means[i].randu(dims);
    means[i] -= 0.5;
    means[i] *= 50;

    // We need to make sure the covariance is positive definite.  We will take a
    // random matrix C and then set our covariance to 4 * C * C', which will be
    // positive semidefinite.
    covars[i].randu(dims, dims);
    covars[i] *= 4 * trans(covars[i]);

    data.cols(point, point + counts[i] - 1) = (covars[i] * gaussian + means[i]
        * arma::ones<arma::rowvec>(counts[i]));

    // Calculate the actual means and covariances because they will probably
    // be different (this is easier to do before we shuffle the points).
    means[i] = arma::mean(data.cols(point, point + counts[i] - 1), 1);
    covars[i] = ccov(data.cols(point, point + counts[i] - 1), 1 /* biased */);

    point += counts[i];
  }

  // Calculate actual weights.
  for (size_t i = 0; i < gaussians; i++)
    weights[i] = (double) counts[i] / data.n_cols;

  // Now train the model.
  GMM gmm(gaussians, dims);
  gmm.Train(data, 10);

  arma::uvec sortRef = sort_index(weights);
  arma::uvec sortTry = sort_index(gmm.Weights());

  // Check the model to see that it is correct.
  for (size_t i = 0; i < gaussians; i++)
  {
    // Check the mean.
    CheckMatrices(gmm.Component(sortTry[i]).Mean(), means[sortRef[i]], 1e-3);
    // Check the covariance.
    CheckMatrices(gmm.Component(sortTry[i]).Covariance(), covars[sortRef[i]],
                  0.05);
    // Check the weight.
    BOOST_REQUIRE_CLOSE(gmm.Weights()[sortTry[i]], weights[sortRef[i]],
        0.001);
  }
}

/**
 * Train a single-gaussian mixture, but using the overload of Train() where
 * probabilities of the observation are given.
 */
BOOST_AUTO_TEST_CASE(GMMTrainEMSingleGaussianWithProbability)
{
  // Generate observations from a Gaussian distribution.
  distribution::GaussianDistribution d("0.5 1.0", "1.0 0.3; 0.3 1.0");

  // 10000 observations, each with random probability.
  arma::mat observations(2, 20000);
  for (size_t i = 0; i < 20000; i++)
    observations.col(i) = d.Random();
  arma::vec probabilities;
  probabilities.randu(20000); // Random probabilities.

  // Now train the model.
  GMM g(1, 2);
  g.Train(observations, probabilities, 10);

  // Check that it is trained correctly.  5% tolerance because of random error
  // present in observations.
  BOOST_REQUIRE_CLOSE(g.Component(0).Mean()[0], 0.5, 5.0);
  BOOST_REQUIRE_CLOSE(g.Component(0).Mean()[1], 1.0, 5.0);

  // 6% tolerance on the large numbers, 10% on the smaller numbers.
  BOOST_REQUIRE_CLOSE(g.Component(0).Covariance()(0, 0), 1.0, 6.0);
  BOOST_REQUIRE_CLOSE(g.Component(0).Covariance()(0, 1), 0.3, 10.0);
  BOOST_REQUIRE_CLOSE(g.Component(0).Covariance()(1, 0), 0.3, 10.0);
  BOOST_REQUIRE_CLOSE(g.Component(0).Covariance()(1, 1), 1.0, 6.0);

  BOOST_REQUIRE_CLOSE(g.Weights()[0], 1.0, 1e-5);
}

/**
 * Train a multi-Gaussian mixture, using the overload of Train() where
 * probabilities of the observation are given.
 */
BOOST_AUTO_TEST_CASE(GMMTrainEMMultipleGaussiansWithProbability)
{
  // We'll have three Gaussian distributions from this mixture, and one Gaussian
  // not from this mixture (but we'll put some observations from it in).
  distribution::GaussianDistribution d1("0.0 1.0 0.0", "1.0 0.0 0.5;"
                                                       "0.0 0.8 0.1;"
                                                       "0.5 0.1 1.0");
  distribution::GaussianDistribution d2("2.0 -1.0 5.0", "3.0 0.0 0.5;"
                                                        "0.0 1.2 0.2;"
                                                        "0.5 0.2 1.3");
  distribution::GaussianDistribution d3("0.0 5.0 -3.0", "2.0 0.0 0.0;"
                                                        "0.0 0.3 0.0;"
                                                        "0.0 0.0 1.0");
  distribution::GaussianDistribution d4("4.0 2.0 2.0", "1.5 0.6 0.5;"
                                                       "0.6 1.1 0.1;"
                                                       "0.5 0.1 1.0");

  // Now we'll generate points and probabilities.  1500 points.  Slower than I
  // would like...
  arma::mat points(3, 5000);
  arma::vec probabilities(5000);

  for (size_t i = 0; i < 5000; i++)
  {
    double randValue = math::Random();

    if (randValue <= 0.20) // p(d1) = 0.20
      points.col(i) = d1.Random();
    else if (randValue <= 0.50) // p(d2) = 0.30
      points.col(i) = d2.Random();
    else if (randValue <= 0.90) // p(d3) = 0.40
      points.col(i) = d3.Random();
    else // p(d4) = 0.10
      points.col(i) = d4.Random();

    // Set the probability right.  If it came from this mixture, it should be
    // 0.97 plus or minus a little bit of noise.  If not, then it should be 0.03
    // plus or minus a little bit of noise.  The base probability (minus the
    // noise) is parameterizable for easy modification of the test.
    double confidence = 0.998;
    double perturbation = math::Random(-0.002, 0.002);

    if (randValue <= 0.90)
      probabilities(i) = confidence + perturbation;
    else
      probabilities(i) = (1 - confidence) + perturbation;
  }

  // Now train the model.
  GMM g(3, 3); // 3 dimensions, 3 components (the fourth component is fake).

  g.Train(points, probabilities, 8);

  // Now check the results.  We need to order by weights so that when we do the
  // checking, things will be correct.
  arma::uvec sortedIndices = sort_index(g.Weights());

  // The tolerances in our checks are quite large, but it is good to remember
  // that we introduced a fair amount of random noise into this whole process.
  // We don't need to look for the fourth Gaussian since that is not supposed to
  // be a part of this mixture.

  // First Gaussian (d1).
  BOOST_REQUIRE_SMALL(g.Weights()[sortedIndices[0]] - 0.2, 0.1);

  for (size_t i = 0; i < 3; i++)
    BOOST_REQUIRE_SMALL((g.Component(sortedIndices[0]).Mean()[i]
        - d1.Mean()[i]), 0.4);

  for (size_t row = 0; row < 3; row++)
    for (size_t col = 0; col < 3; col++)
      BOOST_REQUIRE_SMALL((g.Component(sortedIndices[0]).Covariance()(row, col)
          - d1.Covariance()(row, col)), 0.7); // Big tolerance!  Lots of noise.

  // Second Gaussian (d2).
  BOOST_REQUIRE_SMALL(g.Weights()[sortedIndices[1]] - 0.3, 0.1);

  for (size_t i = 0; i < 3; i++)
    BOOST_REQUIRE_SMALL((g.Component(sortedIndices[1]).Mean()[i]
        - d2.Mean()[i]), 0.4);

  for (size_t row = 0; row < 3; row++)
    for (size_t col = 0; col < 3; col++)
      BOOST_REQUIRE_SMALL((g.Component(sortedIndices[1]).Covariance()(row, col)
          - d2.Covariance()(row, col)), 0.7); // Big tolerance!  Lots of noise.

  // Third Gaussian (d3).
  BOOST_REQUIRE_SMALL(g.Weights()[sortedIndices[2]] - 0.4, 0.1);

  for (size_t i = 0; i < 3; ++i)
    BOOST_REQUIRE_SMALL((g.Component(sortedIndices[2]).Mean()[i]
        - d3.Mean()[i]), 0.4);

  for (size_t row = 0; row < 3; ++row)
    for (size_t col = 0; col < 3; ++col)
      BOOST_REQUIRE_SMALL((g.Component(sortedIndices[2]).Covariance()(row, col)
          - d3.Covariance()(row, col)), 0.7);
}

/**
 * Make sure generating observations randomly works.  We'll do this by
 * generating a bunch of random observations and then re-training on them, and
 * hope that our model is the same.
 */
BOOST_AUTO_TEST_CASE(GMMRandomTest)
{
  // Simple GMM distribution.
  GMM gmm(2, 2);
  gmm.Weights() = arma::vec("0.40 0.60");

  // N([2.25 3.10], [1.00 0.20; 0.20 0.89])
  gmm.Component(0) = distribution::GaussianDistribution("2.25 3.10",
      "1.00 0.60; 0.60 0.89");

  // N([4.10 1.01], [1.00 0.00; 0.00 1.01])
  gmm.Component(1) = distribution::GaussianDistribution("4.10 1.01",
      "1.00 0.70; 0.70 1.01");

  // Now generate a bunch of observations.
  arma::mat observations(2, 4000);
  for (size_t i = 0; i < 4000; i++)
    observations.col(i) = gmm.Random();

  // A new one which we'll train.
  GMM gmm2(2, 2);
  gmm2.Train(observations, 10);

  // Now check the results.  We need to order by weights so that when we do the
  // checking, things will be correct.
  arma::uvec sortedIndices = sort_index(gmm2.Weights());

  // Now check that the parameters are the same.  Tolerances are kind of big
  // because we only used 2000 observations.
  BOOST_REQUIRE_CLOSE(gmm.Weights()[0], gmm2.Weights()[sortedIndices[0]], 7.0);
  BOOST_REQUIRE_CLOSE(gmm.Weights()[1], gmm2.Weights()[sortedIndices[1]], 7.0);

  BOOST_REQUIRE_CLOSE(gmm.Component(0).Mean()[0],
      gmm2.Component(sortedIndices[0]).Mean()[0], 7.5);
  BOOST_REQUIRE_CLOSE(gmm.Component(0).Mean()[1],
      gmm2.Component(sortedIndices[0]).Mean()[1], 7.5);

  BOOST_REQUIRE_CLOSE(gmm.Component(0).Covariance()(0, 0),
      gmm2.Component(sortedIndices[0]).Covariance()(0, 0), 13.0);
  BOOST_REQUIRE_CLOSE(gmm.Component(0).Covariance()(0, 1),
      gmm2.Component(sortedIndices[0]).Covariance()(0, 1), 22.0);
  BOOST_REQUIRE_CLOSE(gmm.Component(0).Covariance()(1, 0),
      gmm2.Component(sortedIndices[0]).Covariance()(1, 0), 22.0);
  BOOST_REQUIRE_CLOSE(gmm.Component(0).Covariance()(1, 1),
      gmm2.Component(sortedIndices[0]).Covariance()(1, 1), 13.0);

  BOOST_REQUIRE_CLOSE(gmm.Component(1).Mean()[0],
      gmm2.Component(sortedIndices[1]).Mean()[0], 7.5);
  BOOST_REQUIRE_CLOSE(gmm.Component(1).Mean()[1],
      gmm2.Component(sortedIndices[1]).Mean()[1], 7.5);

  BOOST_REQUIRE_CLOSE(gmm.Component(1).Covariance()(0, 0),
      gmm2.Component(sortedIndices[1]).Covariance()(0, 0), 13.0);
  BOOST_REQUIRE_CLOSE(gmm.Component(1).Covariance()(0, 1),
      gmm2.Component(sortedIndices[1]).Covariance()(0, 1), 22.0);
  BOOST_REQUIRE_CLOSE(gmm.Component(1).Covariance()(1, 0),
      gmm2.Component(sortedIndices[1]).Covariance()(1, 0), 22.0);
  BOOST_REQUIRE_CLOSE(gmm.Component(1).Covariance()(1, 1),
      gmm2.Component(sortedIndices[1]).Covariance()(1, 1), 13.0);
}

/**
 * Test classification of observations by component.
 */
BOOST_AUTO_TEST_CASE(GMMClassifyTest)
{
  // First create a Gaussian with a few components.
  GMM gmm(3, 2);
  gmm.Component(0) = distribution::GaussianDistribution("0 0", "1 0; 0 1");
  gmm.Component(1) = distribution::GaussianDistribution("1 3", "3 2; 2 3");
  gmm.Component(2) = distribution::GaussianDistribution("-2 -2",
      "2.2 1.4; 1.4 5.1");
  gmm.Weights() = "0.6 0.25 0.15";

  arma::mat observations = arma::trans(arma::mat(
    " 0  0;"
    " 0  1;"
    " 0  2;"
    " 1 -2;"
    " 2 -2;"
    "-2  0;"
    " 5  5;"
    "-2 -2;"
    " 3  3;"
    "25 25;"
    "-1 -1;"
    "-3 -3;"
    "-5  1"));

  arma::Row<size_t> classes;

  gmm.Classify(observations, classes);

  // Test classification of points.  Classifications produced by hand.
  BOOST_REQUIRE_EQUAL(classes[ 0], 0);
  BOOST_REQUIRE_EQUAL(classes[ 1], 0);
  BOOST_REQUIRE_EQUAL(classes[ 2], 1);
  BOOST_REQUIRE_EQUAL(classes[ 3], 0);
  BOOST_REQUIRE_EQUAL(classes[ 4], 0);
  BOOST_REQUIRE_EQUAL(classes[ 5], 0);
  BOOST_REQUIRE_EQUAL(classes[ 6], 1);
  BOOST_REQUIRE_EQUAL(classes[ 7], 2);
  BOOST_REQUIRE_EQUAL(classes[ 8], 1);
  BOOST_REQUIRE_EQUAL(classes[ 9], 1);
  BOOST_REQUIRE_EQUAL(classes[10], 0);
  BOOST_REQUIRE_EQUAL(classes[11], 2);
  BOOST_REQUIRE_EQUAL(classes[12], 2);
}

BOOST_AUTO_TEST_CASE(GMMLoadSaveTest)
{
  // Create a GMM, save it, and load it.
  GMM gmm(10, 4);
  gmm.Weights().randu();

  for (size_t i = 0; i < gmm.Gaussians(); ++i)
  {
    gmm.Component(i).Mean().randu();
    arma::mat covariance = arma::randu<arma::mat>(
        gmm.Component(i).Covariance().n_rows,
        gmm.Component(i).Covariance().n_cols);
    covariance *= covariance.t();
    covariance += arma::eye<arma::mat>(covariance.n_rows, covariance.n_cols);
    gmm.Component(i).Covariance(std::move(covariance));
  }

  // Save the GMM.
  {
    std::ofstream ofs("test-gmm-save.xml");
    boost::archive::xml_oarchive ar(ofs);
    ar << data::CreateNVP(gmm, "gmm");
  }

  // Load the GMM.
  GMM gmm2;
  {
    std::ifstream ifs("test-gmm-save.xml");
    boost::archive::xml_iarchive ar(ifs);
    ar >> data::CreateNVP(gmm2, "gmm");
  }

  // Remove clutter.
  //remove("test-gmm-save.xml");

  BOOST_REQUIRE_EQUAL(gmm.Gaussians(), gmm2.Gaussians());
  BOOST_REQUIRE_EQUAL(gmm.Dimensionality(), gmm2.Dimensionality());

  for (size_t i = 0; i < gmm.Dimensionality(); ++i)
    BOOST_REQUIRE_CLOSE(gmm.Weights()[i], gmm2.Weights()[i], 1e-3);

  for (size_t i = 0; i < gmm.Gaussians(); ++i)
  {
    for (size_t j = 0; j < gmm.Dimensionality(); ++j)
      BOOST_REQUIRE_CLOSE(gmm.Component(i).Mean()[j],
          gmm2.Component(i).Mean()[j], 1e-3);

    for (size_t j = 0; j < gmm.Dimensionality(); ++j)
    {
      for (size_t k = 0; k < gmm.Dimensionality(); ++k)
      {
        BOOST_REQUIRE_CLOSE(gmm.Component(i).Covariance()(j, k),
            gmm2.Component(i).Covariance()(j, k), 1e-3);
      }
    }
  }
}

BOOST_AUTO_TEST_CASE(NoConstraintTest)
{
  // Generate random matrices and make sure they end up the same.
  for (size_t i = 0; i < 30; ++i)
  {
    const size_t rows = 5 + math::RandInt(100);
    const size_t cols = 5 + math::RandInt(100);
    arma::mat cov(rows, cols);
    cov.randu();
    arma::mat newcov(cov);

    NoConstraint::ApplyConstraint(newcov);

    for (size_t j = 0; j < cov.n_elem; ++j)
      BOOST_REQUIRE_CLOSE(newcov(j), cov(j), 1e-20);
  }
}

BOOST_AUTO_TEST_CASE(PositiveDefiniteConstraintTest)
{
  // Make sure matrices are made to be positive definite, or more specifically,
  // that they can be Cholesky decomposed.
  for (size_t i = 0; i < 30; ++i)
  {
    const size_t elem = 5 + math::RandInt(50);
    arma::mat cov(elem, elem);
    cov.randu();

    PositiveDefiniteConstraint::ApplyConstraint(cov);

    arma::mat c;
    #if (ARMA_VERSION_MAJOR < 4) || \
        ((ARMA_VERSION_MAJOR == 4) && (ARMA_VERSION_MINOR < 500))
    BOOST_REQUIRE(arma::chol(c, cov));
    #else
    BOOST_REQUIRE(arma::chol(c, cov, "lower"));
    #endif

  }
}

BOOST_AUTO_TEST_CASE(DiagonalConstraintTest)
{
  // Make sure matrices are made to be positive definite.
  for (size_t i = 0; i < 30; ++i)
  {
    const size_t elem = 5 + math::RandInt(50);
    arma::mat cov(elem, elem);
    cov.randu();

    DiagonalConstraint::ApplyConstraint(cov);

    for (size_t j = 0; j < elem; ++j)
      for (size_t k = 0; k < elem; ++k)
        if (j != k)
          BOOST_REQUIRE_SMALL(cov(j, k), 1e-50);
  }
}

BOOST_AUTO_TEST_CASE(EigenvalueRatioConstraintTest)
{
  // Generate a list of eigenvalue ratios.
  arma::vec ratios("1.0 0.7 0.4 0.2 0.1 0.1 0.05 0.01");
  EigenvalueRatioConstraint erc(ratios);

  // Now make some random matrices and see if the constraint works.
  for (size_t i = 0; i < 30; ++i)
  {
    arma::mat cov(8, 8);
    cov.randu();

    erc.ApplyConstraint(cov);

    // Decompose the matrix and make sure things are right.
    arma::vec eigenvalues = arma::eig_sym(cov);

    for (size_t i = 0; i < eigenvalues.n_elem; ++i)
      BOOST_REQUIRE_CLOSE(eigenvalues[i] / eigenvalues[0], ratios[i], 1e-5);
  }
}

BOOST_AUTO_TEST_CASE(UseExistingModelTest)
{
  // If we run a GMM and it converges, then if we run it again using the
  // converged results as the starting point, then it should terminate after one
  // iteration and give basically the same results.

  // Higher dimensionality gives us a greater chance of having separated
  // Gaussians.
  size_t dims = 8;
  size_t gaussians = 3;

  // Generate dataset.
  arma::mat data;
  data.zeros(dims, 500);

  std::vector<arma::vec> means(gaussians);
  std::vector<arma::mat> covars(gaussians);
  arma::vec weights(gaussians);
  arma::Col<size_t> counts(gaussians);

  // Choose weights randomly.
  weights.zeros();
  while (weights.min() < 0.02)
  {
    weights.randu(gaussians);
    weights /= accu(weights);
  }

  for (size_t i = 0; i < gaussians; i++)
    counts[i] = round(weights[i] * (data.n_cols - gaussians));
  // Ensure one point minimum in each.
  counts += 1;

  // Account for rounding errors (possibly necessary).
  counts[gaussians - 1] += (data.n_cols - arma::accu(counts));

  // Build each Gaussian individually.
  size_t point = 0;
  for (size_t i = 0; i < gaussians; i++)
  {
    arma::mat gaussian;
    gaussian.randn(dims, counts[i]);

    // Randomly generate mean and covariance.
    means[i].randu(dims);
    means[i] -= 0.5;
    means[i] *= 50;

    // We need to make sure the covariance is positive definite.  We will take a
    // random matrix C and then set our covariance to 4 * C * C', which will be
    // positive semidefinite.
    covars[i].randu(dims, dims);
    covars[i] *= 4 * trans(covars[i]);

    data.cols(point, point + counts[i] - 1) = (covars[i] * gaussian + means[i]
        * arma::ones<arma::rowvec>(counts[i]));

    // Calculate the actual means and covariances because they will probably
    // be different (this is easier to do before we shuffle the points).
    means[i] = arma::mean(data.cols(point, point + counts[i] - 1), 1);
    covars[i] = ccov(data.cols(point, point + counts[i] - 1), 1 /* biased */);

    point += counts[i];
  }

  // Calculate actual weights.
  for (size_t i = 0; i < gaussians; i++)
    weights[i] = (double) counts[i] / data.n_cols;

  // Now train the model.
  GMM gmm(gaussians, dims);
  gmm.Train(data, 10);

  GMM oldgmm(gmm);

  // Retrain the model with the existing model as the starting point.
  gmm.Train(data, 1, true);

  // Check for similarity.
  for (size_t i = 0; i < gmm.Gaussians(); ++i)
  {
    BOOST_REQUIRE_CLOSE(gmm.Weights()[i], oldgmm.Weights()[i], 1e-4);

    for (size_t j = 0; j < gmm.Dimensionality(); ++j)
    {
      BOOST_REQUIRE_CLOSE(gmm.Component(i).Mean()[j],
                          oldgmm.Component(i).Mean()[j], 1e-3);

      for (size_t k = 0; k < gmm.Dimensionality(); ++k)
        BOOST_REQUIRE_CLOSE(gmm.Component(i).Covariance()(j, k),
                            oldgmm.Component(i).Covariance()(j, k), 1e-3);
    }
  }

  // Do it again, with a larger number of trials.
  gmm = oldgmm;

  // Retrain the model with the existing model as the starting point.
  gmm.Train(data, 10, true);

  // Check for similarity.
  for (size_t i = 0; i < gmm.Gaussians(); ++i)
  {
    BOOST_REQUIRE_CLOSE(gmm.Weights()[i], oldgmm.Weights()[i], 1e-4);

    for (size_t j = 0; j < gmm.Dimensionality(); ++j)
    {
      BOOST_REQUIRE_CLOSE(gmm.Component(i).Mean()[j],
                          oldgmm.Component(i).Mean()[j], 1e-3);

      for (size_t k = 0; k < gmm.Dimensionality(); ++k)
        BOOST_REQUIRE_CLOSE(gmm.Component(i).Covariance()(j, k),
                            oldgmm.Component(i).Covariance()(j, k), 1e-3);
    }
  }

  // Do it again, but using the overload of Train() that takes probabilities
  // into account.
  arma::vec probabilities(data.n_cols);
  probabilities.ones(); // Fill with ones.

  gmm = oldgmm;
  gmm.Train(data, probabilities, 1, true);

  // Check for similarity.
  for (size_t i = 0; i < gmm.Gaussians(); ++i)
  {
    BOOST_REQUIRE_CLOSE(gmm.Weights()[i], oldgmm.Weights()[i], 1e-4);

    for (size_t j = 0; j < gmm.Dimensionality(); ++j)
    {
      BOOST_REQUIRE_CLOSE(gmm.Component(i).Mean()[j],
          oldgmm.Component(i).Mean()[j], 1e-3);

      for (size_t k = 0; k < gmm.Dimensionality(); ++k)
        BOOST_REQUIRE_CLOSE(gmm.Component(i).Covariance()(j, k),
                            oldgmm.Component(i).Covariance()(j, k), 1e-3);
    }
  }

  // One more time, with multiple trials.
  gmm = oldgmm;
  gmm.Train(data, probabilities, 10, true);

  // Check for similarity.
  for (size_t i = 0; i < gmm.Gaussians(); ++i)
  {
    BOOST_REQUIRE_CLOSE(gmm.Weights()[i], oldgmm.Weights()[i], 1e-4);

    for (size_t j = 0; j < gmm.Dimensionality(); ++j)
    {
      BOOST_REQUIRE_CLOSE(gmm.Component(i).Mean()[j],
          oldgmm.Component(i).Mean()[j], 1e-3);

      for (size_t k = 0; k < gmm.Dimensionality(); ++k)
        BOOST_REQUIRE_CLOSE(gmm.Component(i).Covariance()(j, k),
                            oldgmm.Component(i).Covariance()(j, k), 1e-3);
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef QATUX_GENERAL
#define QATUX_GENERAL

#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <Eigen/KroneckerProduct>
#include <bitset>
#include <cmath>
#include <random>
#include <assert.h>

namespace Qatux {
    template<typename T = float>
    using Complex = std::complex<T>;

    template<typename T = float>
    using Vector = Eigen::Matrix<Complex<T>, Eigen::Dynamic, 1>;

    template<typename T = float>
    using Matrix = Eigen::SparseMatrix<Complex<T>>;

    int pow2(int exp);
    void printBin(int N, int BITS);

    //return |0>
    template<typename T = float>
    Vector<T> zero(void);

    //return |1>
    template<typename T = float>
    Vector<T> one(void);

    template<typename T = float>
    Matrix<T> identityGate(int NQUBITS);

    //returns |N> for a space of size NQUBITS
    template<typename T = float>
    Vector<T> basis(int N, int NQUBITS);
}

#endif


//
// Created by lei on 4/22/19.
//

#include "catch.hpp"

#include "tgo.hpp"

#include <armadillo>
#include <vector>
#include <cmath>
#include <functional>
#include <fmt/format.h>

int count = 0;

double func(double x) {
    count += 1;
    return std::pow(x - 0.5, 2);
}

TEST_CASE("x^2", "[tgo]") {
    int nsample = 1e2;
    int nk = 4;
    double xmin = -1.0;
    double xmax = 1.0;
    double xtol = 1.0e-7;
    TGO tgo(func, nsample, nk, xmin, xmax, xtol);
    OptimizeResult result = tgo.optimize();
    fmt::print("count: {:d}\n", count);
    REQUIRE(result.x == Approx(0.5));
}


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/gil/point.hpp>
#include <cmath>
#include <cstddef>

namespace boost
{
namespace gil
{
/// \defgroup Rasterization
/// \brief A set of functions to rasterize shapes
///
/// Due to images being discrete, most shapes require specialized algorithms to
/// handle rasterization efficiently and solve problem of connectivity and being
/// close to the original shape.

/// \defgroup LineRasterization
/// \ingroup Rasterization
/// \brief A set of rasterizers for lines
///
/// The main problem with line rasterization is to do it efficiently, e.g. less
/// floating point operations. There are multiple algorithms that on paper
/// should reach the same result, but due to quirks of IEEE-754 they don't.
/// Please select one and stick to it if possible. At the moment only Bresenham
/// rasterizer is implemented.

/// \ingroup LineRasterization
/// \brief Rasterize a line according to Bresenham algorithm
///
/// Do note that if either width or height is 1, slope is set to zero.
/// reference:
/// https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm#:~:text=Bresenham's%20line%20algorithm%20is%20a,straight%20line%20between%20two%20points.
struct bresenham_line_rasterizer
{
    constexpr std::ptrdiff_t point_count(std::ptrdiff_t width, std::ptrdiff_t height) const noexcept
    {
        return width > height ? width : height;
    }

    std::ptrdiff_t point_count(point_t start, point_t end) const noexcept
    {
        const auto abs_width = std::abs(end.x - start.x) + 1;
        const auto abs_height = std::abs(end.y - start.y) + 1;
        return point_count(abs_width, abs_height);
    }

    template <typename RandomAccessIterator>
    void operator()(point_t start, point_t end, RandomAccessIterator d_first) const
    {
        if (start == end)
        {
            // put the point and immediately exit, as later on division by zero will
            // occur
            *d_first = start;
            return;
        }

        auto width = std::abs(end.x - start.x) + 1;
        auto height = std::abs(end.y - start.y) + 1;
        bool const needs_flip = width < height;
        if (needs_flip)
        {
            // transpose the coordinate system if uncomfortable angle detected
            std::swap(width, height);
            std::swap(start.x, start.y);
            std::swap(end.x, end.y);
        }
        std::ptrdiff_t const x_increment = end.x >= start.x ? 1 : -1;
        std::ptrdiff_t const y_increment = end.y >= start.y ? 1 : -1;
        double const slope =
            height == 1 ? 0 : static_cast<double>(height) / static_cast<double>(width);
        std::ptrdiff_t y = start.y;
        double error_term = 0;
        for (std::ptrdiff_t x = start.x; x != end.x; x += x_increment)
        {
            // transpose coordinate system back to proper form if needed
            *d_first++ = needs_flip ? point_t{y, x} : point_t{x, y};
            error_term += slope;
            if (error_term >= 0.5)
            {
                --error_term;
                y += y_increment;
            }
        }
        *d_first++ = needs_flip ? point_t{end.y, end.x} : end;
    }
};

}} // namespace boost::gil


// Copyright(c) 2019-present, Alexander Silva Barbosa & bflib contributors.
// Distributed under the MIT License (http://opensource.org/licenses/MIT)

/**
 * @author Alexander Silva Barbosa <alexander.ti.ufv@gmail.com>
 * @date 2019
 * Kalman Filter
 */

#pragma once

#include <Eigen/Dense>
#include <random>
#include <chrono>
#include <vector>
#include <thread>

using namespace Eigen;

template <typename dataType, int states, int inputs, int outputs>
class KF
{
    private:
        typedef Matrix<dataType, states, 1> MatNx1;
        typedef Matrix<dataType, states, states> MatNxN;
        typedef Matrix<dataType, states, inputs> MatNxM;
        typedef Matrix<dataType, states, outputs> MatNxP;
        typedef Matrix<dataType, outputs, states> MatPxN;
        typedef Matrix<dataType, outputs, outputs> MatPxP;
        typedef Matrix<dataType, inputs, 1> MatMx1;
        typedef Matrix<dataType, outputs, 1> MatPx1;
        typedef Matrix<dataType, 3, 1> Mat3x1;

    public:
        typedef MatNx1 State;
        typedef MatMx1 Input;
        typedef MatPx1 Output;
        typedef Input Control;
        typedef Output Sensor;
        typedef MatNxN ModelCovariance;
        typedef MatPxP SensorCovariance;
        typedef MatNxN StateMatrix;
        typedef MatNxM InputMatrix;
        typedef MatPxN OutputMatrix;
        typedef Mat3x1 Uncertainty;

    private:
        typedef void (*ProcessFunction)(StateMatrix &A, InputMatrix &B, OutputMatrix &C, double dt);

        std::default_random_engine gen;
        std::normal_distribution<double> distr{0.0, 1.0};
        std::chrono::time_point<std::chrono::high_resolution_clock> start;

        State x;
        StateMatrix A;
        ModelCovariance Q;
        InputMatrix B;
        OutputMatrix C;
        SensorCovariance R;

        MatNx1 randX;
        MatPx1 randY;

        MatNxN P;
        MatNxN Qsqrt;
        MatPxP Rsqrt;

        Output z, yError;
        MatPxP S;
        MatNxP K;
        MatNxN I;

        ProcessFunction processFn;

        void init()
        {
            processFn = NULL;
            P = Q;
            I.setIdentity();

            Qsqrt = Q.cwiseSqrt();
            Rsqrt = R.cwiseSqrt();

            start = std::chrono::high_resolution_clock::now();
        }
    public:
        KF()
        {
            Q.setIdentity();
            R.setIdentity();
            x.setZero();
            init();
        }

        KF(State X) : x(X)
        {
            Q.setIdentity();
            R.setIdentity();
            init();
        }

        KF(ModelCovariance Q, SensorCovariance R) : Q(Q), R(R)
        {
            x.setZero();
            init();
        }

        KF(State X, ModelCovariance Q, SensorCovariance R) : x(X), Q(Q), R(R)
        {
            init();
        }

        virtual ~KF()
        {

        }

        void seed()
        {
            gen = std::default_random_engine(std::chrono::system_clock::now().time_since_epoch().count());
        }

        void seed(long long s)
        {
            gen = std::default_random_engine(s);
        }

        State state()
        {
            State x;
            x.setZero();
            return x;
        }

        Input input()
        {
            Input u;
            u.setZero();
            return u;
        }

        Output output()
        {
            Output y;
            y.setZero();
            return y;
        }

        ModelCovariance createQ()
        {
            ModelCovariance Q;
            Q.setZero();
            return Q;
        }

        SensorCovariance createR()
        {
            SensorCovariance R;
            R.setZero();
            return R;
        }

        ModelCovariance getP()
        {
            return P;
        }

        Uncertainty getUncertainty(unsigned int x1, unsigned int x2)
        {
            Uncertainty C;
            C.setZero();
            if(x1 >= states || x2 >= states)
                return C;

            Matrix<dataType, 2, 2> p;
            p(0, 0) = P(x1, x1);
            p(0, 1) = P(x1, x2);
            p(1, 0) = P(x2, x1);
            p(1, 1) = P(x2, x2);

            EigenSolver< Matrix<dataType, 2, 2> > es(p);
            Matrix<dataType, 2, 2> eValue = es.pseudoEigenvalueMatrix();
            Matrix<dataType, 2, 2> eVector = es.pseudoEigenvectors();

            C[0] = eValue(0,0);
            C[1] = eValue(1,1);
            C[2] = std::atan2(eVector(0, 1), eVector(0, 0));

            return C;
        }

        void setQ(ModelCovariance Q)
        {
            this->Q = Q;
            P = Q;
            Qsqrt = Q.cwiseSqrt();
        }

        void setR(SensorCovariance R)
        {
            this->R = R;
            Rsqrt = R.cwiseSqrt();
        }

        double time()
        {
            auto end = std::chrono::high_resolution_clock::now();
            std::chrono::duration<double> diff = end - start;
            start = std::chrono::high_resolution_clock::now();
            return diff.count();
        }

        double delay(double s)
        {
            double ellapsed = time();
            double remain = s - ellapsed;
            if(remain < 0)
                return ellapsed;
            std::this_thread::sleep_for(std::chrono::nanoseconds((long long)(remain * 1e9)));
            ellapsed += time();
            return ellapsed;
        }

        void setProcess(ProcessFunction fn)
        {
            processFn = fn;
        }

        virtual void process(StateMatrix &A, InputMatrix &B, OutputMatrix &C, double dt)
        {

        }

        void simulate(State &x, Output &y, Input &u, double dt)
        {
            doProcess(dt);

            randn(randX);
            randn(randY);

            x = A * x + B * u + Qsqrt * randX;
            y = C * x + Rsqrt * randY;
        }

        void run(State &xK, Output &y, Input &u, double dt)
        {
            predict(u, dt);
            update(y);
            xK = x;
        }

    private:
        void predict(Input &u, double dt)
        {
            doProcess(dt);
            x = A * x + B * u;
            P = A * P * A.transpose() + Q;
        }

        void update(Output &y)
        {
            z = C * x;
            yError = y - z;

            S = C * P * C.transpose() + R;
            K = P * C.transpose() * S.inverse();
            x = x + K * yError;

            P = (I - K * C) * P;
        }

        void doProcess(double dt)
        {
            if(processFn != NULL)
                processFn(A, B, C, dt);
            else
                process(A, B, C, dt);
        }

        template<class T>
        void randn(T &mat)
        {
            for (size_t i = 0; i < mat.rows(); i++)
            {
                for (size_t j = 0; j < mat.cols(); j++)
                {
                    mat(i, j) = distr(gen);
                }
            }
        }

};


/* This file (C) 2008 Maik Beckmann
 * https://lists.boost.org/MailArchives/ublas/2008/09/2984.php
 */

#ifndef determinant_hpp
#define determinant_hpp

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>

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

namespace ublas = boost::numeric::ublas;

template<class matrix_T>
double determinant(ublas::matrix_expression<matrix_T> const& mat_r)
{
  double det = 1.0;

  matrix_T mLu(mat_r());
  ublas::permutation_matrix<std::size_t> pivots(mat_r().size1());

  int is_singular = lu_factorize(mLu, pivots);

  if (!is_singular)
  {
    for (std::size_t i=0; i < pivots.size(); ++i)
    {
      if (pivots(i) != i)
        det *= -1.0;

      det *= mLu(i,i);
    }
  }
  else
    det = 0.0;

  return det;
}

#endif


// BLAS level 1 (vectors)

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/atlas/cblas1.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#endif
#include "utils.h"

namespace atlas = boost::numeric::bindings::atlas;
namespace ublas = boost::numeric::ublas;

using std::cout;
using std::endl;
using std::size_t;

typedef double real_t;

#ifndef F_USE_STD_VECTOR
typedef ublas::vector<real_t> vct_t;
#else
typedef ublas::vector<real_t, std::vector<double> > vct_t;
#endif

int main() {

  cout << endl;

  vct_t v (10);
  init_v (v, times_plus<real_t> (0.1, 0.1));
  print_v (v, "v");
  vct_t vy (10);
  atlas::set (1., vy);
  print_v (vy, "vy");
  cout << endl;

  // v <- 2 v
  atlas::scal (2.0, v);
  print_v (v, "v <- 2 v");

  // vy <- 0.5 v + vy
  atlas::axpy (0.5, v, vy);
  print_v (vy, "vy <- 0.5 v + vy");

  // v^T vy
  cout << "v vy = " << atlas::dot (v, vy) << endl;
  cout << endl;

  /////////////////
  // ranges

  // v -- new init
  init_v (v, times_plus<real_t> (0.1, 0.1));
  print_v (v, "v");
  // vy -- new init
  init_v (vy, kpp (1));
  print_v (vy, "vy");

  // v[2..6]
  ublas::vector_range<vct_t> vr (v, ublas::range (2, 6));
  print_v (vr, "v[2..6]");
  // vy[4..8]
  ublas::vector_range<vct_t> vry (vy, ublas::range (4, 8));
  print_v (vry, "vy[4..8]");

  // v[2..6] <- 0.1 v[2..6]
  atlas::scal (0.1, vr);
  print_v (v, "v[2..6] <- 0.1 v[2..6]");

  // vr^T vr
  // ublas::vector_range<vct_t const> cvr (v, ublas::range (2, 6));
  cout << "v[2..6] v[2..6] = " << atlas::dot (vr, vr) << endl;

  // vy[4..8] <- v[2..6] + vy[4..8]
  atlas::xpy (vr, vry);
  print_v (vy, "vy[4..8] <- v[2..6] + vy[4..8]");
  cout << endl;

  /////////////////
  // slices

  // v -- new init
  init_v (v, times_plus<real_t> (0.1, 0.1));
  print_v (v, "v");
  // vy -- new init
  init_v (vy, kpp (1));
  print_v (vy, "vy");

  // v[1:2:4]
  ublas::vector_slice<vct_t> vs (v, ublas::slice (1, 2, 4));
  print_v (vs, "v[1:2:4]");
  // vy[2:2:4]
  ublas::vector_slice<vct_t> vsy (vy, ublas::slice (2, 2, 4));
  print_v (vsy, "vy[2:2:4]");

  // v[1:2:4] <- 10 v[1:2:4]
  atlas::scal (10.0, vs);
  print_v (v, "v[1:2:4] <- 10 v[1:2:4]");

  // vs^T vs
  cout << "v[1:2:4] v[1:2:4] = " << atlas::dot (vs, vs) << endl;

  // vy[2:2:4] <- 0.01 v[1:2:4] + vy[2:2:4]
  atlas::axpy (0.01, vs, vsy);
  print_v (vy, "vy[2:2:4] <- 0.01 v[1:2:4] + vy[2:2:4]");
  cout << endl;

  ////////////////////////////////////////////
  // ranges & slices

  // v -- new init
  init_v (v, times_plus<real_t> (0.1, 0.1));
  print_v (v, "v");
  // vy <- 1.0
  atlas::set (1., vy);
  print_v (vy, "vy <- 1.0");

  // vy[2:2:4] <- 0.01 v[2..6] + vy[2:2:4]
  atlas::axpy (0.01, vr, vsy);
  print_v (vy, "vy[2:2:4] <- 0.01 v[2..6] + vy[2:2:4]");

  // vy <- 1.0
  atlas::set (1., vy);
  print_v (vy, "vy <- 1.0");

  // vy[4..8] <- 0.01 v[1:2:4] + vy[4..8]
  atlas::axpy (0.01, vs, vry);
  print_v (vy, "vy[4..8] <- 0.01 v[1:2:4] + vy[4..8]");

  // vr^T vs == vs^T vr
  cout << "v[2..6] v[1:2:4] = " << atlas::dot (vr, vs) << " == "
       << atlas::dot (vs, vr) << endl;

  // vr^T vsy == vsy^T vr
  cout << "v[2..6] vy[2:2:4] = " << atlas::dot (vr, vsy) << " == "
       << atlas::dot (vsy, vr) << endl;

  cout << endl;

  ///////////////////
  // slice of range

  // v -- new init
  init_v (v, times_plus<real_t> (0.1, 0.1));
  print_v (v, "v");

  // v[1..9][1:2:3]
  ublas::vector_range<vct_t> vr1 (v, ublas::range (1, 9));
  ublas::vector_slice<
    ublas::vector_range<vct_t>
  > vsr (vr1, ublas::slice (1, 2, 3));
  print_v (vr1, "v[1..9]");
  print_v (vsr, "v[1..9][1:2:3]");

  // v[1..9][1:2:3] <- 0.1 v[1..9][1:2:3]
  atlas::scal (0.1, vsr);
  print_v (v, "0.1 v[1..9][1:2:3]");

  // ||vsr||_2
  cout << "||v[1..9][1:2:3]||_2 = " << atlas::nrm2 (vsr) << endl;
  cout << endl;

  ///////////////////
  // range of slice

  // vy <- 1.0
  init_v (vy, kpp (1));
  print_v (vy, "vy");

  // v[0:2:5][1:4]
  ublas::vector_slice<vct_t> vsy1 (vy, ublas::slice (0, 2, 5));
  ublas::vector_range<
    ublas::vector_slice<vct_t>
  > vrs (vsy1, ublas::range (1, 4));
  print_v (vsy1, "v[0:2:5]");
  print_v (vrs, "v[0:2:5][1:4]");

  // v[0:2:5][1:4] <- 0.01 v[1..9][1:2:3] + v[0:2:5][1:4]
  atlas::axpy (0.01, vsr, vrs);
  print_v (vy, "0.01 v[1..9][1:2:3] + v[0:2:5][1:4]");

  cout << endl;

}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Mark Borgerding mark a borgerding net
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <iostream>

#include <bench/BenchUtil.h>
#include <complex>
#include <vector>
#include <Eigen/Core>

#include <unsupported/Eigen/FFT>

using namespace Eigen;
using namespace std;

template <typename T>
string nameof();

template <> string nameof<float>() {return "float";}
template <> string nameof<double>() {return "double";}
template <> string nameof<long double>() {return "long double";}

#ifndef TYPE
#define TYPE float
#endif

#ifndef NFFT
#define NFFT 1024
#endif
#ifndef NDATA
#define NDATA 1000000
#endif

using namespace Eigen;

template <typename T>
void bench(int nfft,bool fwd,bool unscaled=false, bool halfspec=false)
{
    typedef typename NumTraits<T>::Real Scalar;
    typedef typename std::complex<Scalar> Complex;
    int nits = NDATA/nfft;
    vector<T> inbuf(nfft);
    vector<Complex > outbuf(nfft);
    FFT< Scalar > fft;

    if (unscaled) {
        fft.SetFlag(fft.Unscaled);
        cout << "unscaled ";
    }
    if (halfspec) {
        fft.SetFlag(fft.HalfSpectrum);
        cout << "halfspec ";
    }

    std::fill(inbuf.begin(),inbuf.end(),0);
    fft.fwd( outbuf , inbuf);

    BenchTimer timer;
    timer.reset();
    for (int k=0;k<8;++k) {
        timer.start();
        if (fwd)
            for(int i = 0; i < nits; i++)
                fft.fwd( outbuf , inbuf);
        else
            for(int i = 0; i < nits; i++)
                fft.inv(inbuf,outbuf);
        timer.stop();
    }

    cout << nameof<Scalar>() << " ";
    double mflops = 5.*nfft*log2((double)nfft) / (1e6 * timer.value() / (double)nits );
    if ( NumTraits<T>::IsComplex ) {
        cout << "complex";
    }else{
        cout << "real   ";
        mflops /= 2;
    }

    if (fwd)
        cout << " fwd";
    else
        cout << " inv";

    cout << " NFFT=" << nfft << "  " << (double(1e-6*nfft*nits)/timer.value()) << " MS/s  " << mflops << "MFLOPS\n";
}

int main(int argc,char ** argv)
{
    bench<complex<float> >(NFFT,true);
    bench<complex<float> >(NFFT,false);
    bench<float>(NFFT,true);
    bench<float>(NFFT,false);
    bench<float>(NFFT,false,true);
    bench<float>(NFFT,false,true,true);

    bench<complex<double> >(NFFT,true);
    bench<complex<double> >(NFFT,false);
    bench<double>(NFFT,true);
    bench<double>(NFFT,false);
    bench<complex<long double> >(NFFT,true);
    bench<complex<long double> >(NFFT,false);
    bench<long double>(NFFT,true);
    bench<long double>(NFFT,false);
    return 0;
}


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>
#include <boost/math/special_functions/erf.hpp>

TEST(AgradRev, erf) {
  AVAR a = 1.3;
  AVAR f = erf(a);
  EXPECT_FLOAT_EQ(stan::math::erf(1.3), f.val());

  AVEC x = createAVEC(a);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(2.0 / std::sqrt(boost::math::constants::pi<double>())
                      * std::exp(-1.3 * 1.3),
                  grad_f[0]);
}
struct erf_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return erf(arg1);
  }
};

TEST(AgradRev, erf_NaN) {
  erf_fun erf_;
  test_nan(erf_, false, true);
}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a = 1.3;
  test::check_varis_on_stack(stan::math::erf(a));
}


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include <srook/math/vector.hpp>
#include <srook/math/matrix.hpp>
#include <srook/type_traits/is_same.hpp>
#include <srook/type_traits/detail/logical.hpp>
#include <srook/type_traits/decay.hpp>
#include <srook/tmpl/vt/map.hpp>
#include <srook/math/constants/algorithm/abs.hpp>

#include <boost/type_index.hpp>

BOOST_AUTO_TEST_SUITE(srook_math_vector_test)

BOOST_AUTO_TEST_CASE(vector_construct)
{
    srook::math::vector<int, double> vec0;
    SROOK_ST_ASSERT(std::tuple_size<SROOK_DECLTYPE(vec0)>::value == 2);
    SROOK_ST_ASSERT(srook::type_traits::detail::Land<
        srook::is_same<SROOK_DEDUCED_TYPENAME std::tuple_element<0, SROOK_DECLTYPE(vec0)>::type, int>, srook::is_same<SROOK_DEDUCED_TYPENAME std::tuple_element<1, SROOK_DECLTYPE(vec0)>::type, double>
    >::value);
    BOOST_CHECK_EQUAL(vec0.get<0>(), 0);
    BOOST_CHECK_EQUAL(vec0.get<1>(), 0.0);

    constexpr auto vec1 =
#if SROOK_CPLUSPLUS >= SROOK_CPLUSPLUS17_CONSTANT
        srook::math::vector(1, 2.f, 3.0)
#else
        srook::math::make_vector(1, 2.f, 3.0)
#endif
        ;
    SROOK_ST_ASSERT(srook::is_same<SROOK_DECLTYPE(vec1), const srook::math::vector<int, float, double>>::value);
    SROOK_ST_ASSERT(vec1.get<0>() == 1);
    SROOK_ST_ASSERT(vec1.get<1>() == 2.f);
    SROOK_ST_ASSERT(vec1.get<2>() == 3.0);
    SROOK_ST_ASSERT(srook::tmpl::vt::and_<SROOK_DEDUCED_TYPENAME srook::tmpl::vt::map<srook::is_equality_comparable, srook::tmpl::vt::packer<SROOK_DECLTYPE(vec0), SROOK_DECLTYPE(vec1)>>::type>::value);
}

BOOST_AUTO_TEST_CASE(vector_copy_construct)
{
    constexpr auto vec0 = srook::math::make_vector(1, 2.f, 3.0);
    constexpr auto vec1 = vec0;
    SROOK_ST_ASSERT(srook::is_same<SROOK_DECLTYPE(vec0), SROOK_DECLTYPE(vec1)>::value);
    SROOK_ST_ASSERT(vec0 == vec1);
}

BOOST_AUTO_TEST_CASE(vector_assign)
{
    constexpr auto vec0 = srook::math::make_vector(1, 2.f);
    srook::math::vector<int, float> vec1;
    vec1 = vec0;
    BOOST_CHECK_EQUAL(vec0, vec1);
}

BOOST_AUTO_TEST_CASE(vector_length_equality)
{
    typedef srook::numeric_limits<srook::floatmax_t> result_float;
    auto vec0 = srook::math::make_vector(4.0, 3.0);
    BOOST_TEST(srook::math::abs(vec0.length() - 5) < result_float::epsilon());
    SROOK_ST_ASSERT(srook::math::abs(srook::math::make_vector(12, 5).length() - 13) < result_float::epsilon());
    SROOK_ST_ASSERT(srook::math::abs(srook::math::make_vector(8, 15).length() - 17) < result_float::epsilon());

    BOOST_TEST(!vec0.is_unit());
    if (vec0.normalize()) { // When vec0.length() == 0, it returns false.
        BOOST_TEST(vec0.is_unit());
        BOOST_TEST(srook::math::abs(vec0.get<0>() - 0.8) < srook::numeric_limits<double>::epsilon());
        BOOST_TEST(srook::math::abs(vec0.get<1>() - 0.6) < srook::numeric_limits<double>::epsilon());
    }
    srook::math::make_vector(4.0, 3.0).get_normalized<double>() >>= [](const auto& normalized_vec) { // When vec0.length() == 0, it returns nullopt. Otherwise, it returns optional<vector<...>>.
        BOOST_CHECK_EQUAL(normalized_vec, srook::math::make_vector(0.8, 0.6)); // This equivalence is handled appropriately about floating error.
        return srook::make_optional(normalized_vec);
    };
}

BOOST_AUTO_TEST_CASE(vector_dot_product)
{
    SROOK_ST_ASSERT(!srook::math::make_vector(3, 5).dot_product(5, -3));
    SROOK_ST_ASSERT(srook::math::make_vector(1, 2).dot_product(3, 4) == 11);
    SROOK_ST_ASSERT(srook::math::make_vector(1, 2).dot_product(-2, 3) == 4);
}

BOOST_AUTO_TEST_CASE(vector_cross_product)
{
    SROOK_ST_ASSERT(srook::math::make_vector(1, 2, 3).cross_product(4, 5, 6) == srook::math::make_vector(-3, 6, -3));
}

BOOST_AUTO_TEST_CASE(vector_projection)
{
    SROOK_ST_ASSERT(srook::math::make_vector(1.f, 2.f, 2.f).projection(4.f, 0.f, 3.f) == srook::math::make_vector(1.6f, .0f, 1.2f));
}

BOOST_AUTO_TEST_CASE(vector_perpendicular)
{
    SROOK_ST_ASSERT(srook::math::make_vector(1.f, 2.f, 2.f).perpendicular(4.f, 0.f, 3.f) == srook::math::make_vector(-0.6f, 2.f, .8f));
}

BOOST_AUTO_TEST_CASE(vector_compute_triangle_from_points)
{
    // Compute the area of the triangle for P1 = (1, 3, 0), P2 = (3, 4, 0), P3 = (2, 6, 0)
    // in compile time.
    using namespace srook::math;

    constexpr auto p1 = make_vector(1, 3, 0), p2 = make_vector(3, 4, 0), p3 = make_vector(2, 6, 0);
    SROOK_ST_ASSERT(
        ((p2 - p1).cross_product(p3 - p1).length() / 2 - 2.5) < srook::numeric_limits<double>::epsilon()
    );
}

BOOST_AUTO_TEST_CASE(vector_matrix_conversion)
{
    constexpr auto vec0 = srook::math::make_vector(1, 3, 0);
    constexpr SROOK_DEDUCED_TYPENAME srook::tmpl::vt::transfer<
        srook::math::linear_algebra::detail::matrix_impl,
        SROOK_DEDUCED_TYPENAME srook::tmpl::vt::replicate<
            SROOK_DECLTYPE(vec0)::size,
            SROOK_DEDUCED_TYPENAME srook::tmpl::vt::transfer<
                srook::math::row,
                SROOK_DEDUCED_TYPENAME srook::tmpl::vt::foldr1<std::common_type, SROOK_DEDUCED_TYPENAME SROOK_DECLTYPE(vec0)::packed_type>::type
            >::type
        >::type
    >::type mat0 = srook::math::make_matrix(vec0);
    constexpr auto vec1 = srook::math::make_vector(mat0);
    SROOK_ST_ASSERT(vec0 == vec1);
    SROOK_ST_ASSERT(vec0 == mat0);

    constexpr SROOK_DECLTYPE(mat0) mat1 { 2, 3, 0 };
    SROOK_ST_ASSERT(vec0 != mat1);
    SROOK_ST_ASSERT(srook::math::make_vector(2, 3, 0) == mat1);
}

BOOST_AUTO_TEST_SUITE_END()


#include <tiny_math_types.h>
#include <tiny_matrix_functions.h>
#include <tiny_covariance3x3.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(tiny_covariance3x3);

BOOST_AUTO_TEST_CASE(simple_test)
{

  typedef tiny::MathTypes<double> math_types;

  typedef  math_types::matrix3x3_type   matrix3x3_type;
  typedef  math_types::vector3_type     vector3_type;

  matrix3x3_type C;
  vector3_type mean;

  vector3_type samples[100];

  for( size_t i=0;i<100;++i)
    samples[i] = vector3_type::random(  );

  tiny::covariance( samples, samples+100, mean, C );

  BOOST_CHECK_CLOSE( C(1,0), C(0,1), 0.01 );
  BOOST_CHECK_CLOSE( C(2,0), C(0,2), 0.01 );
  BOOST_CHECK_CLOSE( C(2,1), C(1,2), 0.01 );

  BOOST_CHECK( mean(0) > 0.0 );
  BOOST_CHECK( mean(0) < 1.0 );
  BOOST_CHECK( mean(1) > 0.0 );
  BOOST_CHECK( mean(1) < 1.0 );
  BOOST_CHECK( mean(2) > 0.0 );
  BOOST_CHECK( mean(2) < 1.0 );

  matrix3x3_type C1 = C;
  vector3_type mean1 = mean;

  matrix3x3_type C2 = C;
  vector3_type mean2 = mean;

  matrix3x3_type C3;
  vector3_type mean3;
  tiny::covariance_union( mean1, C1, mean2, C2, mean3, C3 );

  BOOST_CHECK_CLOSE( C3(1,0), C3(0,1), 0.01 );
  BOOST_CHECK_CLOSE( C3(2,0), C3(0,2), 0.01 );
  BOOST_CHECK_CLOSE( C3(2,1), C3(1,2), 0.01 );

  BOOST_CHECK( mean3(0) > 0.0 );
  BOOST_CHECK( mean3(0) < 1.0 );
  BOOST_CHECK( mean3(1) > 0.0 );
  BOOST_CHECK( mean3(1) < 1.0 );
  BOOST_CHECK( mean3(2) > 0.0 );
  BOOST_CHECK( mean3(2) < 1.0 );

}

BOOST_AUTO_TEST_SUITE_END();


#ifndef _TESTSHP_CPP__
#define _TESTSHP_CPP__

#include <iostream>
#include <vector>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <time.h>
#include <stdlib.h>
#include "OrientedGraph.h"
#include "UnorientedGraphValuedEdge.hpp"
#include "GraphManager.h"
#include "AdjacencyMatrixUnoriented.h"

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property,
                               boost::property<boost::edge_weight_t,int> > BoostGraph;
typename BoostGraph::vertex_descriptor u, v;
typedef typename BoostGraph ::edge_property_type Weight;

int randInt()
{
    return rand() % 256;
}

void TestShortestPaths(double probability)
{
    const int SIZE = 10000;
    UnorientedGraphValuedEdge<int>* g = new UnorientedGraphValuedEdge<int>(SIZE, new AdjacencyMatrixUnoriented());
    g->RandomizeGraph(probability, &randInt);
    clock_t start_t, end_t;
    std::cout << "Probability: " << probability << std::endl;
    double sumBFS = 0;
    double sumDj = 0;
    double sumBDj = 0;
    std::vector<int> requests;
    for(int i = 0; i < 20; ++i)
    {
        int u = rand() % SIZE;
        int v = rand() % SIZE;
        requests.push_back(u);
        start_t = clock();
        g->BFS(u, v);
        end_t = clock();
        sumBFS += (float)(end_t - start_t)/CLOCKS_PER_SEC;

        start_t = clock();
        g->Dijkstra(u, v);
        end_t = clock();
        sumDj += (float)(end_t - start_t)/CLOCKS_PER_SEC;
    }
    BoostGraph gb(g->Size());
    for(int i = 0; i < g->Size(); ++i)
        for(int j = i + 1; j < g->Size(); ++j)
            if(g->CheckEdge(i, j))
            {
                boost::add_edge(boost::vertex(i, gb), boost::vertex(j,gb), Weight(g->GetEdgeValue(i, j)), gb);
            }

    std::vector<BoostGraph::vertex_descriptor> p(boost::num_vertices(gb));
    std::vector<int> d(boost::num_vertices(gb));

    auto bgl = boost::predecessor_map(boost::make_iterator_property_map(p.begin(),
                            boost::get(boost::vertex_index, gb))).
                            distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, gb)));
    delete g;
    for(int i = 0; i < 20; ++i)
    {
        BoostGraph::vertex_descriptor s = boost::vertex(u, gb);
        start_t = clock();
        boost::dijkstra_shortest_paths(gb, s, bgl);
        end_t = clock();
        sumBDj += (float)(end_t - start_t)/CLOCKS_PER_SEC;
    }
    std::cout << "\tBFS 1-K " << sumBFS << std::endl;
    std::cout << "\tDijkstra " << sumDj << std::endl;
    std::cout << "\tboost::Dijkstra " << sumBDj << std::endl;
}

void TestMy1KBFS(int u, int v)
{
    /*GraphValuedEdge* g = new GraphValuedEdge(10000);
    g->RandomizeUnorientedGraph(0.02);
    g->Graph::WriteToFile("graph.gr");*/
    UnorientedGraphValuedEdge<int>* g = new UnorientedGraphValuedEdge<int>(1000, new AdjacencyMatrixUnoriented());
    g->RandomizeGraph(0.05, &randInt);
    std::cout << g->BFS(u, v) << std::endl;
    delete g;
}
void TestMy1KBFS()
{
    unsigned int u, v;
    std::cin >> u;
    std::cin >> v;
    TestMy1KBFS(u, v);
}
void TestMyDijkstra(int u, int v)
{
    UnorientedGraphValuedEdge<int>* g = new UnorientedGraphValuedEdge<int>(10000, new AdjacencyMatrixUnoriented());
    g->RandomizeGraph(0.05, &randInt);
    std::cout << g->Dijkstra(u,v) << std::endl;
    delete g;
}
void TestMyDijkstra()
{
    unsigned int u, v;
    std::cin >> u;
    std::cin >> v;
    TestMyDijkstra(u, v);
}
void TestBoostDijkstra(int u, int v)
{
    UnorientedGraphValuedEdge<int>* g = new UnorientedGraphValuedEdge<int>(10000, new AdjacencyMatrixUnoriented());
    g->RandomizeGraph(0.05, &randInt);
    BoostGraph gb(g->Size());
    for(int i = 0; i < g->Size(); ++i)
        for(int j = i + 1; j < g->Size(); ++j)
            if(g->CheckEdge(i, j))
            {
                boost::add_edge(boost::vertex(i, gb), boost::vertex(j,gb), Weight(g->GetEdgeValue(i, j)), gb);
            }

    std::vector<BoostGraph::vertex_descriptor> p(boost::num_vertices(gb));
    std::vector<int> d(boost::num_vertices(gb));
    BoostGraph::vertex_descriptor s = boost::vertex(u, gb);
    boost::dijkstra_shortest_paths(gb, s,
                                                boost::predecessor_map(boost::make_iterator_property_map(p.begin(),
                                                                                boost::get(boost::vertex_index, gb))).
                                   distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, gb))));
    std::cout << d[v] << std::endl;
    delete g;
}
void TestBoostDijkstra()
{
    unsigned int u, v;
    std::cin >> u;
    std::cin >> v;
    TestBoostDijkstra(u, v);
}

#endif


#include <dlib/matrix.h>
#include <iostream>

int main() {
  // definitions
  {
    // compile time sized matrix
    dlib::matrix<double, 3, 1> y;
    // dynamically sized matrix
    dlib::matrix<double> m(3, 3);
    // later we can change size of this matrix
    m.set_size(6, 6);
  }
  // initializations
  {
    // comma operator
    dlib::matrix<double> m(3, 3);
    m = 1., 2., 3., 4., 5., 6., 7., 8., 9.;
    std::cout << "Matix from comma operator\n" << m << std::endl;

    // wrap array
    double data[] = {1, 2, 3, 4, 5, 6};
    auto m2 = dlib::mat(data, 2, 3);  // create matrix with size 2x3
    std::cout << "Matix from array\n" << m2 << std::endl;

    // Matrix elements can be accessed with () operator
    m(1, 2) = 300;
    std::cout << "Matix element updated\n" << m << std::endl;

    // Also you can initialize matrix with some predefined values
    auto a = dlib::identity_matrix<double>(3);
    std::cout << "Identity matix \n" << a << std::endl;

    auto b = dlib::ones_matrix<double>(3, 4);
    std::cout << "Ones matix \n" << b << std::endl;

    auto c = dlib::randm(3, 4);  // matrix with random values with size 3x3
    std::cout << "Random matix \n" << c << std::endl;
  }
  // arithmetic operations
  {
    dlib::matrix<double> a(2, 2);
    a = 1, 1, 1, 1;
    dlib::matrix<double> b(2, 2);
    b = 2, 2, 2, 2;

    auto c = a + b;
    std::cout << "c = a + b \n" << c << std::endl;

    auto e = a * b;  // real matrix multiplication
    std::cout << "e = a dot b \n" << e << std::endl;

    a += 5;
    std::cout << "a += 5 \n" << a << std::endl;

    auto d = dlib::pointwise_multiply(a, b);  // element wise multiplication
    std::cout << "d = a * b \n" << e << std::endl;

    auto t = dlib::trans(a);  // transpose matrix
    std::cout << "transposed matrix a \n" << t << std::endl;
  }
  // partial access
  {
    dlib::matrix<float, 4, 4> m;
    m = 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16;
    auto sm =
        dlib::subm(m, dlib::range(1, 2),
                   dlib::range(1, 2));  // original matrix can't be updated
    std::cout << "Sub matrix \n" << sm << std::endl;

    dlib::set_subm(m, dlib::range(1, 2), dlib::range(1, 2)) = 100;
    std::cout << "Updated sub matrix \n" << m << std::endl;
  }
  // there are no implicit broadcasting in dlib
  {
    // we can simulate broadcasting with partial access
    dlib::matrix<float, 2, 1> v;
    v = 10, 10;
    dlib::matrix<float, 2, 3> m;
    m = 1, 2, 3, 4, 5, 6;
    for (int i = 0; i < m.nc(); ++i) {
      dlib::set_colm(m, i) += v;
    }
    std::cout << "Matrix with updated columns \n" << m << std::endl;
  }
  return 0;
}


/*
 * Copyright (c) 2021, Tetsuro Nagai
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <cmath>
#include <cassert>
#include <string>
#include <boost/format.hpp>

constexpr int BUF_MAX=100000;

constexpr int DIM_DATA=2 ;

using std::string;
using std::vector;
using std::array;
using std::cout;
using std::cerr;
using std::endl;

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

  if(argc != 1){
        cerr << "ERROR: no arguments allowed"  <<endl;
        cerr << "Data should be provided via standard input"  <<endl;
        cerr << "Use, for example, by cat data.txt | ./pdf_analyzer \n" <<endl;
        cerr << "First column must be coordinate, and the second must be probability."  <<endl;
        cerr << "The probability may not be normalized."  <<endl;
        return -1;
  }

  vector<double> pdf;
  vector<double> x;

  string buf;
  while (std::getline(std::cin, buf))
  {
    char tmp1[BUF_MAX];
    char tmp2[BUF_MAX];
    auto bsscanf = std::sscanf(buf.c_str(), "%s %s" , tmp1,tmp2);
    if(bsscanf != DIM_DATA){
        cerr << "ERROR: not enough columns\n" << "Exit!!" <<endl;
        return -1;
    }

    char *err;
    double x_tmp=std::strtod(tmp1, &err);
    if(*err!='\0'){
      cerr << "ERROR: error to convert: " << tmp1 << "\nExit!!" <<endl;
      return -1;
    }
    double pdf_tmp =std::strtod(tmp2, &err);
    if(*err!='\0'){
      cerr << "ERROR: error to convert: " << tmp2 << "\nExit!!" <<endl;
      return -1;
    }

    if(pdf_tmp < 0){
      cerr << "ERROR: pdf should be non-negative\nExit!!" <<endl;
      return -1;
    }

    if(!x.empty() and x.back() > x_tmp){
      cerr << "ERROR: x should be in ascending (increasing) order\nExit!!" <<endl;
      return -1;
    }

    x.push_back(x_tmp);
    pdf.push_back(pdf_tmp);
  }
  if(x.size() == 0){
      cerr << "ERROR: no data provided\nExit!!" <<endl;
      return -1;
  }

  auto sum_pdf = std::accumulate(pdf.begin(), pdf.end(), 0.0);
  std::transform(pdf.begin(), pdf.end(), pdf.begin(), [&sum_pdf](double x){return x / sum_pdf;});
  auto ave_x = std::inner_product(pdf.begin(), pdf.end(), x.begin(), 0.0);

  vector<double> residual_sq(pdf.size(), 0);
  std::transform(x.begin(), x.end(), residual_sq.begin(), [&ave_x](double x){return (x-ave_x)*(x-ave_x);});

  double  variance = std::inner_product(residual_sq.begin(), residual_sq.end(), pdf.begin(), 0.0) ;
  double  stddev = sqrt(variance);

  vector<double> residual_3th(pdf.size(), 0);
  std::transform(x.begin(), x.end(), residual_3th.begin(), [&ave_x](double x){return (x-ave_x)*(x-ave_x)*(x-ave_x);});

  auto skew = std::inner_product(residual_3th.begin(), residual_3th.end(), pdf.begin(), 0.0) / pow(stddev, 3) ;

  vector<double> residual_4th(pdf.size(), 0);
  std::transform(x.begin(), x.end(), residual_4th.begin(), [&ave_x](double x){return (x-ave_x)*(x-ave_x)*(x-ave_x)*(x-ave_x);});

  auto kurtosis = std::inner_product(residual_4th.begin(), residual_4th.end(), pdf.begin(), 0.0) / pow(variance, 2) -3.0 ;

  auto  at_max = std::max_element(pdf.begin(), pdf.end());
  auto  mode = x[std::distance(pdf.begin(), at_max)];

  //auto  median  = x[std::distance(pdf.begin(), at_max)];
  vector<double> cdf(pdf.size(), 0);
  std::partial_sum(pdf.begin(), pdf.end(), cdf.begin());

  auto at_Q1 = find_if(cdf.begin(), cdf.end(), [](double x){return x>0.25? true : false;});
  auto at_Q2 = find_if(cdf.begin(), cdf.end(), [](double x){return x>0.50? true : false;});
  auto at_Q3 = find_if(cdf.begin(), cdf.end(), [](double x){return x>0.75? true : false;});

  auto Q1 = x[std::distance(cdf.begin(), at_Q1)];
  auto Q2 = x[std::distance(cdf.begin(), at_Q2)];
  auto Q3 = x[std::distance(cdf.begin(), at_Q3)];

  std::cout  << boost::format("#%12s %12s %12s %12s %12s") % "ave_x"%"stddev" % "variance"  % "skew" % "kurtosis" << std::endl;
  std::cout  << boost::format(" %12.8g %12.8g %12.8g %12.8g %12.8g\n") % ave_x % stddev % variance % skew % kurtosis;

  std::cout  << boost::format("#%12s %12s %12s") % "1Q"%"2Q(median)" % "3Q"  << std::endl;
  std::cout  << boost::format(" %12.8g %12.8g %12.8g \n") % Q1  % Q2 % Q3 ;

  std::cout  << boost::format("#%12s %12s %12s") % "mode "%"normalization" % ""  << std::endl;
  std::cout  << boost::format(" %12.8g %12.8g\n") % mode  % sum_pdf ;

  return EXIT_SUCCESS ;
}


#include "util.hpp"
#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <algorithm>

void assert_chi_squared(const std::vector<int>& counts,
                        const std::vector<double>& expected, double tolerance) {
  int bins = counts.size();
  EXPECT_EQ(bins, expected.size());

  double chi = 0;
  for (int i = 0; i < bins; ++i) {
    double discrepancy = expected[i] - counts[i];
    chi += discrepancy * discrepancy / expected[i];
  }
  boost::math::chi_squared dist(bins - 1);
  double chi_threshold = quantile(complement(dist, tolerance));

  EXPECT_TRUE(chi < chi_threshold);
}
void assert_matches_bins(const std::vector<double>& samples,
                         const std::vector<double>& bin_boundaries,
                         const std::vector<double>& proportions,
                         double tolerance) {
  ASSERT_GT(samples.size(), 0);
  int N = samples.size();
  std::vector<double> mysamples = samples;
  std::sort(mysamples.begin(), mysamples.end());

  ASSERT_GT(bin_boundaries.size(), 0);
  ASSERT_TRUE(bin_boundaries.size() == proportions.size());
  int K = bin_boundaries.size();
  std::vector<double> expected;
  for (int i = 0; i < K; i++) {
    ASSERT_TRUE(proportions[i] >= 0 && proportions[i] <= 1);
    expected.push_back(proportions[i] * N);
  }

  std::vector<int> counts(K);
  size_t current_index = 0;
  for (int i = 0; i < N; ++i) {
    while (mysamples[i] >= bin_boundaries[current_index]) {
      ++current_index;
      EXPECT_TRUE(current_index < bin_boundaries.size());
    }
    ++counts[current_index];
  }
  assert_chi_squared(counts, expected, tolerance);
}
void assert_matches_quantiles(const std::vector<double>& samples,
                              const std::vector<double>& quantiles,
                              double tolerance) {
  int K = quantiles.size();
  std::vector<double> proportions;
  for (int i = 0; i < K; ++i)
    proportions.push_back(1.0 / K);

  assert_matches_bins(samples, quantiles, proportions, tolerance);
}


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearloadvector.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <functional>

namespace ElementMatrixComputation {

namespace {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector4d computeLoadVector(
    const Eigen::MatrixXd &vertices,
    std::function<double(const Eigen::Vector2d &)> f) {
  // Number of nodes of the element: triangles = 3, rectangles = 4
  const int num_nodes = vertices.cols();
  // Vector for returning element vector
  Eigen::Vector4d elem_vec = Eigen::Vector4d::Zero();
#if SOLUTION
  // Area of the cell
  double area;
  // Midpoints of edges in the reference cell
  Eigen::MatrixXd midpoints(2, num_nodes);
  switch (num_nodes) {
    case 3: {
      // Compute cell area for triangles
      area = 0.5 * ((vertices(0, 1) - vertices(0, 0)) *
                        (vertices(1, 2) - vertices(1, 0)) -
                    (vertices(1, 1) - vertices(1, 0)) *
                        (vertices(0, 2) - vertices(0, 0)));
      // clang-format off
      midpoints << vertices(0, 0) + vertices(0, 1),
	vertices(0, 1) + vertices(0, 2),
	vertices(0, 2) + vertices(0, 0),
	vertices(1, 0) + vertices(1, 1),
	vertices(1, 1) + vertices(1, 2),
	vertices(1, 2) + vertices(1, 0);
      // clang-format on
      break;
    }
    case 4: {
      // Compute cell area for rectangles
      area =
          (vertices(0, 1) - vertices(0, 0)) * (vertices(1, 3) - vertices(1, 0));
      // clang-format off
      midpoints << vertices(0, 0) + vertices(0, 1),
	vertices(0, 1) + vertices(0, 2),
	vertices(0, 2) + vertices(0, 3),
	vertices(0, 3) + vertices(0, 0),
	vertices(1, 0) + vertices(1, 1),
	vertices(1, 1) + vertices(1, 2),
	vertices(1, 2) + vertices(1, 3),
	vertices(1, 3) + vertices(1, 0);
      // clang-format on
      break;
    }
    default: {
      LF_ASSERT_MSG(false, "Illegal entity type!");
      break;
    }
  }                  // end switch
  midpoints *= 0.5;  // The factor 1/2
  // Evaluate f(x) at the quadrature points, i.e. the midpoints of the edges
  Eigen::VectorXd fvals = Eigen::VectorXd::Zero(4);
  for (int i = 0; i < num_nodes; ++i) {
    fvals(i) = f(midpoints.col(i));
  }
  // Midpoint quadrature for all nodes of the element
  for (int k = 0; k < num_nodes; k++) {
    // Contribution from one end of an edge
    elem_vec[k] += 0.5 * fvals(k);
    // Contribution from the other end of the end
    elem_vec[(k + 1) % num_nodes] += 0.5 * fvals(k);
  }
  // Rescale with quadrature weights
  elem_vec *= (area / num_nodes);
#else

  //====================
  // Your code goes here
  //====================

#endif
  return elem_vec;
}
/* SAM_LISTING_END_1 */

}  // namespace

Eigen::Vector4d MyLinearLoadVector::Eval(const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};
  const lf::base::size_type num_nodes{ref_el.NumNodes()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();

  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());

  return computeLoadVector(vertices, f_);
}

}  // namespace ElementMatrixComputation


/// My typedefs
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>

typedef int32_t    node_t;
typedef int32_t    edge_t;
typedef int64_t    cost_t;

/// From STL library
#include <fstream>

#include <vector>
using std::vector;

#include <string>

using std::pair;
using std::make_pair;

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

/// HashMap by google or-tools
#include "base/logging.h"
#include "base/commandlineflags.h"
#include "base/hash.h"
#include "base/map-util.h"
#include "base/callback.h"
#include "graph/shortestpaths.h"

typedef std::pair<node_t, node_t>                        PairNode;
typedef operations_research::hash_map<PairNode, cost_t>  ArcMap;
typedef operations_research::hash_map<node_t, cost_t>    NodeMap;

/// Graph class as callback as required by Google Or-Tools
struct GraphByCallback {
   vector<NodeMap> C;
   cost_t kDisconnectedDistance;
   GraphByCallback( const vector<NodeMap>& _C, cost_t _kDisconnectedDistance )
      : C(_C), kDisconnectedDistance(_kDisconnectedDistance) {}
   inline cost_t hasArc(node_t i, node_t j) {
      NodeMap::iterator p_iter = C[i].find(j);
      if ( p_iter != C[i].end() )
         return p_iter->second;
      else
         return kDisconnectedDistance;
   }
};

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra( char* argv[] ) {
   /// Read instance from the OR-lib
   std::ifstream infile(argv[1]);
   if (!infile)
      exit ( EXIT_FAILURE );

   int n;     /// Number of variables
   int m;     /// Number of constraints

   // reads file of the form
   // #nodes #edges
   // e_1 = v_i v_j cost[e_m]
   // ..
   // e_m = v_i v_j cost[e_m]

   /// Read the first line
   infile >> n >> m;
   fprintf(stdout,"n %d, m %d\n", n, m);
   /// Build the graph
   int avg_degree = m/n+1;
   vector<NodeMap> A;
   for ( int i = 0; i < n; ++i ) {
      NodeMap node(avg_degree);
      A.push_back(node);
   }
   /// Read arcs from file
   int v, w;
   cost_t c;
   for ( int i = 0; i < m; i++ ) {
      infile >> v >> w >> c;
      A[v-1][w-1] = c;
   }

   /// Elaborate input data for Dijkstra's algorithm
   cost_t kMaxInf = std::numeric_limits<cost_t>::max();
   cost_t T_dist;

   timer TIMER;
   for ( int i = 0; i < 50; ++i ) {
      double t0 = TIMER.elapsed();
      node_t S = i;
      node_t T = n-1-i;
      vector<node_t> paths;
      GraphByCallback call(A,kMaxInf);
      ResultCallback2<cost_t, node_t, node_t>* const arc_callback =
         NewPermanentCallback(&call, &GraphByCallback::hasArc);
      operations_research::DijkstraShortestPath(n, S, T, arc_callback, kMaxInf, &paths);
      /// Compute exact distance (the path is stored in the reversed order)
      T_dist = 0.0;
      for ( unsigned int j = 0; j < paths.size()-1; ++j )
         T_dist += A[paths[j+1]][paths[j]];
      fprintf(stdout,"Time %.4f Cost %"PRId64"\n", TIMER.elapsed()-t0, T_dist);
   }
   fprintf(stdout,"Tot %.4f\n", TIMER.elapsed());

   return T_dist;
}

/// Main function
int
main (int argc, char **argv)
{
   if ( argc != 2 ) {
      fprintf(stdout, "usage: ./dijkstra <filename>\n");
      exit ( EXIT_FAILURE );
   }
   /// Measure overall time
   timer TIMER;
   /// Invoke the different Dijkstra algorithm implementations
   cost_t T_dist = runDijkstra(argv);
   /// Print basic figures
   fprintf(stdout,"Cost %"PRId64" - Time %.3f\n", T_dist, TIMER.elapsed());

   return 0;
}