//==================================================================================================
/*
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
*/
//==================================================================================================
#pragma once

#include <eve/detail/overload.hpp>
#include <eve/module/core.hpp>
#include <eve/module/math.hpp>
#include <eve/module/complex.hpp>
#include <eve/module/complex/regular/traits.hpp>
#include <boost/math/complex/atanh.hpp>
namespace eve
{

  namespace detail
  {
    template<typename Z>
    EVE_FORCEINLINE auto complex_unary_dispatch( eve::tag::atanh_, Z const& a0) noexcept
    {
      // This implementation is a simd (i.e. no branch) transcription and adaptation of the
      // boost_math code which itself is a transcription of the pseudo-code in:
      //
      // Eric W. Weisstein. "Inverse Hyperbolic Tangent."
      // From MathWorld--A Wolfram Web Resource.
      // http://mathworld.wolfram.com/InverseHyperbolicTangent.html
      //
      // Also: The Wolfram Functions Site,
      // http://functions.wolfram.com/ElementaryFunctions/ArcTanh/
      //
      // Also "Abramowitz and Stegun. Handbook of Mathematical Functions."
      // at : http://jove.prohosting.com/~skripty/toc.htm
      //
      auto [a0r, a0i] = a0;
      auto realinf = is_eqz(a0i) && is_infinite(a0r);
      using rtype = decltype(a0r);
      const rtype alpha_crossover(0.3);
      auto  ltzra0 = is_ltz(a0r);
      auto  ltzia0 = is_ltz(a0i);
      auto s_min = eve::sqrtsmallestposval(as(a0r))*2;
      auto s_max = eve::sqrtvalmax(as(a0r))/2;
      rtype const two = rtype(2);
      rtype inf =  eve::inf(as(a0r));
      rtype x = eve::abs(a0r);
      rtype y = eve::abs(a0i);
      rtype r = zero(as(a0r));
      rtype i = zero(as(a0r));
      auto gtxmax = (x > s_max);
      auto ltxmin = (x < s_min);
      auto gtymax = (y > s_max);
      auto ltymin = (y < s_min);
      rtype xx = eve::sqr(x);
      rtype yy = eve::sqr(y);
      rtype sqrabs = xx + yy;

      auto not_in_safe_zone = ((gtxmax || ltxmin) || (gtymax || ltymin));
      if(eve::any(not_in_safe_zone))
      {
        //treat underflow or overflow
        // one or both of x and y are small, calculate divisor carefully:
        rtype div = one(as(a0r));
        div += eve::if_else(ltxmin, xx, zero);
        div += eve::if_else(ltxmin, yy, zero);

        rtype alpha = x/div;
        alpha += alpha;

        auto test =  gtymax;
        // big y, medium x, divide through by y:
        rtype tmp_alpha = (two*x/y) / (y + xx/y);
        // small x and y, whatever alpha is, it's too small to calculate:
        tmp_alpha = eve::if_else(x > one(as(a0r)), tmp_alpha, zero);
        alpha = eve::if_else(test && (x > one(as(a0r))), tmp_alpha, alpha);

        test =  eve::logical_andnot(gtxmax, test);

        // big x small y, as above but neglect y^2/x:
        tmp_alpha =  two/x;
        // big x: divide through by x:
        tmp_alpha =  eve::if_else((y > one(as(a0r))),  two / (x + y*y/x), tmp_alpha);
        // big x and y: divide alpha through by x*y:
        tmp_alpha =  eve::if_else(gtymax, (two/y) / (x/y + y/x), tmp_alpha);
        // x or y are infinite: the result is 0
        tmp_alpha = eve::if_else((y == inf) || (x == inf), zero, tmp_alpha);

        alpha = eve::if_else(test, tmp_alpha, alpha);
        r = eve::if_else((alpha < alpha_crossover),
                        eve::log1p(alpha) - eve::log1p(-alpha),
                         eve::log(inc(two*x + xx)) - eve::log(sqr(dec(x)))
                       );
        test = (x == one(as(a0r))) && ltymin;
        r = eve::if_else(test, -(two*(eve::log(y) - eve::log_2(as(a0r)))), r);
        r *= rtype(0.25);
        //compute the imag part
        // y^2 is negligible:
        i =  eve::atan2(two*y, eve::oneminus(xx));
        i =  if_else(gtymax || gtxmax, pi(as(a0r)), i);
        rtype tmp_i = eve::if_else(ltymin, atan2(two*y, one(as(a0r))),
                                  eve::atan2(two*y, eve::oneminus(yy)));
        i =  if_else(ltxmin, tmp_i, i);
      }
      auto test0 = (inf == x) && (inf == y);
      if(eve::any(test0))
      {
        //inf x, inf y
        r = eve::if_else(test0, zero, r);
        i = eve::if_else(test0, pi(as(a0r)), r);
      }
      auto test = eve::is_nan(a0);

      if(eve::any(test))
      {
        //nan x, inf y
        r = eve::if_else(eve::is_nan(x) && (y == inf), zero, r);
        i = eve::if_else(eve::is_nan(x) && (y == inf), pi(as(a0r)), r);

        r = eve::if_else(is_nan(y) && (x == inf), zero, r);
        i = eve::if_else(is_nan(y) && (x == inf), y, i);

        r = eve::if_else(is_nan(y) && eve::is_eqz(x), zero, r);
        i = eve::if_else(is_nan(y) && is_eqz(x), allbits, i);
      }
      //compute for safe zone
      // the real part is given by:
      //
      // eve::real(atanh(z)) == log((1 + x^2 + y^2 + 2x) / (1 + x^2 + y^2 - 2x))
      //
      // however, when x is either large (x > 1/e) or very small
      // (x < e) then this effectively simplifies
      // to log(1), leading to wildly inaccurate results.
      // by dividing the above (top and bottom) by (1 + x^2 + y^2) we get:
      //
      // eve::real(atanh(z)) == log((1 + (2x / (1 + x^2 + y^2))) / (1 - (-2x / (1 + x^2 + y^2))))
      //
      // which is much more sensitive to the value of x, when x is not near 1
      // (remember we can compute log(1+x) for small x very accurately).
      //
      // the cross-over from one method to the other has to be determined
      // experimentally, the value used below appears correct to within a
      // factor of 2 (and there are larger errors from other parts
      // of the input domain anyway).
      //
      rtype alpha = x*two / (eve::inc(sqrabs));
      rtype sqrxm1 = eve::sqr(eve::dec(x));
      rtype tmp_r = eve::if_else((alpha < alpha_crossover),
                                eve::log1p(alpha) - log1p(-alpha),
                                eve::log1p(x+x + sqrabs) - eve::log(sqrxm1 + yy)
                                )*rtype(0.25);
      r = eve::if_else(not_in_safe_zone, r, tmp_r);

      // compute the imag part
      i = eve::if_else(not_in_safe_zone,
                      i,
                      eve::atan2(y+y, (oneminus(sqrabs)))
                     )*half(as(a0r));

      r = eve::if_else( ltzra0,-r, r);
      i = eve::if_else(is_infinite(y), pio_2(as(a0r))*sign(y), i);
      i = eve::if_else( ltzia0,-i, i);
      r = if_else(realinf, zero(as(a0r)), r);
      i = if_else(realinf, -sign(a0r)*pio_2(as(a0r)), i);
      return  Z{r, i};
    }
  }
}


/*
 * demo.cpp
 *
 *  Created on: 25.09.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/mpi.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/point.h>
#include <deal.II/base/quadrature.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>

#include <base/ConstantMesh.h>
#include <base/DiscretizedFunction.h>
#include <base/Util.h>
#include <forward/WaveEquation.h>

#include <stddef.h>
#include <ctgmath>
#include <exception>
#include <fstream>
#include <iostream>
#include <memory>
#include <vector>

using namespace dealii;
using namespace wavepi;
using namespace wavepi::forward;
using namespace wavepi::base;

template <int dim>
class DemoF : public Function<dim> {
 public:
  static const Point<dim> center;

  double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    if (p.distance(center) < 0.75)
      return std::sin(this->get_time() * 2 * numbers::PI);
    else
      return 0.0;
  }
};

template <int dim>
class DemoC : public Function<dim> {
 public:
  static const Point<dim> center;

  virtual ~DemoC() = default;

  virtual double value(const Point<dim> &p, const unsigned int component = 0) const {
    Assert(component == 0, ExcIndexRange(component, 0, 1));
    if (p.distance(center) < 2)
      return 1.0 / (4.0 + (1.0 - std::pow(p.distance(center) / 2, 4)) * 12.0 *
                              std::pow(std::sin(this->get_time() / 2.5 * numbers::PI), 2));
    else
      return 1.0 / (4.0 + 0.0);
  }
};

template <int dim>
class DemoWaveSpeed : public Function<dim> {
 public:
  DemoC<dim> base;

  virtual ~DemoWaveSpeed() = default;

  virtual double value(const Point<dim> &p, const unsigned int component = 0) const {
    return 1.0 / std::sqrt(base.value(p, component));
  }

  virtual void set_time(double t) {
    Function<dim>::set_time(t);
    base.set_time(t);
  }
};

template <>
const Point<2> DemoF<2>::center = Point<2>(1.0, 0.0);
template <>
const Point<2> DemoC<2>::center = Point<2>(0.0, 2.5);

template <int dim>
void demo() {
  std::ofstream logout("wavepi_demo.log", std::ios_base::trunc);
  deallog.attach(logout);
  deallog.depth_console(3);
  deallog.depth_file(100);
  deallog.precision(3);
  deallog.pop();

  auto triangulation = std::make_shared<Triangulation<dim>>();
  GridGenerator::hyper_cube(*triangulation, -5.0, 5.0);
  Util::set_all_boundary_ids(*triangulation, 0);
  triangulation->refine_global(6);

  double t_end = 10;
  int steps    = t_end * 64;

  double t_start = 0.0, dt = t_end / steps;
  std::vector<double> times;

  for (size_t i = 0; t_start + i * dt <= t_end; i++)
    times.push_back(t_start + i * dt);

  FE_Q<dim> fe(1);
  Quadrature<dim> quad = QGauss<dim>(3);

  auto mesh = std::make_shared<ConstantMesh<dim>>(times, fe, quad, triangulation);
  WaveEquation<dim> wave_eq(mesh);

  DemoC<dim> demo_c_cont;
  auto demo_c = std::make_shared<DiscretizedFunction<dim>>(mesh, demo_c_cont);
  wave_eq.set_param_c(demo_c);

  auto sol = wave_eq.run(std::make_shared<L2RightHandSide<dim>>(std::make_shared<DemoF<dim>>()));
  sol.write_pvd("./", "demo_u", "u");
  demo_c->write_pvd("./", "demo_c", "c");

  DemoWaveSpeed<dim> demo_wave_speed_cont;
  auto demo_wave_speed = std::make_shared<DiscretizedFunction<dim>>(mesh, demo_wave_speed_cont);
  demo_wave_speed->write_pvd("./", "demo_wave_speed", "wave speed");
}

int main(int argc, char *argv[]) {
  Utilities::MPI::MPI_InitFinalize mpi_init(argc, argv);

  try {
    demo<2>();
  } catch (std::exception &exc) {
    std::cerr << "Exception on processing: " << exc.what() << std::endl;
    return 1;
  } catch (...) {
    std::cerr << "Unknown exception!" << std::endl;
    return 1;
  }

  return 0;
}


#include "panel.h"

#include <iostream>

#include <Eigen/Dense>

namespace poflow {

void PanelSolver::init_arrays() {
  num_points_ = num_elements_ + 1;
  FN = Eigen::MatrixXd::Zero(num_elements_, num_elements_);
  FT = Eigen::MatrixXd::Zero(num_elements_, num_elements_);
  R = Eigen::VectorXd::Zero(num_elements_);
  source_density_ = Eigen::VectorXd::Zero(num_elements_);
}

void PanelSolver::build() { init_arrays(); }

void PanelSolver::build_equation_system(double vx, double vy) {
  double pi = 3.14159;
  for (int k = 0; k < num_elements_; ++k) {
    R(k) = vx * panels_[k].si - vy * panels_[k].ci;
    auto kth_panel = panels_[k];
    for (int j = 0; j < num_elements_; ++j) {
      auto jth_panel = panels_[j];
      if (k == j) {
        FN(k, j) = 2.0 * pi;
        FT(k, j) = 0.0;
      } else {
        double dyj = jth_panel.si * jth_panel.ds;
        double dxj = jth_panel.ci * jth_panel.ds;
        double sph = jth_panel.ds / 2.0;

        double xd = kth_panel.xc - jth_panel.xc;
        double yd = kth_panel.yc - jth_panel.yc;
        double rkj = std::sqrt(xd * xd + yd * yd);

        double bkj = std::atan2(yd, xd);
        double alj = std::atan2(dyj, dxj);
        double gkj = alj - bkj;

        double zik = rkj * std::cos(gkj);
        double etk = -rkj * std::sin(gkj);

        double r1s = std::pow((zik + sph), 2.0) + std::pow(etk, 2.0);
        double r2s = std::pow((zik - sph), 2.0) + std::pow(etk, 2.0);
        double qt = std::log(r1s / r2s);

        double den = zik * zik + etk * etk - sph * sph;
        double gnm = etk * jth_panel.ds;
        double qn = 2.0 * std::atan2(gnm, den);

        double ukj = qt * jth_panel.ci - qn * jth_panel.si;
        double vkj = qt * jth_panel.si + qn * jth_panel.ci;

        FN(k, j) = -ukj * kth_panel.si + vkj * kth_panel.ci;
        FT(k, j) = ukj * kth_panel.ci + vkj * kth_panel.si;
      }
    }
  }
}

std::map<std::string, Eigen::VectorXd>
PanelSolver::compute_surface_results(double vx, double vy) {
  build_equation_system(vx, vy);
  source_density_ = FN.colPivHouseholderQr().solve(R);

  int n = num_elements_;
  Eigen::VectorXd R(n);
  Eigen::VectorXd qt(n);
  Eigen::VectorXd qn(n);
  Eigen::VectorXd u(n);
  Eigen::VectorXd v(n);
  Eigen::VectorXd p(n);
  Eigen::VectorXd xc(n);
  Eigen::VectorXd yc(n);
  Eigen::VectorXd theta(n);

  auto qts = FT * source_density_;
  auto qns = FN * source_density_;

  for (int i = 0; i < n; ++i) {
    auto &panel = panels_[i];
    xc[i] = panel.xc;
    yc[i] = panel.yc;
    theta[i] = std::atan2(yc[i], xc[i]);
    qt[i] = qts[i] + vy * panel.si + vx * panel.ci;
    qn[i] = qns[i] + vy * panel.ci - vx * panel.si;
    u[i] = vx - qns[i] * panel.si + qts[i] * panel.ci;
    v[i] = vy + qns[i] * panel.ci + qts[i] * panel.si;
    p[i] = 1.0 - std::pow(u[i], 2.0) - std::pow(v[i], 2.0);
  }
  std::map<std::string, Eigen::VectorXd> results;
  results["xc"] = xc;
  results["yc"] = yc;
  results["theta"] = theta;
  results["qt"] = qt;
  results["qn"] = qn;
  results["u"] = u;
  results["v"] = v;
  results["p"] = p;

  return results;
}

} // namespace poflow


/**
 * @ file norms.cc
 * @ brief NPDE homework PointEvaluationRhs code
 * @ author Christian Mitsch, Liaowang Huang (refactoring)
 * @ date 22/03/2019, 06/01/2020 (refactoring)
 * @ copyright Developed at ETH Zurich
 */

#include "pointevaluationrhs_norms.h"

#include <cmath>

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/quad/quad.h>
#include <lf/uscalfe/uscalfe.h>

namespace PointEvaluationRhs {

/* SAM_LISTING_BEGIN_1 */
double computeL2normLinearFE(const lf::assemble::DofHandler &dofh,
                             const Eigen::VectorXd &mu) {
  double result = 0.0;
  //====================
  // Your code goes here
  //====================
  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double computeH1seminormLinearFE(const lf::assemble::DofHandler &dofh,
                                 const Eigen::VectorXd &mu) {
  // calculate stiffness matrix by using the already existing local assembler
  // LinearFELaplaceElementMatrix
  double result = 0.0;
  //====================
  // Your code goes here
  //====================
  return result;
}
/* SAM_LISTING_END_2 */

Eigen::MatrixXd MassLocalMatrixAssembler::Eval(const lf::mesh::Entity &entity) {
  Eigen::MatrixXd result;
  //====================
  // Your code goes here
  //====================
  return result;
}

}  // namespace PointEvaluationRhs


/** \file
 * Support for Eigen linear algebra library in Chaiscript.
 *
 * \author Truong X. Nghiem (xuan.nghiem@epfl.ch)
 */

#ifndef _CHAISCRIPT_EXTRAS_EIGEN_H
#define _CHAISCRIPT_EXTRAS_EIGEN_H

#include <vector>
#include <Eigen/Dense>
#include <chaiscript/chaiscript.hpp>

namespace chaiscript {
    namespace extras {
        namespace eigenlinalg {
            typedef std::vector<chaiscript::Boxed_Value> TChaiVector;   ///< The C++ type of a Chaiscript's vector object

            Eigen::IOFormat EigenMatlabFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");

            // Register with Chaiscript a new Eigen vector type
            template<typename CLS, typename Scalar>
            ModulePtr eigen_vector_type(const char* CLSNAME, ModulePtr m = std::make_shared<Module>()) {
                //// 1.1. Type and Construction
                m->add(user_type<CLS>(), CLSNAME);
                m->add(constructor<CLS ()>(), CLSNAME);
                m->add(constructor<CLS (typename CLS::Index)>(), CLSNAME);
                m->add(bootstrap::copy_constructor<CLS>(CLSNAME));
                m->add(fun(static_cast<CLS& (CLS::*)(const CLS&)>(&CLS::operator=)), "=");

                // Construct vector from a Chaiscript vector of elements of EXACTLY the type Scalar
                m->add(fun([](const TChaiVector& cv) {
                    auto n = cv.size();
                    CLS v(n);
                    if (n > 0) {
                        int k = 0;
                        for (auto& elem: cv) {
                            v(k++) = chaiscript::boxed_cast<Scalar>(elem);
                        }
                    }
                    return v;
                }), CLSNAME);

                //// 1.2. Accessors
                m->add(fun(static_cast<Scalar& (CLS::*)(typename CLS::Index)>(&CLS::operator())), "[]");
                m->add(fun(static_cast<Scalar& (CLS::*)(typename CLS::Index)>(&CLS::operator())), "coeff");

                //// 1.3. Size and resize
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::size)), "size");
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::rows)), "rows");
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::cols)), "cols");
                m->add(fun(static_cast<void (CLS::*)(typename CLS::Index)>(&CLS::resize)), "resize"); // destructive resize

                //// 1.4. Misc

                // Convert to string to print
                m->add(fun([](const CLS& v) {
                    std::stringstream s;
                    s << v.format(EigenMatlabFmt);
                    return s.str();
                }), "to_string");

                return m;
            }

            // Register with Chaiscript a new Eigen matrix type
            template<typename CLS, typename Scalar>
            ModulePtr eigen_matrix_type(const char* CLSNAME, ModulePtr m = std::make_shared<Module>()) {
                //// 1.1. Type and Construction
                m->add(user_type<CLS>(), CLSNAME);
                m->add(constructor<CLS ()>(), CLSNAME);
                // Somehow Eigen does not work nicely with Chaiscript's constructor utility, so I must use a workaround
                m->add(fun([](typename CLS::Index r, typename CLS::Index c) {
                    return CLS(r, c);
                }), CLSNAME);
                bootstrap::copy_constructor<CLS>(CLSNAME, *m);
                m->add(fun(static_cast<CLS& (CLS::*)(const CLS&)>(&CLS::operator=)), "=");

                // Conversions from vector to matrix
                //m->add(fun([](const Eigen::Matrix<Scalar,Eigen::Dynamic,1>& v) { return CLS(v); }), CLSNAME);
                //m->add(fun([](const Eigen::Matrix<Scalar,1,Eigen::Dynamic>& v) { return CLS(v); }), CLSNAME);

                // Construct a matrix of given size from a Chaiscript vector of elements of EXACTLY the type Scalar.
                // This is done ROW-WISE.
                // Only r*c elements are copied from the vector to the matrix. If the vector contains fewer elements, only those will be copied, the remaining elements of the matrix are undefined.
                m->add(fun([](typename CLS::Index r, typename CLS::Index c, const TChaiVector& cv) {
                    auto n = cv.size();
                    CLS v(r, c);
                    if (n > 0 && r*c > 0) {
                        int k = 0;
                        for (auto ri = 0; ri < r; ++ri) {
                            for (auto ci = 0; ci < c; ++ci) {
                                v(ri, ci) = chaiscript::Boxed_Number(cv[k++]).get_as<Scalar>();
                                if (k == n) {
                                    break;
                                }
                            }
                            if (k == n) {
                                break;
                            }
                        }
                    }
                    return v;
                }), CLSNAME);

                //// 1.2. Accessors
                m->add(fun(static_cast<Scalar& (CLS::*)(typename CLS::Index, typename CLS::Index)>(&CLS::operator())), "coeff");
                m->add(fun(static_cast<Scalar& (CLS::*)(typename CLS::Index)>(&CLS::operator())), "[]");

                //// 1.3. Size and resize
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::size)), "size");
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::rows)), "rows");
                m->add(fun(static_cast<typename CLS::Index (CLS::*)() const>(&CLS::cols)), "cols");
                m->add(fun(static_cast<void (CLS::*)(typename CLS::Index, typename CLS::Index)>(&CLS::resize)), "resize"); // destructive resize

                //// 1.4. Misc
                // Convert to string to print
                m->add(fun([](const CLS& v) {
                    std::stringstream s;
                    s << v.format(EigenMatlabFmt);
                    return s.str();
                }), "to_string");

                return m;
            }

            // Predefined matrices
            template<typename CLS>
            ModulePtr eigen_matrix_predefined(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](typename CLS::Index r, typename CLS::Index c) { return CLS(CLS::Zero(r,c)); }), "zeros");  // Zero matrix
                m->add(fun([](CLS &m) { m.setZero(); }), "setZero");  // set all coefficients to 0

                m->add(fun([](typename CLS::Index r, typename CLS::Index c) { return CLS(CLS::Ones(r,c)); }), "ones");  // matrix of 1's
                m->add(fun([](CLS &m) { m.setOnes(); }), "setOnes");  // set all coefficients to 1

                m->add(fun(&CLS::fill), "fill");  // set all coefficients to a given constant

                m->add(fun([](typename CLS::Index r, typename CLS::Index c) { return CLS(CLS::Identity(r,c));}), "eyes");  // Identity matrix
                m->add(fun([](CLS &m) { m.setIdentity(); }), "setIdentity");  // set the matrix to identity
                m->add(fun([](CLS &m, typename CLS::Index r, typename CLS::Index c) { m.setIdentity(r,c); }), "setIdentity");  // resize and set the matrix to identity

                return m;
            }

            // Binary Addition and subtraction
            // CLS1 and CLS2 should be different types, where CLS1 should be "larger" in the sense that CLS2 is a special case of CLS1 (e.g. Matrix and Vector)
            template<typename CLS1, typename CLS2>
            ModulePtr binary_addition_substraction(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS1& a, const CLS2& b) { return CLS1(a + b); }), "+");
                m->add(fun([](const CLS2& a, const CLS1& b) { return CLS1(a + b); }), "+");
                m->add(fun([](const CLS1& a, const CLS2& b) { return CLS1(a - b); }), "-");
                m->add(fun([](const CLS2& a, const CLS1& b) { return CLS1(a - b); }), "-");
                return m;
            }

            // Binary Addition and subtraction for objects of same class
            template<typename CLS1>
            ModulePtr binary_addition_substraction(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS1& a, const CLS1& b) { return (a + b).eval(); }), "+");
                m->add(fun([](const CLS1& a, const CLS1& b) { return (a - b).eval(); }), "-");
                m->add(fun([](CLS1& a, const CLS1& b) { return (a += b); }), "+=");

                // Between matrix and scalar
                m->add(fun([](const CLS1& a, const typename CLS1::Scalar b) { return CLS1(a.array() + b); }), "+");
                //m->add(fun([](const typename CLS1::Scalar b, const CLS1& a) { return CLS1(a.array() + b); }), "+");
                m->add(fun([](const CLS1& a, const typename CLS1::Scalar b) { return CLS1(a.array() - b); }), "-");
                //m->add(fun([](const typename CLS1::Scalar b, const CLS1& a) { return CLS1((-a).array() + b); }), "-");

                return m;
            }

            // Unary Addition and subtraction
            template<typename CLS>
            ModulePtr unary_addition_substraction(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS& a) { return a; }), "+");
                m->add(fun([](const CLS& a) { return (-a).eval(); }), "-");
                return m;
            }

            // Scalar multiplication and division
            template<typename CLS, typename Scalar>
            ModulePtr scalar_mult_div(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS& a, const Scalar b) { return (a * b).eval(); }), "*");
                m->add(fun([](const Scalar a, const CLS& b) { return (a * b).eval(); }), "*");
                m->add(fun([](const CLS& a, const Scalar b) { return (a / b).eval(); }), "/");
                m->add(fun([](CLS& a, const Scalar b) { return (a *= b); }), "*=");
                m->add(fun([](CLS& a, const Scalar b) { return (a /= b); }), "/=");
                return m;
            }

            // Transpose and conjugation
            template<typename CLS, const bool withConjugation>
            ModulePtr transpose_conjugation(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS& a) { return CLS(a.transpose()); }), "transpose");
                m->add(fun(static_cast<void (CLS::*)()>(&CLS::transposeInPlace)), "transposeInPlace");    // in-place transpose: void transposeInPlace()

                if (withConjugation) {
                    m->add(fun([](const CLS& a) { return CLS(a.conjugate()); }), "conjugate");
                    m->add(fun([](const CLS& a) { return CLS(a.adjoint()); }), "adjoint");
                    m->add(fun(static_cast<void (CLS::*)()>(&CLS::adjointInPlace)), "adjointInPlace");    // in-place adjoint: void adjointInPlace()
                }
                return m;
            }

            // Matrix multiplication
            template<typename CLS1, typename CLS2 = CLS1>
            ModulePtr matrix_mult(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS1& a, const CLS2& b) { return (a * b).eval(); }), "*");
                m->add(fun([](CLS1& a, const CLS2& b) { return a *= b; }), "*=");
                return m;
            }

            // Dot product (only for vector types)
            template<typename CLS1>
            ModulePtr dot_product(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS1& a, const CLS1& b) { return a.dot(b); }), "dot");
                return m;
            }

            // Basic arithmetic reduction operations
            template<typename CLS>
            ModulePtr arith_reduction(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::sum)), "sum");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::prod)), "prod");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::mean)), "mean");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::minCoeff)), "min");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::maxCoeff)), "max");
                m->add(fun(static_cast<typename CLS::Scalar (CLS::*)() const>(&CLS::trace)), "trace");
                return m;
            }

            // Coefficient-wise operations
            template<typename CLS>
            ModulePtr coefficient_wise_operations(ModulePtr m = std::make_shared<Module>()) {
                m->add(fun([](const CLS& a) { return a.cwiseAbs().eval(); }), "abs");
                m->add(fun([](const CLS& a) { return a.cwiseAbs2().eval(); }), "abs2");
                m->add(fun([](const CLS& a) { return a.cwiseSqrt().eval(); }), "sqrt");
                m->add(fun([](const CLS& a) { return a.array().log().matrix().eval(); }), "log");
                m->add(fun([](const CLS& a) { return a.array().exp().matrix().eval().eval(); }), "exp");
                m->add(fun([](const CLS& a, double e) { return a.array().pow(e).matrix().eval(); }), "pow");
                m->add(fun([](const CLS& a) { return a.array().square().matrix().eval(); }), "square");
                m->add(fun([](const CLS& a) { return a.array().sin().matrix().eval(); }), "sin");
                m->add(fun([](const CLS& a) { return a.array().cos().matrix().eval(); }), "cos");
                m->add(fun([](const CLS& a) { return a.array().tan().matrix().eval(); }), "tan");
                m->add(fun([](const CLS& a) { return a.array().asin().matrix().eval(); }), "asin");
                m->add(fun([](const CLS& a) { return a.array().acos().matrix().eval(); }), "acos");

                return m;
            }

        }
    }
}
#endif // _CHAISCRIPT_EXTRAS_EIGEN_H


#include <stan/math/prim/arr.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

TEST(MathFunctions, inverse_softmax_exception) {
  std::vector<double> simplex(2);
  std::vector<double> y(3);
  EXPECT_THROW(stan::math::inverse_softmax<std::vector<double> >(simplex, y),
               std::invalid_argument);
}

TEST(MathFunctions, inverse_softmax) {
  std::vector<double> simplex(2);
  std::vector<double> y(2);

  simplex[0] = 0.2;
  simplex[1] = 0.8;

  stan::math::inverse_softmax(simplex, y);
  EXPECT_FLOAT_EQ(log(0.2), y[0]);
  EXPECT_FLOAT_EQ(log(0.8), y[1]);
}

TEST(MathFunctions, inverse_softmax_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  std::vector<double> simplex(2);
  std::vector<double> y(2);

  simplex[0] = nan;
  simplex[1] = nan;

  stan::math::inverse_softmax(simplex, y);
  EXPECT_PRED1(boost::math::isnan<double>, y[0]);

  EXPECT_PRED1(boost::math::isnan<double>, y[1]);
}


#ifndef DATAOPS_H
#define DATAOPS_H

#include <map>
#include <iterator>
#include <string>
#include <fstream>
#include <iostream>
#include <sstream>
#include <vector>
#include <algorithm>
#include <utility>
#include <functional>
#include <numeric>
#include <math.h>
#include <cmath>
#include <complex>
#include <boost/multi_array.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <typeinfo>

namespace DataOps
{
	template <typename T>
		std::vector<T>& sample_every(std::vector<T>& lhs, const std::vector<T>& rhs, const size_t nskip = 10, const size_t offset = 0)
		{
			if (lhs.size() < 1){
				lhs.resize(int(rhs.size()/nskip),T(0));
			}
			if (lhs.size() > (rhs.size()+offset)/nskip ){
				lhs.resize(size_t((rhs.size()+offset)/nskip));
			}
			for (size_t i = 0; i<lhs.size(); ++i){
				lhs[i] = T(rhs[nskip*i+offset]);
			}
			return lhs;
		}

	template <typename T>
		T* clone(T* lhs,const T* rhs,const size_t n)
		{
			std::copy(rhs, rhs + n, lhs);
			return lhs;
		}

	template <typename T>
		std::vector<T>& clone(std::vector<T> & lhs, const std::vector<T> & rhs)
		{
			lhs.resize(rhs.size());
			std::copy(rhs.begin(),rhs.end(),lhs.begin());
			return lhs;
		}

	template <typename T>
		T& gauss(const T & xin, const T& x0, const T& w,T& a =T(1),T& y0 = T(0)){
			T x = xin-x0;
			return (T) (y0+a*std::exp(- std::pow(x/w,int(2)))) ;
		}

	template <typename X_t,typename Y_t>
		Y_t polynomial(const X_t xin,const std::vector<Y_t> c){
			assert(c.size()>1);
			Y_t x = Y_t(xin) - c.front();
			Y_t y = c[1];
			for (unsigned i=1; i<c.size();++i){
				y += c[i] * std::pow(x,(int)i-1);
			}
			return y;
		}

	template <typename T>
		void fixedfilter(T * vec, const size_t sz){
			T noise(T(.1));
			vec[0] *= T(1);
			vec[sz/2] *= T(1)/(T(1) + noise/10*std::pow(T(sz/2),2));
			for( size_t i=1;i<sz/2;++i){
				vec[i] *= T(1)/(T(1) + noise/T(10)*std::pow(T(i),2));
				vec[sz-i] *= T(1)/(T(1) + noise/T(10)*std::pow(T(i),2));
			}
			return;
		}

	template <typename T>
		bool inwin(const T val, const std::vector<T> win){
			assert(win.size()==2);
			return (val >= win[0] && val < win[1]);
		}

	template <typename T>
		void endstozero(T * vec, const size_t sz, const size_t steps)
		{
			T mean1(0);
			T mean2(0);
			T x1(0);
			T x2(0);
			for (size_t i=0;i<steps;++i){
				mean1 += vec[i];
				x1 += T(i);
				mean2 += vec[sz-i-1];
				x2 += T(i);
			}
			for (size_t i=0; i<sz; ++i){
				vec[i] -= (mean2-mean1)/(x2-x1)*(T(i)-x1) + mean1/T(steps);
			}
		}

	template <typename T>
		void sin2roll(T * vec, const size_t sz, const T center, const T width)
		{
			for (size_t i = 0;i<sz;++i){
				T x = T(M_PI)*(T(i)-center)/(T(2)*width);
				if (std::abs(x) > T(1)){
					vec[i] = T(0);
				} else {
					vec[i] *= std::pow(std::cos(x),int(2));
				}
			}
			return;
		}

	template <typename T>
		void gaussroll(T * vec, const size_t sz, const T center, const T width)
		{
			for (size_t i = 0;i<sz;++i){
				T x = (T(i)-center)/width;
				vec[i] *= std::exp(-1.*std::pow(x,2));
			}
			return;
		}

	template <typename T,typename V>
		V interpolate(const std::map<T,V> &data, T x)
		{
			typedef typename std::map<T,V>::const_iterator MapIterator;
			MapIterator i = data.upper_bound(x);
			MapIterator l = i;
			double slope;
			double span = (double) (x - l->first);
			if (i==data.begin())
			{
				MapIterator u=i;
				++u;
				slope = (double)(u->second - i->second)/(double)(u->first - i->first);
				return (V)( i->second + (slope * span/(u->first - i->first) ) );
			}
			--l;
			if(i==data.end())
			{
				MapIterator ll=l;
				--ll;
				slope = (double)(l->second - ll->second)/(double)(l->first - ll->first);
				return (V)( l->second + (slope * span/(l->first - ll->first) ) ) ;
			}
			slope = (double)( i->second - l->second ) / (double)( i->first - l->first );
			return (V)(l->second + (slope * span/(i->first - l->first)) );
		}

	template <typename T>
		void condense(std::vector< T > & invec,const unsigned newlen)
		{
			double ratio = ((double)invec.size()/(double)newlen);

			if (ratio <= 1){std::cerr << "Cannot condense2(): size mismatch" << std::endl;return;}
			for (unsigned j=1;j<(int)ratio;++j){
				invec[0] += invec[j];
			}
			invec[0] /= ratio;
			for (unsigned i=1;i<newlen;++i){
				invec[i] = invec[(int)(i*ratio)];
				for (unsigned j=1;j<ratio;++j){
					invec[i] += invec[(int)(i*ratio + j)];
				}
				invec[i] /= (double)ratio;
			}
			invec.resize(newlen);

			return;
		}

	template <typename T>
		void condense(std::vector< std::vector < T > > & inmat, const unsigned newlen)
		{
			for (unsigned i=0;i<inmat.size();++i)
				condense(inmat[i],newlen);
			return;
		}

	template <typename T>
		std::istream& operator >> (std::istream & ins, std::vector<T> & record)
		{
			record.clear();
			std::string line;
			getline( ins, line );
			const char head='#';
			if (line.find(head) != std::string::npos){
				return ins;
			}
			std::istringstream iss( (std::string)line );
			T value;
			while (iss >> value){
				record.push_back(value);
			}
			return ins;
		}

	template <typename T>
		std::istream& operator >> (std::istream & ins, std::vector< std::vector<T> > & matrix)
		{
			matrix.clear();
			std::vector<T> record;
			while (ins >> record)
			{
				if (record.size() > 0)
					matrix.push_back( record );
			}
			return ins;
		}
	/*
	   template <typename T, int dim>
	   std::ostream& operator << (std::ostream & outs, boost::multi_array<T,dim> & record)
	   {
	   for (unsigned i=0;i<record.shape()[0];++i){
	   outs << record[i] << "\t";
	   }
	   outs << std::flush;
	   return outs;
	   }
	 */

	template <typename T,typename V>
		std::ostream& operator << (std::ostream & outs, std::pair<T,V> outpair)
		{
			outs << "(" << outpair.first << "," << outpair.second << ")\t" << std::flush;
			return outs;
		}

	template <typename T>
		std::ostream& operator << (std::ostream & outs, std::vector< T > & vec)
		{
			for (unsigned i=0;i<vec.size();++i){
				outs << vec[i] << "\t";
			}
			outs << "\n" << std::flush;
			return outs;
		}

	template <typename T>
		std::ostream& operator << (std::ostream & outs, std::vector< std::vector <T> > & matrix)
		{
			for (unsigned i=0;i<matrix.size();++i){
				for (unsigned j=0;j<matrix[i].size();++j){
					outs << matrix[i][j] << "\t";
				}
				outs << "\n";
			}
			outs << std::flush;
			return outs;
		}

	template <typename T>
		std::complex<T>*& sum(std::complex<T>*& lhs,std::complex<T>* const & rhs,const size_t n)
		{
			for (size_t i = 0;i<n;++i){
				lhs[i] += rhs[i];
			}
			/*
			std::transform(lhs,lhs+n,rhs,[](std::complex<T>* d_lhs, std::complex<T>* d_rhs){
					return *d_lhs + *d_rhs;
					});
			*/
			return lhs;
		}

	template <typename T>
		std::complex<T>* diff(std::complex<T>*& lhs,std::complex<T>* const & rhs,const size_t n)
		{
			for (size_t i = 0;i<n;++i){
				lhs[i] -= rhs[i];
			}
			/*
			std::transform(lhs,lhs+n,rhs,[](std::complex<T>* d_lhs, std::complex<T>* d_rhs){
					return *d_lhs - *d_rhs;
					});
			*/
			return lhs;
		}

	template <typename T>
		std::complex<T>* mul(std::complex<T>*& lhs,std::complex<T>* const & rhs,const size_t n)
		{
			for (size_t i = 0;i<n;++i){
				lhs[i] *= rhs[i];
			}
			/*
			std::transform(lhs,lhs+n,rhs,lhs,[](std::complex<T>* d_lhs, std::complex<T>* d_rhs){
					return (*d_lhs) * (*d_rhs);
					});
			*/
			return lhs;
		}

	template <typename T>
		std::complex<T>* mul(std::complex<T>*& lhs,const T& scale,const size_t n)
		{
			std::transform(lhs, lhs+n, lhs, std::bind2nd(std::multiplies<std::complex<T> >(),scale));
			//std::cerr << "using this complex vec scaling function in DataOps::" << std::endl << std::flush;
			return lhs;
		}
	template <typename T>
		std::complex<T>* mul(std::complex<T>*& lhs,std::complex<T>& scale,const size_t n)
		{
			std::transform(lhs, lhs+n, lhs, std::bind2nd(std::multiplies<std::complex<T> >(),scale));
			return lhs;
		}

	template <typename T>
		std::complex<T>* div(std::complex<T>*& lhs,std::complex<T>* const & rhs,const size_t n)
		{
			for (size_t i=0;i<n;++i){
				lhs[i] /= rhs[i];
			}
			/*
			std::transform(lhs,lhs+n,rhs,lhs,[](std::complex<T>* d_lhs, std::complex<T>* d_rhs){
					return *d_lhs / *d_rhs;
					});
			*/
			return lhs;
		}

	template <typename T>
		inline bool inwin(std::vector<T> win,T val)
		{
			std::sort(win.begin(),win.end());
			return (val >= win.front() && val < win.back());
		}

	template <typename T>
		std::vector<T> & expscaled(const T scale, std::vector<T> & x)
		{
			x *= scale;
			std::transform(x.begin(), x.end(), x.begin(), [&](T xval){return std::exp(xval);});
			return x;
		}

	template <typename T>
		std::vector<std::complex<T> > & operator *= (std::vector<std::complex<T> > & vec,std::complex<T> val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::multiplies<std::complex<T> >(),val));
			return vec;
		}

	template <typename T>
		std::vector<std::complex<T> > & operator *= (std::vector<std::complex<T> > & vec,T val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::multiplies<std::complex<T> >(),val));
			return vec;
		}

	template <typename T>
		std::vector<T> & operator *= (std::vector<T>& vec,T const val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::multiplies<T>(),val));
			return vec;
		}

	//================ HERE HERE HERE HERE ================//
	template <typename T>
		std::vector<T> & operator /= (std::vector<T>& vec,const T val)
		{
			assert(std::abs(val) != T(0) );
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::divides<T>(),val));
			return vec;
		}

	template <typename T>
		std::vector<T> & operator += (std::vector<T>& vec, const T val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::plus<T>(),val));
			return vec;
		}

	template <typename T,typename T2>
		std::vector<T> & operator -= (std::vector<T>& vec, const T2 val)
		{
			std::transform(vec.begin(), vec.end(), vec.begin(), std::bind2nd(std::minus<T>(),val));
			return vec;
		}

	template <typename T>
		std::vector<T>& operator += (std::vector<T>& resvec,const std::vector<T> & srcvec)
		{
			assert(resvec.size() == srcvec.size());
			for (unsigned i=0;i<resvec.size();++i){
				resvec[i] += srcvec[i];
			}
			return resvec;
		}

	template <typename T>
		std::vector<T>& operator -= (std::vector<T>& resvec,const std::vector<T> & srcvec)
		{
			assert(resvec.size() == srcvec.size());
			for (unsigned i=0;i<resvec.size();++i){
				resvec[i] -= srcvec[i];
			}
			return resvec;
		}
	template <typename T>
		std::vector<T>& operator *= (std::vector<T>& resvec,const std::vector<T> & srcvec)
		{
			assert(resvec.size() == srcvec.size());
			for (unsigned i=0;i<resvec.size();++i){
				resvec[i] *= srcvec[i];
			}
			return resvec;
		}
	template <typename T>
		std::vector<T>& operator /= (std::vector<T>& resvec,const std::vector<T> & srcvec)
		{
			assert(resvec.size() == srcvec.size());
			for (unsigned i=0;i<resvec.size();++i){
				resvec[i] /= srcvec[i];
			}
			return resvec;
		}

	template <typename T>
		inline T projection(std::vector<T> &in1,std::vector<T> in2,std::vector<bool> & mask){
			assert(in1.size()==in2.size());
			assert(in1.size() == mask.size());
			T ip(0);
			for (unsigned i=0;i<in1.size();++i){
				if (mask[i]){
					ip += in1[i] * in2[i];
				}
			}
			return ip;
		}

	inline size_t sum(std::vector<bool> & mask){
		size_t sum = 0;
		for (size_t i = 0 ; i<mask.size();++i){
			if (mask[i]==true) sum++;
		}
		return sum;
	}

	template <typename T>
		inline T projection(std::vector<T> &in1,std::vector<T> in2){
			assert(in1.size()==in2.size());
			return std::inner_product(in1.begin(),in1.end(),in2.begin(),0.);
		}

	template <typename T>
		inline void safe_normalize(std::vector<T> & in)
		{
			T ip(0);
			for (unsigned i=0;i<in.size();++i){
				ip += (in[i] * in[i]);
			}
			assert(ip!=T(0));
			T scale = T(1)/std::sqrt(ip);
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::multiplies<T>(),scale) );
		}

	template <typename T>
		inline void sqr_normalize(std::vector<T> & in,std::vector<bool>& mask)
		{
			assert(in.size() == mask.size());
			T ip = T(0);
			for (unsigned i=0;i<in.size();++i)
				if (mask[i]) {ip += (in[i] * in[i]);}
			assert(ip!=T(0));
			T scale = T(1)/std::sqrt(ip);
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::multiplies<T>(),scale) );
		}

	template <typename T>
		inline void sqr_normalize(std::vector<T> & in) {
			T scale = sqrt( std::inner_product(in.begin(), in.end(), in.begin(),T(0)) );
			assert(scale != T(0));
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::divides<T>(),scale) );
		};

	template <typename T>
		inline T removemean(T* in, size_t sz) {
			T sum = std::accumulate(in, in + sz, T(0));
			T mean = sum / T(sz);
			std::transform(in, in + sz, in, std::bind2nd(std::minus<T>(),mean));
			return mean;
		};

	template <typename T>
		inline T removemean(std::vector<T> & in,std::vector<bool> & mask) {
			assert(in.size() == mask.size());
			T sum(0);
			unsigned nvals(0);
			for (unsigned i=0;i<in.size();++i){
				if (mask[i]) {
					sum += in[i];
					++nvals;
				}
			}
			T mean = sum / T(nvals);
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::minus<T>(),mean));
			return mean;
		};

	template <typename T>
		inline T removemean(std::vector<T> & in) {
			T sum = std::accumulate(in.begin(), in.end(), T(0));
			T mean = sum / T(in.size());
			std::transform(in.begin(), in.end(), in.begin(), std::bind2nd(std::minus<T>(),mean));
			return mean;
		};

	template <typename T>
		inline T mean(std::vector<T> &in){
			T mean = std::accumulate(in.begin(), in.end(), T(0));
			mean /= T(in.size());
			return mean;
		};

	// WHoah  logarithmic stuff  //

	template <typename T>
		inline void meanstdlog(std::vector<T> & in, T& mean, T& std)
		{
			// I want the log of the values, then sum, then divide by size for mean
			// Diff is the log(values)-mean(log valuse)
			mean = std::log( std::accumulate(in.begin(),in.end(),T(1),std::multiplies<T>()) );
			mean /= T(in.size());
			std::vector<T> diff(in.size());
			for (unsigned i=0;i<diff.size();++i){
				diff[i] = std::log(in[i]) - mean;
			}
			T sq_sum = std::inner_product(diff.begin(),diff.end(),diff.begin(),T(0));
			std = std::sqrt(sq_sum / T(diff.size()));
		}

	template <typename T>
		inline T meanlog(std::vector<T> & in,T offset){
			T mean = offset;
			mean += std::log( std::accumulate(in.begin(),in.end(),T(1),std::multiplies<T>()) );
			mean /= T(in.size());
			return mean;
		};

	template <typename T>
		inline T stdlog(std::vector<T> & in, T mean){
			std::vector<T> diff(in.size());
			for (unsigned i=0;i<diff.size();++i){
				diff[i] = std::log(in[i]) - mean;
			}
			T sq_sum = std::inner_product(diff.begin(),diff.end(),diff.begin(),T(0));
			T stdev = std::sqrt(sq_sum / T(diff.size()));
			return stdev;
		};

	template <typename T>
		void detrend(T * vec, const size_t sz)
		{
			T num(0);
			T den(0);
			T xm = T(sz-1)/T(2);
			T ym = removemean(vec,sz);
			for (size_t i=0;i<sz;++i){
				num += (i-xm)*vec[i];
				den += std::pow((i-xm),int(2));
			}
			T beta = num/den;
			for (size_t i=0;i<sz;++i){
				vec[i] -= beta*T(i) - beta*xm;
			}
			return;
		}

}

#endif


///////////////////////////////////////////////////////////////////////////////
// weighted_p_square_quantile.hpp
//
//  Copyright 2005 Daniel Egloff. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_P_SQUARE_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_P_SQUARE_QUANTILE_HPP_DE_01_01_2006

#include <cmath>
#include <functional>
#include <boost/array.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>

namespace boost { namespace accumulators
{

namespace impl {
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_p_square_quantile_impl
    //  single quantile estimation with weighted samples
    /**
        @brief Single quantile estimation with the \f$P^2\f$ algorithm for weighted samples

        This version of the \f$P^2\f$ algorithm extends the \f$P^2\f$ algorithm to support weighted samples.
        The \f$P^2\f$ algorithm estimates a quantile dynamically without storing samples. Instead of
        storing the whole sample cumulative distribution, only five points (markers) are stored. The heights
        of these markers are the minimum and the maximum of the samples and the current estimates of the
        \f$(p/2)\f$-, \f$p\f$ - and \f$(1+p)/2\f$ -quantiles. Their positions are equal to the number
        of samples that are smaller or equal to the markers. Each time a new sample is added, the
        positions of the markers are updated and if necessary their heights are adjusted using a piecewise-
        parabolic formula.

        For further details, see

        R. Jain and I. Chlamtac, The P^2 algorithm for dynamic calculation of quantiles and
        histograms without storing observations, Communications of the ACM,
        Volume 28 (October), Number 10, 1985, p. 1076-1085.

        @param quantile_probability
    */
    template<typename Sample, typename Weight, typename Impl>
    struct weighted_p_square_quantile_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        typedef typename numeric::functional::average<weighted_sample, std::size_t>::result_type float_type;
        typedef array<float_type, 5> array_type;
        // for boost::result_of
        typedef float_type result_type;

        template<typename Args>
        weighted_p_square_quantile_impl(Args const &args)
          : p(is_same<Impl, for_median>::value ? 0.5 : args[quantile_probability | 0.5])
          , heights()
          , actual_positions()
          , desired_positions()
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            std::size_t cnt = count(args);

            // accumulate 5 first samples
            if (cnt <= 5)
            {
                this->heights[cnt - 1] = args[sample];

                // In this initialization phase, actual_positions stores the weights of the
                // initial samples that are needed at the end of the initialization phase to
                // compute the correct initial positions of the markers.
                this->actual_positions[cnt - 1] = args[weight];

                // complete the initialization of heights and actual_positions by sorting
                if (cnt == 5)
                {
                    // TODO: we need to sort the initial samples (in heights) in ascending order and
                    // sort their weights (in actual_positions) the same way. The following lines do
                    // it, but there must be a better and more efficient way of doing this.
                    typename array_type::iterator it_begin, it_end, it_min;

                    it_begin = this->heights.begin();
                    it_end   = this->heights.end();

                    std::size_t pos = 0;

                    while (it_begin != it_end)
                    {
                        it_min = std::min_element(it_begin, it_end);
                        std::size_t d = std::distance(it_begin, it_min);
                        std::swap(*it_begin, *it_min);
                        std::swap(this->actual_positions[pos], this->actual_positions[pos + d]);
                        ++it_begin;
                        ++pos;
                    }

                    // calculate correct initial actual positions
                    for (std::size_t i = 1; i < 5; ++i)
                    {
                        this->actual_positions[i] += this->actual_positions[i - 1];
                    }
                }
            }
            else
            {
                std::size_t sample_cell = 1; // k

                // find cell k such that heights[k-1] <= args[sample] < heights[k] and adjust extreme values
                if (args[sample] < this->heights[0])
                {
                    this->heights[0] = args[sample];
                    this->actual_positions[0] = args[weight];
                    sample_cell = 1;
                }
                else if (this->heights[4] <= args[sample])
                {
                    this->heights[4] = args[sample];
                    sample_cell = 4;
                }
                else
                {
                    typedef typename array_type::iterator iterator;
                    iterator it = std::upper_bound(
                        this->heights.begin()
                      , this->heights.end()
                      , args[sample]
                    );

                    sample_cell = std::distance(this->heights.begin(), it);
                }

                // increment positions of markers above sample_cell
                for (std::size_t i = sample_cell; i < 5; ++i)
                {
                    this->actual_positions[i] += args[weight];
                }

                // update desired positions for all markers
                this->desired_positions[0] = this->actual_positions[0];
                this->desired_positions[1] = (sum_of_weights(args) - this->actual_positions[0])
                                           * this->p/2. + this->actual_positions[0];
                this->desired_positions[2] = (sum_of_weights(args) - this->actual_positions[0])
                                           * this->p + this->actual_positions[0];
                this->desired_positions[3] = (sum_of_weights(args) - this->actual_positions[0])
                                           * (1. + this->p)/2. + this->actual_positions[0];
                this->desired_positions[4] = sum_of_weights(args);

                // adjust height and actual positions of markers 1 to 3 if necessary
                for (std::size_t i = 1; i <= 3; ++i)
                {
                    // offset to desired positions
                    float_type d = this->desired_positions[i] - this->actual_positions[i];

                    // offset to next position
                    float_type dp = this->actual_positions[i + 1] - this->actual_positions[i];

                    // offset to previous position
                    float_type dm = this->actual_positions[i - 1] - this->actual_positions[i];

                    // height ds
                    float_type hp = (this->heights[i + 1] - this->heights[i]) / dp;
                    float_type hm = (this->heights[i - 1] - this->heights[i]) / dm;

                    if ( ( d >= 1. && dp > 1. ) || ( d <= -1. && dm < -1. ) )
                    {
                        short sign_d = static_cast<short>(d / std::abs(d));

                        // try adjusting heights[i] using p-squared formula
                        float_type h = this->heights[i] + sign_d / (dp - dm) * ( (sign_d - dm) * hp + (dp - sign_d) * hm );

                        if ( this->heights[i - 1] < h && h < this->heights[i + 1] )
                        {
                            this->heights[i] = h;
                        }
                        else
                        {
                            // use linear formula
                            if (d>0)
                            {
                                this->heights[i] += hp;
                            }
                            if (d<0)
                            {
                                this->heights[i] -= hm;
                            }
                        }
                        this->actual_positions[i] += sign_d;
                    }
                }
            }
        }

        result_type result(dont_care) const
        {
            return this->heights[2];
        }

    private:
        float_type p;                    // the quantile probability p
        array_type heights;              // q_i
        array_type actual_positions;     // n_i
        array_type desired_positions;    // n'_i
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_p_square_quantile
//
namespace tag
{
    struct weighted_p_square_quantile
      : depends_on<count, sum_of_weights>
    {
        typedef accumulators::impl::weighted_p_square_quantile_impl<mpl::_1, mpl::_2, regular> impl;
    };
    struct weighted_p_square_quantile_for_median
      : depends_on<count, sum_of_weights>
    {
        typedef accumulators::impl::weighted_p_square_quantile_impl<mpl::_1, mpl::_2, for_median> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_p_square_quantile
// extract::weighted_p_square_quantile_for_median
//
namespace extract
{
    extractor<tag::weighted_p_square_quantile> const weighted_p_square_quantile = {};
    extractor<tag::weighted_p_square_quantile_for_median> const weighted_p_square_quantile_for_median = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_p_square_quantile)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_p_square_quantile_for_median)
}

using extract::weighted_p_square_quantile;
using extract::weighted_p_square_quantile_for_median;

}} // namespace boost::accumulators

#endif


#ifndef ZSVM_REAL_VARIATIONAL_SOLVER_HPP_INCLUDED
#define ZSVM_REAL_VARIATIONAL_SOLVER_HPP_INCLUDED

// C++ standard library headers
#include <cmath> // for std::sqrt, std::pow, std::abs
#include <cstddef> // for std::size_t
#include <limits>

// Eigen linear algebra library headers
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

namespace zsvm {

    template <typename T>
    class RealVariationalSolver {

    private: // =============================================== MEMBER VARIABLES

        typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXT;
        typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXT;
        typedef Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ArrayXT;

        std::size_t size;
        MatrixXT overlap_matrix;
        MatrixXT hamiltonian_matrix;
        VectorXT eigenvalues;
        MatrixXT eigenvectors;
        bool clean;

    public: // ===================================================== CONSTRUCTOR

        RealVariationalSolver() : size(0), clean(false) {}

    public: // ======================================================== MUTATORS

        void set_basis_size_conservative(std::size_t basis_size) {
            size = basis_size;
            overlap_matrix.conservativeResize(basis_size, basis_size);
            hamiltonian_matrix.conservativeResize(basis_size, basis_size);
            clean = false;
        }

        void set_basis_size_destructive(std::size_t basis_size) {
            size = basis_size;
            overlap_matrix.resize(basis_size, basis_size);
            hamiltonian_matrix.resize(basis_size, basis_size);
            clean = false;
        }

        T &overlap_matrix_element(std::size_t i, std::size_t j) {
            clean = false;
            return overlap_matrix(i, j);
        }

        T &hamiltonian_matrix_element(std::size_t i, std::size_t j) {
            clean = false;
            return hamiltonian_matrix(i, j);
        }

        void compute_eigenstates() {
            if (!clean) {
                Eigen::GeneralizedSelfAdjointEigenSolver<MatrixXT> eigen_solver(
                        hamiltonian_matrix, overlap_matrix,
                        Eigen::ComputeEigenvectors | Eigen::Ax_lBx);
                eigenvalues = eigen_solver.eigenvalues();
                eigenvectors = eigen_solver.eigenvectors();
                clean = true;
            }
        }

    public: // ======================================================== ACCESORS

        bool empty() const { return (size == 0); }

        T get_eigenvalue(std::size_t i) {
            if (!clean) { compute_eigenstates(); }
            return eigenvalues(i);
        }

    private: // ========================== EIGENVALUE COMPUTATION HELPER METHODS

        T secular_objective_function(const T &x, const T &t,
                                     const ArrayXT &beta) const {
            return t - x - (beta / (eigenvalues.array() - x)).sum();
        }

        void find_lower_bracketing_interval(
                T &lower_bound, T &upper_bound,
                const T &strict_upper_bound,
                const T &t, const ArrayXT &beta) const {
            using std::ldexp;
            const T one(1);
            int k = 0;
            lower_bound = strict_upper_bound - ldexp(one, -k);
            upper_bound = strict_upper_bound - ldexp(one, -k - 1);
            T lower_objective = secular_objective_function(
                    lower_bound, t, beta);
            T upper_objective = secular_objective_function(
                    upper_bound, t, beta);
            while (true) {
                if (lower_objective > 0 && upper_objective > 0) {
                    ++k;
                    lower_bound = upper_bound;
                    lower_objective = upper_objective;
                    upper_bound = strict_upper_bound - ldexp(one, -k - 1);
                    upper_objective = secular_objective_function(
                            upper_bound, t, beta);
                } else if (lower_objective < 0 && upper_objective < 0) {
                    --k;
                    upper_bound = lower_bound;
                    upper_objective = lower_objective;
                    lower_bound = strict_upper_bound - ldexp(one, -k);
                    lower_objective = secular_objective_function(
                            lower_bound, t, beta);
                } else {
                    break;
                }
            }
        }

        T solve_secular_equation_bisection(
                T lower_bound, T upper_bound,
                const T &t, const ArrayXT &beta) const {
            using std::abs;
            const T tolerance = 64 * std::numeric_limits<T>::epsilon();
            while (true) {
                const T midpoint = (lower_bound + upper_bound) / 2;
                const T relative_difference = abs(
                        (upper_bound - lower_bound) / midpoint);
                if (relative_difference < tolerance) { return midpoint; }
                const T midpoint_objective = secular_objective_function(
                        midpoint, t, beta);
                if (midpoint_objective == 0) {
                    return midpoint;
                } else if (midpoint_objective > 0) {
                    lower_bound = midpoint;
                } else if (midpoint_objective < 0) {
                    upper_bound = midpoint;
                } else {
                    return std::numeric_limits<T>::quiet_NaN();
                }
            }
        }

    public: // ============================= FAST EIGENVALUE COMPUTATION METHODS

        T minimum_augmented_eigenvalue(
                const T *new_overlap_column,
                const T *new_hamiltonian_column) {
            using std::sqrt;
            if (!clean) { compute_eigenstates(); }
            VectorXT overlap_vector(size);
            for (std::size_t i = 0; i < size; ++i) {
                overlap_vector(i) = new_overlap_column[i];
            }
            VectorXT hamiltonian_vector(size);
            for (std::size_t i = 0; i < size; ++i) {
                hamiltonian_vector(i) = new_hamiltonian_column[i];
            }
            const VectorXT alpha = eigenvectors.transpose() * overlap_vector;
            const T norm_factor = 1 / sqrt(
                    new_overlap_column[size] - alpha.squaredNorm());
            const VectorXT phi = -norm_factor * (eigenvectors * alpha);
            const VectorXT psi = hamiltonian_matrix * phi +
                                 norm_factor * hamiltonian_vector;
            const ArrayXT beta =
                    (eigenvectors.transpose() * psi).array().square();
            const T t = phi.dot(psi) + norm_factor * (
                    hamiltonian_vector.dot(phi) +
                    norm_factor * new_hamiltonian_column[size]);
            T lower_bound, upper_bound;
            find_lower_bracketing_interval(
                    lower_bound, upper_bound, eigenvalues[0], t, beta);
            return solve_secular_equation_bisection(
                    lower_bound, upper_bound, t, beta);
        }

    }; // class RealVariationalSolver

} // namespace zsvm

#endif // ZSVM_REAL_VARIATIONAL_SOLVER_HPP_INCLUDED


#include <boost/random/piecewise_linear_distribution.hpp>


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_STIRLING_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_STIRLING_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-euler
    Function object implementing stirling capabilities

    Computes stirling formula for the gamma function

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = stirling(x);
    @endcode

    Computes  \sqrt{2 \pi} x^{x-\frac12} e^{-x} ( 1 + \frac1{x} P(\frac1{x}))\f$,
    where \f$P\f$ is a polynomial.

    The formula implementation is usable for x between 33 and 172,
    according cephes to approximate \f$\Gamma(x).

    @see gamma, gammaln

  **/
  Value stirling(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/stirling.hpp>
#include <boost/simd/function/simd/stirling.hpp>

#endif


//
//  test_helib.cpp
//  kcalg
//
//  Created by knightc on 2019/7/19.
//  Copyright © 2019 knightc. All rights reserved.
//

#include "opentsb/test.h"

#include <NTL/ZZ.h>

#include <helib/FHEContext.h>
#include <helib/EncryptedArray.h>
#include <helib/FHE.h>
#include <helib/DoubleCRT.h>

#include <stack>
#include <string>
#include <stdlib.h>

using namespace std;
using namespace NTL;

stack<Ctxt> theStack;
FHEcontext* context;
FHESecKey* secretKey;
FHEPubKey* publicKey;
EncryptedArray* ea;
ZZX Gx;

void setupHElib();
bool isOp(string token);
void evaluate(char op);

void greeting(){
    cout <<"Welcome to the homomorphic encryption calculator" <<endl;
    cout <<"Enter expression in reverse polish natation"<<endl;
    cout <<"Enter q to quit"<<endl;
}

void test_helib_all_main(){

    string token;

    greeting();
    setupHElib();
    ea = new EncryptedArray(*context, Gx);

    while(true){

        cin >> token;

        if(token[0] == 'q'){
            break;
        }
        else if(isOp(token)){
            if(theStack.size()<2){
                cout << "not enough numbers on the stack"<<endl;
            }
            else{
                evaluate(token[0]);
            }
        }
        else{
            Ctxt& c0= *(new Ctxt(*publicKey));
            PlaintextArray p0(*ea);
            encode(*ea,p0,atoi(token.data()));
            ea->encrypt(c0, *publicKey, p0);

            theStack.push(c0);
        }
    }

    PlaintextArray p_decrypted(*ea);
    ea->decrypt(theStack.top(), *secretKey, p_decrypted);
    cout << "The answer is: ";
    p_decrypted.print(cout);
    cout << endl;

}

void setupHElib(){
    long p=101;
    long r=1;
    long L=4;
    long c=2;
    long k=80;
    long s=0;
    long d=0;
    long w=64;
    long m=FindM(k,L,c,p,d,s,0);

    context = new FHEcontext(m,p,r);
    buildModChain(*context, L, c);
    Gx = context->alMod.getFactorsOverZZ()[0];

    secretKey = new FHESecKey(*context);
    publicKey = secretKey;

    secretKey->GenSecKey(w);
    addSome1DMatrices(*secretKey); // compute key-switching matrices that we need
}

bool isOp(string token){
    return (token[0] == '+' || token[0] == '-' || token[0] == '*');
}

void evaluate(char op){
    Ctxt *op1,*op2;

    switch(op) {
        case '+':
            op1 = new Ctxt(theStack.top()); theStack.pop();
            op2 = new Ctxt(theStack.top()); theStack.pop();
            (*op1) += (*op2);
            theStack.push(*op1);
            break;
        case '-':
            op1 = new Ctxt(theStack.top()); theStack.pop();
            op2 = new Ctxt(theStack.top()); theStack.pop();
            (*op1) -= (*op2);
            theStack.push(*op1);
            break;
        case '*':
            op1 = new Ctxt(theStack.top()); theStack.pop();
            op2 = new Ctxt(theStack.top()); theStack.pop();
            (*op1) *= (*op2);
            theStack.push(*op1);
            break;
    }
}


// Filename: matrix_free_cg.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

int main(int, char**)
{
    // For a more realistic example set size to 1000 or larger
    const int size = 10, N = size * size;

    typedef mtl::mat::poisson2D_dirichlet  matrix_type;
    matrix_type                               A(size, size);
    itl::pc::identity<matrix_type>            P(A);

    mtl::dense_vector<double>                 x(N, 1.0), b(N);

    b = A * x;
    x= 0;
    itl::cyclic_iteration<double>             iter(b, 100, 1.e-11, 0.0, 5);
    cg(A, x, b, P, iter);

    return 0;
}


#define _USE_MATH_DEFINES
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <limits>
#include <RansacLib/ransac.h>

#include <time.h>
#include <cmath>
#include <iostream>
#include <numeric>
#include "radialpose.h"
#include "misc/ransac_estimator.h"
#include "misc/unit_test_misc.h"

bool test_simple_ransac_no_outliers() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params2 = { -0.12, 0.034 };

	larsson_iccv19::Solver<2, 0, true> estimator;

	generate_scene_and_image(100, 2, 20, 70, false, &pose_gt, &x, &X, 1.0);
	add_rational_distortion(params2, 2, 0, &pose_gt, &x);
	add_focal(2000.0, &pose_gt, &x);
	add_noise(0.5, &x);

	RansacEstimator<larsson_iccv19::Solver<2, 0, true>> solver(x, X, estimator);

	ransac_lib::LORansacOptions options;
	options.squared_inlier_threshold_ = 4;

	ransac_lib::LocallyOptimizedMSAC<Camera,
		std::vector<Camera>,
		RansacEstimator<larsson_iccv19::Solver<2, 0, true>>> lomsac;
	ransac_lib::RansacStatistics ransac_stats;

	Camera best_model;
	int num_ransac_inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	std::cout << "   ... LOMSAC found " << num_ransac_inliers
		<< " inliers in " << ransac_stats.num_iterations
		<< " iterations with an inlier ratio of "
		<< ransac_stats.inlier_ratio << std::endl;

	return (ransac_stats.inlier_ratio > 0.99);
}

bool test_simple_ransac_some_outliers() {
	Matrix<double, 2, Dynamic> x;
	Matrix<double, 3, Dynamic> X;
	Camera pose_gt;

	std::vector<double> params2 = { -0.12, 0.034 };

	larsson_iccv19::Solver<2, 0, true> estimator;

	generate_scene_and_image(100, 2, 20, 70, false, &pose_gt, &x, &X, 1.0);
	add_rational_distortion(params2, 2, 0, &pose_gt, &x);
	add_focal(2000.0, &pose_gt, &x);
	add_noise(1.0, &x);

	for (int i = 0; i < 20; ++i) {
		Vector2d n; n.setRandom(); n *= 0.2 * pose_gt.focal;
		x.col(i) += n;
	}

	RansacEstimator<larsson_iccv19::Solver<2, 0, true>> solver(x, X, estimator);

	ransac_lib::LORansacOptions options;
	options.squared_inlier_threshold_ = 4;

	ransac_lib::LocallyOptimizedMSAC<Camera,
		std::vector<Camera>,
		RansacEstimator<larsson_iccv19::Solver<2, 0, true>>> lomsac;
	ransac_lib::RansacStatistics ransac_stats;

	Camera best_model;
	int num_ransac_inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	std::cout << "   ... LOMSAC found " << num_ransac_inliers
		<< " inliers in " << ransac_stats.num_iterations
		<< " iterations with an inlier ratio of "
		<< ransac_stats.inlier_ratio << std::endl;

	return (ransac_stats.inlier_ratio > .79);
}

int main() {
	std::cout << "Running tests...\n\n";
	srand((unsigned int)time(0));
	//srand(2.0);

	int passed = 0;
	int num_tests = 0;

	TEST(test_simple_ransac_no_outliers);
	TEST(test_simple_ransac_some_outliers);

}


#include "gtest/gtest.h"
#include <armadillo>
#include "libsvm_runner.h"
#include "svm_basic.h"
#include "two_e_svm_post.h"
#include "two_e_svm_pre.h"
#include "svm_utils.h"

using namespace arma;

TEST(SVMUtilsT7est, SqrtInvOfMatrix) {
	double epsilon = 0.05;
	mat A;
	A << 3 << 4 << 4 << endr
			<< 1 << 3 << 4 << endr
			<< 5 << 3 << 2 <<endr
			<< 4 << 7 << 4 << endr
			<< 8 << 9 << 4;
	A = cov(A);
	arma::mat sqrtInv;
	SvmUtils::sqrtInvMat(A, sqrtInv);
	ASSERT_LT(abs(arma::norm(sqrtInv * sqrtInv * A) - 1), epsilon);
}

TEST(TwoESVMTest, TestPreprocessor) {
	SVMConfiguration svm_config;
	double epsilon = 0.05;
	mat posMat, negMat, SqrtInv;
	posMat << 1.3 << 1.5 << endr << 2.0 << 2.3 << endr << 3.0 << 4.0;
	negMat << 0.2 << 0.4 << endr << 0.1 << 0.3 << endr << 0.2 << 0.5;

	vec TrainingTarget;
	TrainingTarget << 1 << 1 << 1 << -1 << -1 << -1;

	svm_config.setPrediction(false); // training model
	svm_config.data = arma::join_vert(posMat, negMat);
	svm_config.target = TrainingTarget;

	TwoeSVMPreprocessor two_e_pre_runner;
	two_e_pre_runner.processRequest(svm_config);
	arma::mat my_cov = cov(posMat) + cov(negMat);
	SvmUtils::sqrtInvMat(my_cov, SqrtInv);
	ASSERT_LT(abs(arma::norm(two_e_pre_runner.cov0InvSqrt - SqrtInv)), epsilon);
}

TEST(TwoESVMTest, TestAll) {
	SVMConfiguration svm_config;
	mat TrainingMatrix;
	TrainingMatrix << 0.2 << 0.4 << endr << 0.1 << 0.3 << endr << 0.2 << 0.5
			<< endr << 1.3 << 1.5 << endr << 2.0 << 2.3 << endr << 3.0 << 4.0;

	vec TrainingTarget;
	TrainingTarget << -1 << -1 << -1 << 1 << 1 << 1;

	mat A;
	A << -1 << -1 << endr << -0.5 << -0.5 << endr << 2 << 1.5 << endr << 3 << 3;

	svm_config.setPrediction(false); // training model
	svm_config.data = TrainingMatrix;
	svm_config.target = TrainingTarget;
	LibSVMRunner lib_svm_runner;

	TwoeSVMPreprocessor two_e_pre_runner;
	TwoeSVMPostprocessor two_e_post_runner;

	two_e_pre_runner.processRequest(svm_config);
	lib_svm_runner.processRequest(svm_config);
	two_e_post_runner.processRequest(svm_config);

	svm_config.setPrediction(true);
	svm_config.data = A;
	two_e_pre_runner.processRequest(svm_config);
	lib_svm_runner.processRequest(svm_config);

	ASSERT_EQ(-1.0, svm_config.result[0]);
	ASSERT_EQ(-1.0, svm_config.result[1]);
	ASSERT_EQ(1.0, svm_config.result[2]);
	ASSERT_EQ(1.0, svm_config.result[3]);
}


#include "sigen/toolbox/toolbox.h"
#include "sigen/common/disjoint_set.h"
#include "sigen/common/math.h"
#include <algorithm>
#include <boost/foreach.hpp>
#include <boost/scoped_array.hpp>
#include <cmath>
#include <functional>
#include <iostream>
#include <kdtree/kdtree.h>
#include <limits>
#include <map>
#include <queue>
#include <set>
#include <utility>
#include <vector>
namespace sigen {
static double norm2(
    const NeuronNodePtr &a,
    const NeuronNodePtr &b) {
  const double dx = std::abs(a->gx_ - b->gx_);
  const double dy = std::abs(a->gy_ - b->gy_);
  const double dz = std::abs(a->gz_ - b->gz_);
  return std::sqrt(dx * dx + dy * dy + dz * dz);
}

// N = left.NumNodes()
// M = right.NumNodes()
// O(N*M)
static std::pair<double, std::pair<int, int> > normNeuronFastPath(const Neuron &left, const Neuron &right) {
  int l, r;
  double minimum = std::numeric_limits<double>::max();
  for (int i = 0; i < (int)left.NumNodes(); ++i) {
    for (int j = 0; j < (int)right.NumNodes(); ++j) {
      double d = norm2(left.storage_[i], right.storage_[j]);
      if (minimum > d) {
        minimum = d;
        l = i;
        r = j;
      }
    }
  }
  return std::make_pair(minimum, std::make_pair(l, r));
}

// N = left.NumNodes()
// M = right.NumNodes()
// O((N + M) log N)
// Use https://github.com/jtsiomb/kdtree
static std::pair<double, std::pair<int, int> > normNeuronSlowPath(const Neuron &left, const Neuron &right) {
  kdtree *tree = kd_create(3);

  boost::scoped_array<int> indexes(new int[left.NumNodes()]);
  for (int i = 0; i < (int)left.NumNodes(); ++i) {
    indexes[i] = i;
    kd_insert3(
        tree,
        left.storage_[i]->gx_,
        left.storage_[i]->gy_,
        left.storage_[i]->gz_,
        &indexes[i]);
  }

  int l, r;
  double minimum = std::numeric_limits<double>::max();
  for (int j = 0; j < (int)right.NumNodes(); ++j) {
    kdres *set = kd_nearest3(
        tree,
        right.storage_[j]->gx_,
        right.storage_[j]->gy_,
        right.storage_[j]->gz_);
    int i = *(int *)kd_res_item_data(set);
    kd_res_free(set);
    double d = norm2(left.storage_[i], right.storage_[j]);
    if (minimum > d) {
      minimum = d;
      l = i;
      r = j;
    }
  }

  kd_free(tree);
  return std::make_pair(minimum, std::make_pair(l, r));
}

static std::pair<double, std::pair<int, int> > normNeuron(const Neuron &left, const Neuron &right) {
  assert(!left.IsEmpty());
  assert(!right.IsEmpty());
  if (std::min(left.NumNodes(), right.NumNodes()) >= 300) {
    return normNeuronSlowPath(left, right);
  } else {
    return normNeuronFastPath(left, right);
  }
}

std::vector<Neuron> Interpolate(const std::vector<Neuron> &input, const double dt, const int vt) {
  const int N = input.size();
  std::vector<Neuron> forest;
  for (int i = 0; i < N; ++i) {
    forest.push_back(input[i].Clone());
  }
  DisjointSet<int> set;
  std::vector<bool> is_not_small(forest.size(), false);
  for (int i = 0; i < N; ++i) {
    if (forest[i].NumNodes() >= vt) {
      set.Add(i);
      is_not_small[i] = true;
    }
  }
  set.SetUp();

  typedef std::pair<double, std::pair<int, int> > priorityQueueNode;
  std::priority_queue<
      priorityQueueNode,
      std::vector<priorityQueueNode>,
      std::greater<priorityQueueNode> >
      pq;

  std::vector<std::map<int, double> > distance(N);

  for (int i = 0; i < N; ++i) {
    if (is_not_small[i]) {
      for (int j = i + 1; j < N; ++j) {
        if (is_not_small[j]) {
          assert(i != j);
          double d = normNeuron(forest[i], forest[j]).first;
          if (d <= dt) {
            pq.push(std::make_pair(d, std::make_pair(i, j)));
            distance[i][j] = d;
            distance[j][i] = d;
          }
        }
      }
    }
  }
  while (!pq.empty()) {
    priorityQueueNode node = pq.top();
    pq.pop();
    int l = node.second.first;
    int r = node.second.second;
    if (forest[l].IsEmpty())
      continue;
    if (forest[r].IsEmpty())
      continue;
    if (set.IsSame(l, r))
      continue;
    std::pair<double, std::pair<int, int> > dist = normNeuron(forest[l], forest[r]);
    set.Merge(l, r);
    forest[l].ConnectToOtherNeuron(dist.second.first, forest[r], dist.second.second);
    forest[r].ConnectToOtherNeuron(dist.second.second, forest[l], dist.second.first);
    forest[l].Extend(forest[r]);
    forest[r].Clear();

    for (std::map<int, double>::iterator it = distance[r].begin(); it != distance[r].end(); ++it) {
      int i = it->first;
      double d = it->second;
      assert(is_not_small[i]);
      if (set.IsSame(l, i) == false && forest[i].IsEmpty() == false) {
        if (distance[l].count(i)) {
          distance[l][i] = std::min(distance[l][i], d);
        } else {
          distance[l][i] = d;
        }

        if (distance[i].count(l)) {
          distance[i][l] = std::min(distance[i][l], d);
        } else {
          distance[i][l] = d;
        }

        pq.push(std::make_pair(d, std::make_pair(l, i)));
      }
    }
  }
  for (int i = 0; i < (int)forest.size(); ++i) {
    if (forest[i].IsEmpty()) {
      forest.erase(forest.begin() + i);
      i--;
    }
  }
  return forest;
}

struct PointAndRadius {
  double gx_, gy_, gz_, radius_;
  void setCoord(const double gx, const double gy, const double gz) {
    gx_ = gx;
    gy_ = gy;
    gz_ = gz;
  }
};

std::vector<Neuron> Smoothing(const std::vector<Neuron> &input, const int n_iter) {
  std::vector<Neuron> forest;
  for (int i = 0; i < (int)input.size(); ++i) {
    forest.push_back(input[i].Clone());
  }
  for (int iter = 0; iter < n_iter; ++iter) {
    std::map<int, PointAndRadius> next_value;
    for (int i = 0; i < (int)forest.size(); ++i) {
      BOOST_FOREACH (NeuronNodePtr node, forest[i].storage_) {
        std::vector<double> gx, gy, gz, radius;
        gx.push_back(node->gx_);
        gy.push_back(node->gy_);
        gz.push_back(node->gz_);
        radius.push_back(node->radius_);
        BOOST_FOREACH (NeuronNode *adj, node->adjacent_) {
          gx.push_back(adj->gx_);
          gy.push_back(adj->gy_);
          gz.push_back(adj->gz_);
          radius.push_back(adj->radius_);
        }
        PointAndRadius next_node;
        next_node.setCoord(Mean(gx), Mean(gy), Mean(gz));
        next_node.radius_ = Mean(radius);
        next_value[node->id_] = next_node;
      }
    }
    for (int i = 0; i < (int)forest.size(); ++i) {
      BOOST_FOREACH (NeuronNodePtr node, forest[i].storage_) {
        PointAndRadius next_node = next_value[node->id_];
        node->setCoord(next_node.gx_, next_node.gy_, next_node.gz_);
        node->radius_ = next_node.radius_;
      }
    }
  }
  return forest;
}

// return max_height
static int clippingDfs(
    NeuronNode *node,
    NeuronNode *parent,
    const int level,
    std::set<int> &will_remove,
    std::map<NeuronNode *, int> &memo) {
  if (memo.count(node))
    return memo[node];
  if (node->CountNumChild(parent) < 2) {
    // If count_num_child == 1
    BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
      if (next != parent) {
        return memo[node] = clippingDfs(next, node, level, will_remove, memo) + 1;
      }
    }
    // If count_num_child == 0
    return 1;
  }
  int has_longpath = 0;
  BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
    if (next != parent) {
      int depth = clippingDfs(next, node, level, will_remove, memo);
      if (depth > level)
        has_longpath = true;
    }
  }
  if (has_longpath) {
    int maxdepth = 0;
    BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
      if (next != parent) {
        int depth = clippingDfs(next, node, level, will_remove, memo);
        if (depth <= level) {
          will_remove.insert(next->id_);
        }
        maxdepth = std::max(maxdepth, depth);
      }
    }
    return memo[node] = maxdepth + 1;
  } else {
    int maxdepth = 0;
    NeuronNode *longest_child = NULL;
    BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
      if (next != parent) {
        int depth = clippingDfs(next, node, level, will_remove, memo);
        if (maxdepth < depth) {
          maxdepth = depth;
          longest_child = next;
        }
      }
    }
    if (maxdepth > 0) {
      BOOST_FOREACH (NeuronNode *next, node->adjacent_) {
        if (next != parent && next != longest_child) {
          will_remove.insert(next->id_);
        }
      }
    }
    return memo[node] = maxdepth + 1;
  }
}

std::vector<Neuron> Clipping(const std::vector<Neuron> &input, const int level) {
  std::set<int> will_remove;
  std::vector<Neuron> forest;
  std::map<NeuronNode *, int> memo;
  for (int i = 0; i < (int)input.size(); ++i) {
    forest.push_back(input[i].Clone());
    clippingDfs(forest[i].get_root(), NULL, level, will_remove, memo);
  }
  for (int i = 0; i < (int)forest.size(); ++i) {
    forest[i].RemoveConnections(will_remove);
  }
  return forest;
}
} // namespace sigen


/*
 *
 * Copyright Jeremy Conlin 2008
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HSEQR_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HSEQR_HPP

#include <iostream>
#include <complex>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/matrix_traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

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

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Compute eigenvalues of an Hessenberg matrix, H.
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * hseqr() computes the eigenvalues of a Hessenberg matrix H
     * and, optionally, the matrices T and Z from the Schur decomposition
     * H = Z U Z**T, where U is an upper quasi-triangular matrix (the
     * Schur form), and Z is the orthogonal matrix of Schur vectors.
     *
     * Optionally Z may be postmultiplied into an input orthogonal
     * matrix Q so that this routine can give the Schur factorization
     * of a matrix A which has been reduced to the Hessenberg form H
     * by the orthogonal matrix Q:  A = Q*H*Q**T = (QZ)*U*(QZ)**T.
     *
     * There are two forms of the hseqr function:
     *
     * int hseqr( const char job, A& H, W& w)
     * int hseqr( const char job, const char compz, A& H, W& w, Z& z)
     *
     * The first form does not compute Schur vectors and is equivelant to
     * setting compz = 'N' in the second form.  hseqr returns a '0' if the
     * computation is successful.
     *
     * job.
     *   = 'E': compute eigenvalues only
     *   = 'S': compute eigenvalues and the Schur form U
     *
     * compz. (input)
     *   = 'N':  no Schur vectors are computed;  Equivalent to using the
     *           first form of the hseqr function.
     *   = 'I':  Z is initialized to the unit matrix and the matrix Z
     *           of Schur vectors of H is returned;
     *   = 'V':  Z must contain an orthogonal matrix Q on entry, and
     *           the product Q*Z is returned.
     *
     * H is the Hessenberg matrix whose eigenpairs you're interested
     * in. (input/output) On exit, if computation is successful and
     * job = 'S', then H contains the
     * upper quasi-triangular matrix U from the Schur decomposition
     * (the Schur form); 2-by-2 diagonal blocks (corresponding to
     * complex conjugate pairs of eigenvalues) are returned in
     * standard form, with H(i,i) = H(i+1,i+1) and
     * H(i+1,i)*H(i,i+1) < 0. If computation is successful and
     * job = 'E', the contents of H are unspecified on exit.
     *
     * w (output) contains the computed eigenvalues of H which is the diagonal
     * of U. Must be a complex object.
     *
     * Z. (input/output)
     * If compz = 'N', Z is not referenced.
     * If compz = 'I', on entry Z need not be set and on exit,
     * if computation is successful, Z contains the orthogonal matrix Z of the Schur
     * vectors of H.  If compz = 'V', on entry Z must contain an
     * N-by-N matrix Q, which is assumed to be equal to the unit
     * matrix . On exit, if computation is successful, Z contains Q*Z.
     *
     */

    namespace detail {
        // float
        inline
        int hseqr_backend(const char* job, const char* compz, const int* n,
                const int ilo, const int ihi, float* H, const int ldH,
                float* wr, float* wi, float* Z, const int ldz, float* work,
                const int* lwork){
            int info;
//          std::cout << "I'm inside lapack::detail::hseqr_backend for floats"
//              << std::endl;
            LAPACK_SHSEQR(job, compz, n, &ilo, &ihi, H, &ldH, wr, wi,
                    Z, &ldz, work, lwork, &info);
            return info;
        }

        // double
        inline
        int hseqr_backend(const char* job, const char* compz, const int* n,
                const int ilo, const int ihi, double* H, const int ldH,
                double* wr, double* wi, double* Z, const int ldz, double* work,
                const int* lwork){
            int info;
//          std::cout << "I'm inside lapack::detail::hseqr_backend for doubles"
//              << std::endl;
            LAPACK_DHSEQR(job, compz, n, &ilo, &ihi, H, &ldH, wr, wi,
                    Z, &ldz, work, lwork, &info);
            return info;
        }

        // complex<float>
        inline
        int hseqr_backend(const char* job, const char* compz, int* n,
                const int ilo, const int ihi, traits::complex_f* H, const int ldH,
                traits::complex_f* w, traits::complex_f* Z, int ldz,
                traits::complex_f* work, const int* lwork){
            int info;
//          std::cout << "I'm inside lapack::detail::hseqr_backend for complex<float>"
//              << std::endl;
            LAPACK_CHSEQR(job, compz, n, &ilo, &ihi,
                    traits::complex_ptr(H), &ldH,
                    traits::complex_ptr(w),
                    traits::complex_ptr(Z), &ldz,
                    traits::complex_ptr(work), lwork, &info);
            return info;
        }

        // complex<double>
        inline
        int hseqr_backend(const char* job, const char* compz, int* n,
                const int ilo, const int ihi, traits::complex_d* H, const int ldH,
                traits::complex_d* w, traits::complex_d* Z, int ldz,
                traits::complex_d* work, const int* lwork){
            int info;
//          std::cout << "I'm inside lapack::detail::hseqr_backend for complex<double>"
//              << std::endl;
            LAPACK_ZHSEQR(job, compz, n, &ilo, &ihi,
                    traits::complex_ptr(H), &ldH,
                    traits::complex_ptr(w),
                    traits::complex_ptr(Z), &ldz,
                    traits::complex_ptr(work), lwork, &info);
            return info;
        }

        template <int N>
        struct Hseqr{};

        template <>
        struct Hseqr< 1 >{
            template < typename A, typename W, typename V>
            int operator() ( const char job, const char compz, A& H, W& w, V& Z ){
//              std::cout << "Inside Hseqr<1>." << std::endl;

                int n = traits::matrix_size1(H);
                typedef typename A::value_type value_type;
                traits::detail::array<value_type> wr(n);
                traits::detail::array<value_type> wi(n);

                // workspace query
                int lwork = -1;
                value_type work_temp;
                int result = detail::hseqr_backend(&job, &compz, &n, 1, n,
                                            traits::matrix_storage(H),
                                            traits::leading_dimension(H),
                                            wr.storage(), wi.storage(),
                                            traits::matrix_storage(Z),
                                            traits::leading_dimension(Z),
                                            &work_temp, &lwork);

                if( result !=0 ) return result;

                lwork = (int) work_temp;
                traits::detail::array<value_type> work(lwork);
                result = detail::hseqr_backend(&job, &compz, &n, 1, n,
                                            traits::matrix_storage(H),
                                            traits::leading_dimension(H),
                                            wr.storage(), wi.storage(),
                                            traits::matrix_storage(Z),
                                            traits::leading_dimension(Z),
                                            work.storage(), &lwork);

                for (int i = 0; i < n; i++)
                    w[i] = std::complex<value_type>(wr[i], wi[i]);

                return result;
            }
        };

        template <>
        struct Hseqr< 2 >{
            template < typename A, typename W, typename V>
            int operator() ( const char job, const char compz, A& H, W& w, V& Z ){
//              std::cout << "Inside Hseqr<2>." << std::endl;

                int n = traits::matrix_size1(H);
                typedef typename A::value_type value_type;

                // workspace query
                int lwork = -1;
                value_type work_temp;
                int result = detail::hseqr_backend(&job, &compz, &n, 1, n,
                        traits::matrix_storage(H),
                        traits::leading_dimension(H),
                        traits::vector_storage(w),
                        traits::matrix_storage(Z), traits::leading_dimension(Z),
                        &work_temp, &lwork);

                if( result !=0 ) return result;

                lwork = (int) std::real(work_temp);
                traits::detail::array<value_type> work(lwork);
                result = detail::hseqr_backend(&job, &compz, &n, 1, n,
                        traits::matrix_storage(H),
                        traits::leading_dimension(H),
                        traits::vector_storage(w),
                        traits::matrix_storage(Z), traits::leading_dimension(Z),
                        work.storage(), &lwork);

                return result;
            }
        };

        template < typename A, typename W, typename V>
        int hseqr( const char job, const char compz, A& H, W& w, V& Z ){
//          std::cout << "I'm inside lapack::detail::hseqr." << std::endl;

            assert ( job == 'E' || job == 'S' );
            assert ( compz == 'N' || compz == 'I' || compz == 'V' );

            typedef typename A::value_type value_type;

            int result = detail::Hseqr< n_workspace_args<value_type>::value >()(
                    job, compz, H, w, Z);

            return result;
        }
    }   // namespace detail

    // Compute eigenvalues without the Schur vectors
    template < typename A, typename W>
    int hseqr( const char job, A& H, W& w){
      // input checking
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
			   typename traits::matrix_traits<A>::matrix_structure,
			   traits::general_t
			   >::value));
#endif

#ifndef NDEBUG
        int const n = traits::matrix_size1(H);
#endif

        typedef typename A::value_type value_type;
        typedef typename W::value_type complex_value_type;

        assert(traits::matrix_size2(H) == n); // Square matrix
        assert(traits::vector_size(w) == n);

        ublas::matrix<value_type, ublas::column_major> Z(1,1);
        return detail::hseqr( job, 'N', H, w, Z );
    }

    // Compute eigenvalues and the Schur vectors
    template < typename A, typename W, typename Z>
    int hseqr( const char job, const char compz, A& H, W& w, Z& z){
      // input checking
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
			   typename traits::matrix_traits<A>::matrix_structure,
			   traits::general_t
			   >::value));
#endif

#ifndef NDEBUG
        int const n = traits::matrix_size1(H);
#endif

        typedef typename A::value_type value_type;
        assert(traits::matrix_size2(H) == n); // Square matrix
        assert(traits::vector_size(w) == n);
        assert(traits::matrix_size2(z) == n);

        return detail::hseqr( job, compz, H, w, z );
    }

  }
}}}

#endif