#include <Eigen/Core>
#include <iostream>
#include <mathtoolbox/acquisition-functions.hpp>
#include <mathtoolbox/gaussian-process-regression.hpp>
#include <mathtoolbox/probability-distributions.hpp>
#include <timer.hpp>

using Eigen::Matrix2d;
using Eigen::MatrixXd;
using Eigen::Vector2d;
using Eigen::VectorXd;

double CalcFunc(const Vector2d& x)
{
    return mathtoolbox::GetNormalDist(3.0 * x, Vector2d::Zero(), Matrix2d::Identity(), 1.0);
}

int main(int argc, char** argv)
{
    // Define the scene setting
    constexpr int    num_samples     = 10;
    constexpr double noise_intensity = 1e-04;

    // Generate scattered data
    MatrixXd X(2, num_samples);
    VectorXd y(num_samples);
    for (int i = 0; i < num_samples; ++i)
    {
        X.col(i) = Vector2d::Random();
        y(i)     = CalcFunc(X.col(i)) + noise_intensity * (VectorXd::Random(1))(0);
    }

    // Define the kernel type
    const auto kernel_type = mathtoolbox::GaussianProcessRegressor::KernelType::ArdMatern52;

    // Instantiate the interpolation object
    mathtoolbox::GaussianProcessRegressor regressor(X, y, kernel_type);

    // Perform hyperparameter estimation
    const Eigen::Vector3d default_kernel_hyperparams(0.50, 0.50, 0.50);
    regressor.PerformMaximumLikelihood(default_kernel_hyperparams, 1e-04);

    // Calculate EI values and their derivatives
    for (int i = 0; i < 100; ++i)
    {
        constexpr int    num_dims = 2;
        constexpr double epsilon  = 1e-06;

        const VectorXd x_plus = [&]() {
            int index;
            y.maxCoeff(&index);
            return X.col(index);
        }();

        const VectorXd x = Vector2d::Random();

        const VectorXd acquisition_deriv = mathtoolbox::GetExpectedImprovementDerivative(
            x,
            [&](const VectorXd& x) { return regressor.PredictMean(x); },
            [&](const VectorXd& x) { return regressor.PredictStdev(x); },
            x_plus,
            [&](const VectorXd& x) { return regressor.PredictMeanDeriv(x); },
            [&](const VectorXd& x) { return regressor.PredictStdevDeriv(x); });

        VectorXd acquisition_numerical_deriv(num_dims);
        for (int d = 0; d < num_dims; ++d)
        {
            VectorXd delta = VectorXd::Zero(num_dims);
            delta(d)       = epsilon;

            const double value_plus = mathtoolbox::GetExpectedImprovement(
                x + delta,
                [&](const VectorXd& x) { return regressor.PredictMean(x); },
                [&](const VectorXd& x) { return regressor.PredictStdev(x); },
                x_plus);

            const double value_minus = mathtoolbox::GetExpectedImprovement(
                x - delta,
                [&](const VectorXd& x) { return regressor.PredictMean(x); },
                [&](const VectorXd& x) { return regressor.PredictStdev(x); },
                x_plus);

            acquisition_numerical_deriv(d) = value_plus - value_minus;
        }
        acquisition_numerical_deriv /= 2.0 * epsilon;

        const auto scale     = acquisition_deriv.norm();
        const auto abs_error = (acquisition_deriv - acquisition_numerical_deriv).norm();
        const auto rel_error = abs_error / scale;

        if (scale > 1e-06 && rel_error > 1e-02)
        {
            std::cout << "point location: " << x.transpose() << std::endl;
            std::cout << "analytic      : " << acquisition_deriv.transpose() << std::endl;
            std::cout << "numerical     : " << acquisition_numerical_deriv.transpose() << std::endl;
            std::cout << "error         : " << abs_error << std::endl;

            exit(1);
        }
    }

    // Calculate GP-UCB values and their derivatives
    for (int i = 0; i < 100; ++i)
    {
        constexpr int    num_dims = 2;
        constexpr double epsilon  = 1e-06;

        const double hyperparam = 1.0 + (Eigen::VectorXd::Random(1))(0);

        const VectorXd x = Vector2d::Random();

        const VectorXd acquisition_deriv = mathtoolbox::GetGaussianProcessUpperConfidenceBoundDerivative(
            x,
            [&](const VectorXd& x) { return regressor.PredictMean(x); },
            [&](const VectorXd& x) { return regressor.PredictStdev(x); },
            hyperparam,
            [&](const VectorXd& x) { return regressor.PredictMeanDeriv(x); },
            [&](const VectorXd& x) { return regressor.PredictStdevDeriv(x); });

        VectorXd acquisition_numerical_deriv(num_dims);
        for (int d = 0; d < num_dims; ++d)
        {
            VectorXd delta = VectorXd::Zero(num_dims);
            delta(d)       = epsilon;

            const double value_plus = mathtoolbox::GetGaussianProcessUpperConfidenceBound(
                x + delta,
                [&](const VectorXd& x) { return regressor.PredictMean(x); },
                [&](const VectorXd& x) { return regressor.PredictStdev(x); },
                hyperparam);

            const double value_minus = mathtoolbox::GetGaussianProcessUpperConfidenceBound(
                x - delta,
                [&](const VectorXd& x) { return regressor.PredictMean(x); },
                [&](const VectorXd& x) { return regressor.PredictStdev(x); },
                hyperparam);

            acquisition_numerical_deriv(d) = value_plus - value_minus;
        }
        acquisition_numerical_deriv /= 2.0 * epsilon;

        const auto scale     = acquisition_deriv.norm();
        const auto abs_error = (acquisition_deriv - acquisition_numerical_deriv).norm();
        const auto rel_error = abs_error / scale;

        if (scale > 1e-06 && rel_error > 1e-02)
        {
            std::cout << "point location: " << x.transpose() << std::endl;
            std::cout << "analytic      : " << acquisition_deriv.transpose() << std::endl;
            std::cout << "numerical     : " << acquisition_numerical_deriv.transpose() << std::endl;
            std::cout << "error         : " << abs_error << std::endl;

            exit(1);
        }
    }

    return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include <math.h>

//Quaternion Exponential
//implements simple 0th-order integration
//ref: quaternion kinematics, section 4.6.1
using Eigen::Quaternionf;
using Eigen::Vector3f;
Quaternionf qExponential(float dt, Vector3f w)
{
  float wn = w.norm();
  Vector3f wN = w.normalized();
  Quaternionf qReturn;
  qReturn.w() = cos(wn * dt / 2);
  qReturn.vec() = wN * sin(wn * dt / 2);
  return qReturn;
}

int main()
{
  using Eigen::Quaternionf;
  using Eigen::Vector3f;
  float dt = 0.01;
  Vector3f w(1.0, 2.0, -3.0);
  Quaternionf qExp = qExponential(dt, w);
  std::cout << qExp.w() << std::endl << qExp.vec() << std::endl;
}


#include <blitzml/base/math_util.h>

#include <utility>

using std::pair;

namespace BlitzML {

pair<value_t, value_t> compute_quadratic_roots(value_t a, value_t b,
                                                    value_t c) {
  value_t discriminant = b*b - 4*a*c;
  bool result_exists = (discriminant >= 0);
  if (a == 0. && b == 0. && c != 0.) {
    result_exists = false;
  }
  if (!result_exists) {
    value_t nan = std::numeric_limits<value_t>::quiet_NaN();
    return pair<value_t, value_t>(nan, nan);
  }
  if (a == 0.) {
    value_t result = -c/b;
    return pair<value_t, value_t>(result, result);
  }

  value_t sqrt_discriminant = sqrt(discriminant);
  value_t root1 = (-b - sqrt_discriminant) / (2 * a);
  value_t root2 = (-b + sqrt_discriminant) / (2 * a);
  if (a > 0) {
    return pair<value_t, value_t>(root1, root2);
  } else {
    return pair<value_t, value_t>(root2, root1);
  }
}

} // namespace BlitzML


#include <cmath>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include "kernel.h"

const std::map<std::string, double> calculator::WExpression::global_constants
{
    {"pi", boost::math::constants::pi<double>()},
    {"e", boost::math::constants::e<double>()}
};

double calculator::WExpression::expression()
{
    Token t {ts.pop()};
    bool isNeg {false};
    if (t.kind == TokenKind::minus)
        isNeg = true;
    else if (t.kind == TokenKind::plus)
        isNeg = false;
    else
        ts.push(t);

    double left {term()};
    if (isNeg) left = -left;

    while (true)
    {
        Token t {ts.pop()};
        switch(t.kind)
        {
            case TokenKind::plus:
                left += term();
                break;
            case TokenKind::minus:
                left -= term();
                break;
            default:
                ts.push(t);
                return left;
        }
    }
}

double calculator::WExpression::term()
{
    using boost::numeric_cast;
    double left {primary()};

    while (true)
    {
        Token t {ts.pop()};
        switch (t.kind) {
            case TokenKind::multiply:
                left *= primary();
                break;
            case TokenKind::divide:
            {
                double d {primary()};
                if (d == 0.0) throw std::logic_error("division by 0");
                left /= d;
                break;
            }
            case TokenKind::mod:
            {
                double d{primary()};
                int i1 = numeric_cast<int>(left);
                double di1 = numeric_cast<double>(i1);
                if (left - di1 > std::numeric_limits<double>::min())
                    throw std::logic_error("left operand of %(mod) is not integer");
                int i2 = numeric_cast<int>(d);
                double di2 = numeric_cast<double>(i2);
                if (d - di2 > std::numeric_limits<double>::min())
                    throw std::logic_error("right operand of %(mod) is not integer");
                left = i1 % i2;
                break;
            }
            default:
                ts.push(t);
                return left;
        }
    }
}

double calculator::WExpression::primary()
{
    using boost::numeric_cast;
    double result {0.0};
    Token t {ts.pop()};
    auto endBracket = [](TokenKind tk)->TokenKind{
        switch (tk)
        {
            case TokenKind::bracket_left0:
                return TokenKind::bracket_right0;
            case TokenKind::bracket_left1:
                return TokenKind::bracket_right1;
            default:
                return tk;
        }
    };
    switch (t.kind) {
        case TokenKind::number:
        {
            result = t.value;
            break;
        }
        case TokenKind::variable:
        {
            // first find in constants
            auto gcit {global_constants.find(t.name)};
            if (gcit == cend(global_constants))
            {
                auto vit = variables.find(t.name);
                if (vit == cend(variables))
                {
                    throw std::logic_error("undefined symbolic literal " + t.name);
                }
                result = vit->second;
                break;
            }
            result = gcit->second;
            break;
//            auto it {variables.find(t.name)};
//            if (it == end(variables))
//                throw std::logic_error("undefined variable "+t.name);
//            result = it->second;
//            break;
        }
        case TokenKind::function:
        {
            result = function(t.name);
            break;
        }
        case TokenKind::bracket_left0:
        case TokenKind::bracket_left1:
        {
            double d {expression()};
            Token t2 {ts.pop()};
            if (t2.kind != endBracket(t.kind)/*TokenKind::bracket_right0*/)
                throw std::logic_error("bracket_right expected");
            result = d;
            break;
        }
//        case TokenKind::bracket_left1:
//        {
//            double d {expression()};
//            t = ts.pop();
//            if (t.kind != TokenKind::bracket_right1)
//                throw std::logic_error("bracket_right expected");
//            result = d;
//            break;
//        }
        default:
            throw std::logic_error("primary is expected");
    }
    while (true)
    {
        Token tp {ts.pop()};
        switch (tp.kind)
        {
            case TokenKind::factorial:
            {
                unsigned int f = numeric_cast<unsigned int>(result);
                double df = numeric_cast<double>(f);
                if (result-df > std::numeric_limits<double>::min())
                    throw std::logic_error("factorial applied to not integer");
                result = boost::math::factorial<double>(f);
                break;
            }
            default:
            {
                ts.push(tp);
                return result;
            }
        }
    }
//    return result;
//    switch (tf.kind)
//    {
//        case TokenKind::factorial:
//        {
//            unsigned int f = numeric_cast<unsigned int>(result);
//            double df = numeric_cast<double>(f);
//            if (result-df > std::numeric_limits<double>::min())
//                throw std::logic_error("factorial applied to not integer");
//            return boost::math::factorial<double>(f);
//        }
//        default:
//            ts.push(tf);
//            return result;
    //    }
}

double calculator::WExpression::function(std::string function_name)
{
//    double result {0.0};
    Token t {ts.pop()};
    std::vector<double> args;
    auto endBracket = [](TokenKind tk)->TokenKind{
        switch (tk)
        {
            case TokenKind::bracket_left0:
                return TokenKind::bracket_right0;
            case TokenKind::bracket_left1:
                return TokenKind::bracket_right1;
            default:
                return tk;
        }
    };
    switch (t.kind)
    {
        case TokenKind::bracket_left0:
        case TokenKind::bracket_left1:
        {
            while (ist)
            {
                double d {expression()};
                args.push_back(d);
                Token t2 {ts.pop()};
                if (t2.kind != TokenKind::comma)
                {
                    ts.push(t2);
                    break;
                }
            } // args is done
            Token t3 {ts.pop()};
            if (t3.kind != endBracket(t.kind))
                throw std::logic_error("correct bracket_right is expected");
            break;
        }
        default:
            throw std::logic_error("bracket_left is expected");
    }
    if (function_name == "pow")
    {
        if (args.size() != 2)
            throw std::logic_error("2 args for pow function");
        return std::pow(args[0], args[1]);
    }
    else if (function_name == "cos")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for cos function");
        return std::cos(args[0]);
    }
    else if (function_name == "sin")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for sin function");
        return std::sin(args[0]);
    }
    else if (function_name == "tan")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for tan function");
        return std::tan(args[0]);
    }
    else if (function_name == "acos")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for acos function");
        return std::acos(args[0]);
    }
    else if (function_name == "asin")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for asin function");
        return std::asin(args[0]);
    }
    else if (function_name == "atan")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for atan function");
        return std::atan(args[0]);
    }
    else if (function_name == "atan2")
    {
        if (args.size() != 2)
            throw std::logic_error("2 args for atan2 function");
        return std::atan2(args[0], args[1]);
    }
    else if (function_name == "exp")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for exp function");
        return std::exp(args[0]);
    }
    else if (function_name == "log")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for log function");
        return std::log(args[0]);
    }
    else if (function_name == "log10")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for log10 function");
        return std::log10(args[0]);
    }
    else if (function_name == "sqrt")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for sqrt function");
        return std::sqrt(args[0]);
    }
    else if (function_name == "abs")
    {
        if (args.size() != 1)
            throw std::logic_error("1 args for abs function");
        return std::fabs(args[0]);
    }
    else
    {
        throw std::logic_error("undefined function"+function_name);
    }

}

calculator::Token calculator::Token_stream::pop()
{
    if (!(data.empty()))
    {
        auto res = data.top();
        data.pop();
        return res;
    }
    Token result;
    if (!ist) return result;
    char sym {0};
    ist >> sym;
    if (ist.eof()) return result;
    switch (sym)
    {
        case '(':
            result.kind = TokenKind::bracket_left0;
            break;
        case ')':
            result.kind = TokenKind::bracket_right0;
            break;
        case '{':
            result.kind = TokenKind::bracket_left1;
            break;
        case '}':
            result.kind = TokenKind::bracket_right1;
            break;
        case '*':
            result.kind = TokenKind::multiply;
            break;
        case '/':
            result.kind = TokenKind::divide;
            break;
        case '%':
            result.kind = TokenKind::mod;
            break;
        case '+':
            result.kind = TokenKind::plus;
            break;
        case '-':
            result.kind = TokenKind::minus;
            break;
        case '!':
            result.kind = TokenKind::factorial;
            break;
        case ',':
            result.kind = TokenKind::comma;
            break;
        case '.':
        case '0':
        case '1':
        case '2':
        case '3':
        case '4':
        case '5':
        case '6':
        case '7':
        case '8':
        case '9':
        {
            ist.unget();
            double val {0.0};
            ist >> val;
            result.kind = TokenKind::number;
            result.value = val;
            break;
        }
        default:
        {
            if (isalpha(sym)) // function or variable name: func ( | lsldkm239283
            {
                std::string s;
                s += sym;
                char ch {0};
                while (ist.get(ch) && (isalpha(ch) || isdigit(ch))) s += ch;
                ist.putback(ch);
                ist >> ch;
                if (ch == '(' || ch == '{')
                {
                    ist.putback(ch);
                    result.kind = TokenKind::function;
                    result.name = s;
                    break;
                }
                else
                {
                    ist.putback(ch);
                    result.kind = TokenKind::variable;
                    result.name = s;
                    break;
                }
            }
            else
            {
                throw std::logic_error("bad Token");
            }
        }
    }
    return result;
}


#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <vector>
#include <map>
#include <png.h>
#include <TaskGraph>
#include <TaskUserFunctions.h>
#include <boost/ptr_container/ptr_vector.hpp>
#include "mandelbrot.hpp"

using namespace tg;

const float gamma_exponent = 3.5;
const int max_iterations = 1000;

const int max_alloc = 1024*176; // Allocate up to 176K
const int width = 12800;
const int height = 9600;
const char* output_filename = "mandelbrot_set_tu.png";

struct TgInfo
{
  int startRow;
  int rowCount;
  char* data;
};

int main(int argc, char* argv[])
{
  int threads;
  const Options options = getOptions("mandelbrot_tu", argc, argv);

  if (options.threads < 1 || options.threads > NUM_SPES)
  {
    std::cerr << "Please select a number of threads between 1 and " << NUM_SPES << " (inclusive)." << std::endl;
    exit(EXIT_FAILURE);
  }
  else
  {
    threads = options.threads;
  }

  const int rowSize = width * sizeof(char);
  const int rowMultiple = max_alloc/rowSize;
  unsigned int arraySize[2] = {rowMultiple, width};

  std::cout << "Calculating mandelbrot of size " << width << " x " << height << "." << std::endl;
  std::cout << "Will transfer data in blocks of " << rowSize*rowMultiple << " bytes." << std::endl;

  tuTaskGraph mandel;
  tu_taskgraph(mandel)
  {
    tParameter(tVarNamed(int, startRow, "startRow"));
    tParameter(tVarNamed(int, rowCount, "rowCount"));
    tParameter(tArrayFromListNamed(char, imageData, 2, arraySize, "imageData"));

    tVar(float, x);
    tVar(float, y);
    tVar(float, x0);
    tVar(float, y0);
    tVar(float, xtemp);
    tVar(int, x_pos);
    tVar(int, y_pos);
    tVar(int, iteration);
    tVar(float, pixel);

    tFor(y_pos, startRow, startRow+rowCount-1)
    {
      y0 = (y_pos * (3.0f / height)) - 1.5f;

      tFor(x_pos, 0, width-1)
      {
        // Scale the pixel location to be +- 2 from the origin of the complex plane
        x0 = (x_pos * (3.5f / width)) - 2.5f;
        x = x0;
        y = y0;
        iteration = 0;

        // Test if this location is in the set
        tWhile ((x*x + y*y) < 4.0f && iteration  < max_iterations) {
          xtemp = (x*x) - (y*y) + x0;
          y = 2*x*y + y0;
          x = xtemp;
          iteration+=1;
        }

        // Calculate and set the value of the pixel
        tIf(iteration == max_iterations)
          pixel = 0.0f;
        tElse
          pixel = 8.0f * (iteration + 1.0f - tLogf(tLogf(tSqrtf(x*x + y*y))) / std::log(2.0f)) / max_iterations;

        imageData[y_pos-startRow][x_pos] = tPowf(pixel, (1.0f / gamma_exponent)) * 255.0f;
      }
    }
  }

  mandel.compile(tg::SPU_GCC, false);

  boost::ptr_vector<tuTaskGraph> taskGraphs;
  std::map<tuTaskGraph*, TgInfo> tgInfo;
  TaskFarm farm;

  for(int thread=0; thread<threads; ++thread)
  {
    taskGraphs.push_back(new tuTaskGraph(mandel));
    farm.add(&taskGraphs.back());
    tgInfo[&taskGraphs.back()].data = static_cast<char*>(spu_malloc(rowSize*rowMultiple));
  }

  FILE *output_file;
  png_structp png_ptr;
  png_infop info_ptr;

  if (!(output_file = fopen(output_filename, "wb"))) {
    std::cerr << "Unable to open " << output_filename << " for writing. Aborting now." << std::endl;
    exit(1);
  }

  if (!(png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL))) {
    std::cerr << "Failed to create png write struct. Aborting now." << std::endl;
    exit(1);
  }

  if (!(info_ptr = png_create_info_struct(png_ptr))) {
    std::cerr << "Failed to create png info struct. Aborting now." << std::endl;
    // Clean up
    png_destroy_write_struct(&png_ptr,static_cast<png_infopp>(NULL));
    exit(1);
  }

  png_init_io(png_ptr, output_file);
  png_set_IHDR(png_ptr, info_ptr, width, height, 8,
               PNG_COLOR_TYPE_PALETTE,
               PNG_INTERLACE_NONE,
               PNG_COMPRESSION_TYPE_DEFAULT,
               PNG_FILTER_TYPE_DEFAULT);
  png_set_gamma(png_ptr, 0.5, 0.45455);

  std::vector<png_color> palette(generateRedYellowColourMap());
  png_set_PLTE(png_ptr, info_ptr, &palette[0], palette.size());
  png_write_info(png_ptr, info_ptr);

  int currentRow = 0;

  while(currentRow < height)
  {
    for(int thread=0; thread<threads; ++thread)
    {
      tgInfo[&taskGraphs[thread]].startRow = currentRow;
      tgInfo[&taskGraphs[thread]].rowCount = std::min(rowMultiple, height-currentRow);
      currentRow+=tgInfo[&taskGraphs[thread]].rowCount;

      taskGraphs[thread].setParameter("startRow", &tgInfo[&taskGraphs[thread]].startRow);
      taskGraphs[thread].setParameter("rowCount", &tgInfo[&taskGraphs[thread]].rowCount);
      taskGraphs[thread].setParameter("imageData", tgInfo[&taskGraphs[thread]].data);
    }

    farm.execute();

    for(int thread=0; thread<threads; ++thread)
    {
      for (int line = 0; line<tgInfo[&taskGraphs[thread]].rowCount; ++line)
      {
        png_bytep png_row_ptr = reinterpret_cast<png_bytep>(tgInfo[&taskGraphs[thread]].data + rowSize*line);
        png_write_row(png_ptr, png_row_ptr);
      }
    }
  }

  std::cout << "Done." << std::endl;

  png_write_end(png_ptr, info_ptr);
  png_destroy_write_struct(&png_ptr, &info_ptr);

  for(int thread=0; thread<threads; ++thread)
    spu_free(tgInfo[&taskGraphs[thread]].data);

  exit(EXIT_SUCCESS);
}


#include <string>
#include <memory>
#include <complex>
#include <vector>
#include <cmath>
#include <sndfile.h>
#include <fftw3.h>
// #include <boost/format.hpp>

using std::complex;
using std::vector;
using std::pair;

// LPC 係数の次数
const int LPC_ORDER = 64;

// 解析に用いる微小区間 (sec)
const double WINDOW_DURATION = 0.04;

// FFT する
vector<double> fft(const vector<double>& input);

// LPC スペクトル包絡を得る
vector<double> lpc(const vector<double>& input, int order, double df);

// [1 : -1] に正規化
vector<double> normalize(const vector<double>& input);

// ハミング窓をかける
vector<double> hamming(const vector<double>& input);

// デジタルフィルタ
vector<double> freqz(const vector<double>& b, const vector<double>& a, double df, int N);

// フォルマント f1 / f2 を返す
pair<double, double> formant(const vector<double>& input, double df);

// フォルマントから母音を推定する
std::string vowel(double f1, double f2);

// 与えられた範囲の音の平均
double volume(const vector<double>& input);

int main()
{
	// 音声ファイル読み込み
	SF_INFO sinfo;
	std::shared_ptr<SNDFILE> sf(
		sf_open("aiueo.wav", SFM_READ, &sinfo), [](SNDFILE* ptr) { sf_close(ptr); });
	const int frame = sinfo.frames;
	std::unique_ptr<short[]> input(new short[frame]);
	const double sample_rate = sinfo.samplerate;
	const double dt = 1.0 / sample_rate;
	sf_read_short(sf.get(), input.get(), frame);

	// 微小音声区間取り出し
	const int window_size = static_cast<int>(WINDOW_DURATION / dt);
	const double df  = 1.0 / (window_size / sample_rate);
	for (int n = 0; n < frame / window_size; ++n) {
		vector<double> data;
		for (int i = window_size * n; i < window_size * (n + 1) && i < frame; ++i) {
			data.push_back(input[i]);
		}
		auto hamming_result = normalize( hamming(data) );

		auto lpc_result = normalize( lpc(hamming_result, LPC_ORDER, df) );
		auto fft_result = normalize( fft(hamming_result) );

		// 得られた LPC スペクトル包絡線からフォルマントを抽出
		auto formant_result = formant(lpc_result, df);
		double f1 = formant_result.first;
		double f2 = formant_result.second;

		if (volume(data) < 1e4) {
			std::cout << "-";
		} else {
			std::cout << boost::format("%1%  (f1:%2%, f2:%3%)")
				% vowel(f1, f2) % f1 % f2;
		}
		std::cout << std::endl;
	}

	// Gnuplot で結果を確認
	std::shared_ptr<FILE> freq_graph(
		popen("gnuplot -persist", "w"), [](FILE* ptr) { pclose(ptr); });
	fprintf(freq_graph.get(), "load 'freq.plt'\n");

	// std::shared_ptr<FILE> wav_graph(
	// 	popen("gnuplot -persist", "w"), [](FILE* ptr) { pclose(ptr); });
	// fprintf(wav_graph.get(), "load 'wav.plt'\n");

	return 0;
}

// FFT
vector<double> fft(const vector<double>& input)
{
	const int frame = input.size();
	vector<complex<double>> in(frame), out(frame);
	auto plan = fftw_plan_dft_1d(
			frame,
			reinterpret_cast<fftw_complex*>(&in[0]),
			reinterpret_cast<fftw_complex*>(&out[0]),
			FFTW_FORWARD, FFTW_ESTIMATE
	);
	for (int i = 0; i < frame; ++i) {
		in[i] = complex<double>(input[i], 0.0);
	}
	fftw_execute(plan);

	vector<double> fft_result(frame);
	for (int i = 0; i < frame; ++i) {
		fft_result[i] = abs(out[i]);
	}

	return fft_result;
}

// LPC
vector<double> lpc(const vector<double>& input, int order, double df)
{
	const int N = input.size();

	// 自己相関関数
	vector<double> r(N);
	const int lags_num = order + 1;
	for (int l = 0; l < lags_num; ++l) {
		r[l] = 0.0;
		for (int n = 0; n < N - l; ++n) {
			r[l] += input[n] * input[n + l];
		}
	}

	// Levinson-Durbin のアルゴリズムで LPC 係数を計算
	vector<double> a(order + 1, 0.0), e(order + 1, 0.0);
	a[0] = e[0] = 1.0;
	a[1] = - r[1] / r[0];
	e[1] = r[0] + r[1] * a[1];
	for (int k = 1; k < order; ++k) {
		double lambda = 0.0;
		for (int j = 0; j < k + 1; ++j) {
			lambda -= a[j] * r[k + 1 - j];
		}
		lambda /= e[k];

		vector<double> U(k + 2), V(k + 2);
		U[0] = 1.0; V[0] = 0.0;
		for (int i = 1; i < k + 1; ++i) {
			U[i] = a[i];
			V[k + 1 - i] = a[i];
		}
		U[k + 1] = 0.0; V[k + 1] = 1.0;

		for (int i = 0; i < k + 2; ++i) {
			a[i] = U[i] + lambda * V[i];
		}

		e[k + 1] = e[k] * (1.0 - lambda * lambda);
	}

	// LPC 係数から音声信号再現
	// vector<double> lpc_result(N, 0.0);
	// for (int i = 0; i < N; ++i) {
	// 	if (i < order) {
	// 		lpc_result[i] = input[i];
	// 	} else {
	// 		for (int j = 1; j < order; ++j) {
	// 			lpc_result[i] -= a[j] * input[i + 1 - j];
	// 		}
	// 	}
	// }
	// return lpc_result;

	return freqz(e, a, df, N);
}

// 正規化
vector<double> normalize(const vector<double>& input)
{
	// 最大 / 最小値
	auto max = abs( *std::max_element(input.begin(), input.end()) );
	auto min = abs( *std::min_element(input.begin(), input.end()) );
	double factor = std::max(max, min);
	vector<double> result( input.size() );
	std::transform(input.begin(), input.end(), result.begin(), [factor](double x) {
		return x / factor;
	});
	return result;
}

// ハミング窓をかける
vector<double> hamming(const vector<double>& input)
{
	const double N = input.size();
	vector<double> result(N);
	for (int i = 1; i < N - 1; ++i) {
		const double h = 0.54 - 0.46 * cos(2 * M_PI * i / (N - 1));
		result[i] = input[i] * h;
	}
	result[0] = result[N - 1] = 0;
	return result;
}

// デジタルフィルタ
vector<double> freqz(const vector<double>& b, const vector<double>& a, double df, int N)
{
	vector<double> H(N);
	for (int n = 0; n < N + 1; ++n) {
		auto z = std::exp(complex<double>(0.0, -2.0 * M_PI * n / N));
		complex<double> numerator(0.0, 0.0), denominator(0.0, 0.0);
		for (int i = 0; i < b.size(); ++i) {
			numerator += b[b.size() - 1 - i] * pow(z, i);
		}
		for (int i = 0; i < a.size(); ++i) {
			denominator += a[a.size() - 1 - i] * pow(z, i);
		}
		H[n] = abs(numerator / denominator);
	}

	return H;
}

// フォルマント f1 / f2 を返す
pair<double, double> formant(const vector<double>& input, double df)
{
	pair<double, double> result(0.0, 0.0);
	bool is_find_first = false;
	for (int i = 1; i < input.size() - 1; ++i) {
		if (input[i] > input[i-1] && input[i] > input[i+1]) {
			if (!is_find_first) {
				result.first = df * i;
				is_find_first = true;
			} else {
				result.second = df * i;
				break;
			}
		}
	}
	return result;
}

// 母音を推定
// NOTE: 機械学習にする予定
std::string vowel(double f1, double f2)
{
	if (f1 > 600 && f1 < 1400 && f2 > 900  && f2 < 2000) return "a";
	if (f1 > 100 && f1 < 410  && f2 > 1900 && f2 < 3500) return "i";
	if (f1 > 100 && f1 < 700  && f2 > 1100 && f2 < 2000) return "u";
	if (f1 > 400 && f1 < 800  && f2 > 1700 && f2 < 3000) return "e";
	if (f1 > 300 && f1 < 900  && f2 > 500  && f2 < 1300) return "o";
	return "-";
}

// ボリューム
double volume(const vector<double>& input)
{
	double v = 0.0;
	std::for_each(input.begin(), input.end(), [&v](double x) { v += x*x; });
	v /= input.size();
	return v;
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <complex>
#include <tuple>

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

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/splitting.h"
#include "miMaS/lagrange5.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

#define ping(X) std::cerr << __LINE__ << " " << #X << ":" << X << std::endl
int debug = 0;

auto
maxwellian ( double rho , double u , double T ) {
  //std::cout << rho << " " << u << " " << T << std::endl;
  //std::cout << rho/(std::sqrt(2.*math::pi<double>()*T)) << std::endl;
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int main(int,char**)
{
  std::size_t Nx = 135, Nv = 256;

  // $(u_c,E,\hat{f}_h)$ and $f_h$
  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,0.);
  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);

  const double Kx = 0.5;
  // phase-space domain
  fh.range.v_min = -8.; fh.range.v_max = 8.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();

  // compute dx, dv
  fh.step.dv = (fh.range.v_max-fh.range.v_min)/Nv;
  fh.step.dx = (fh.range.x_max-fh.range.x_min)/Nx;

  double dt = 0.05;//1.*fh.step.dv;
  double Tf = 200.;

  // velocity and frequency
  ublas::vector<double> v (Nv,0.); for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  const double l = fh.range.x_max-fh.range.x_min;
  ublas::vector<double> kx(Nx);
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  // initial condition
  double ui=2., alpha=0.2;
  auto tb_M1 = maxwellian(0.5*alpha,ui,1.) , tb_M2 = maxwellian(0.5*alpha,-ui,1.);
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      //fh[k][i] = ( 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-ui)) + 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+ui)) )*(1.+0.04*std::cos(0.3*Xi(i)));
      //fh[k][i] = ( 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-ui)) + 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+ui)) )*(1.+0.04*std::cos(Kx*Xi(i)));

      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(&(fh[k][0]),&(fh[k][Nx-1])+1,&(hfh[k][0]));
  }
  fh.write("vphl/split/init.dat");

  std::cout << "Nx: " << Nx << "\n";
  std::cout << "Nv: " << Nv << "\n";
  std::cout << "v_min: " << fh.range.v_min << "\n";
  std::cout << "v_max: " << fh.range.v_max << "\n";
  std::cout << "x_min: " << fh.range.x_min << "\n";
  std::cout << "x_max: " << fh.range.x_max << "\n";
  std::cout << "dt: " << dt << "\n";
  std::cout << "dx: " << fh.step.dx << "\n";
  std::cout << "dv: " << fh.step.dv << "\n";
  std::cout << "Tf: " << Tf << "\n";
  std::cout << "f_0: " << "\"tb\"" << "\n";
  std::cout << std::endl;

  const double rho_c = 1.-alpha;
  // init E (electric field) with Poisson
  {
    poisson<double> poisson_solver(Nx,l);
    ublas::vector<double> rho(Nx,0.);
    rho = fh.density(); // compute density from init data
    for ( auto i=0 ; i<Nx ; ++i ) { rho[i] += rho_c; } // add (1-alpha) for cold particules
    E = poisson_solver(rho);
  }

  // monitoring data
  std::vector<double> ee;
  std::vector<double> Emax;
  std::vector<double> times;

  times.push_back(0);
  {
    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
  }
  Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

  //U_type<double,1> U({uc,E,hfh};
  splitting<double,1> Lie( fh , l , rho_c );

  std::size_t i_t = 0;
  double current_time = 0.;
  while ( current_time < Tf ) {
    std::cout << " [" << std::setw(5) << i_t << "] " << i_t*dt << "\r" << std::flush;

    Lie.phi_a(dt,uc,E,hfh);
    Lie.phi_b(dt,uc,E,hfh);
    Lie.phi_c(dt,uc,E,hfh);
    //Lie.phi_b(0.5*dt,uc,E,hfh);
    //Lie.phi_a(0.5*dt,uc,E,hfh);

    Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );

    current_time += dt;
    ++i_t;
    times.push_back( current_time );
  } // while current_time < Tf
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt<<std::endl;

  std::ofstream of;
  std::size_t count = 0;
  auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<times[count++]<<" "<<y; return ss.str(); };

  of.open("vphl/split/ee.dat");
  std::transform( ee.begin() , ee.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open("vphl/split/Emax.dat");
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(&(hfh[k][0]),&(hfh[k][Nx-1])+1,&(fh[k][0])); }
  fh.write("vphl/split/vp.dat");

  return 0;
}


#include <iostream>
#include <Eigen/Dense>

#include <cgi/logger/logger.hpp>
#include <cgi/mosaic/Mosaic.hpp>
#include <cgi/mosaic/MosaicUtility.hpp>
#include <boost/filesystem/convenience.hpp>

static const int nBands = 8;

namespace Eigen {
  typedef Eigen::Matrix<float, nBands, nBands> Matrix8f;
  typedef Eigen::Matrix<float, nBands, 1> Vector8f;
}

Eigen::Vector8f v2v(const std::vector<unsigned short>& sVec)
{
  Eigen::Vector8f eVec;
  for(unsigned int i=0; i<sVec.size(); ++i)
    eVec(i,0) = sVec[i];
  return eVec;
}

std::vector<unsigned short> v2v(const Eigen::VectorXf& eVec)
{
  std::vector<unsigned short> sVec(eVec.rows());
  for(unsigned int i=0; i<sVec.size(); ++i)
    sVec[0] = eVec(i,0);
  return sVec;
}

cgi::Tile<unsigned char> threshold(const cgi::Tile<float>& inTile, const Eigen::VectorXf& thresh)
{
  const cgi::core::Size2d<int>& size(inTile.getSize());
  const int height = size.getHeight();
  const int width = size.getWidth();
  const int depth = inTile.getBandCount();

  cgi::Tile<unsigned char> outTile(size, depth, 0);

  for(int r=0; r<height; ++r)
    {
      for(int c=0; c<width; ++c)
	{
	  for(int b=0; b<depth; ++b)
	    {
	      if(inTile[b](r,c) > thresh(b,0))
		outTile[b](r,c) = 255;
	    }
	}
    }

  return outTile;
}

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

int main(int argc, char** argv)
{
  if(argc != 4)
    {
      std::cerr << "USAGE: " << argv[0] << " <input filename> <output filename> <number of output levels>" << std::endl;
      return 1;
    }

  cgi::log::MetaLogStream::instance().setTargets(cgi::log::STDOUT);
  cgi::log::MetaLogStream::instance().setPriorityThreshold(cgi::log::Priority::DEBUG);
  cgi::log::MetaLogStream& log (cgi::log::MetaLogStream::instance());
  ////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////

  const std::string outputFile(argv[2]);
  CGI_THROW_IF(true == boost::filesystem::exists(outputFile), std::runtime_error);

  const int l = boost::lexical_cast<int>(argv[3]);

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

  cgi::Mosaic m;
  CGI_THROW_IF(cgi::NoError != m.open(argv[1]), std::runtime_error);

  CGI_THROW_IF(nBands != m.getBandCount(), std::runtime_error);

  m.setTileSize(cgi::core::Size2d< int >(1000,1000));

  const cgi::core::Size2d< int > &size = m.getTileSize();
  const int numRows = size.getHeight();
  const int numCols = size.getWidth();

  //const int everyNthTile = m.getColumnCount()/2 - 1;
  const int everyNthTile = 100;

  // initialization
  Eigen::Vector8f mean = Eigen::Vector8f::Zero();
  int validPixelCount = 0;

  log << cgi::log::Priority::INFO << "eigen_dmp" << "Calculating mean for..." << cgi::log::flush;

  // Calculate the mean vector across all tiles
  for(int i=0; i<m.getTileCount(); ++i)
    {
      if(0 != (i % everyNthTile))
	continue;

      log << cgi::log::Priority::INFO << "eigen_dmp" << "Tile " << i << " of " << m.getTileCount() << cgi::log::flush;

      const cgi::Tile<unsigned short> t(m.getTile<unsigned short>(i));
      const boost::numeric::ublas::matrix< bool > mask (t.getValidMask(cgi::valid_mask::ALL));

      for(int r=0; r<numRows; ++r)
	{
	  for(int c=0; c<numCols; ++c)
	    {
	      if(mask(r,c))
		{
		  const Eigen::Vector8f eVec(v2v(t(r,c)));
		  mean += eVec;
		  ++validPixelCount;
		}
	    }
	}

    }

  mean /= validPixelCount;
  std::cout << "mean = " << std::endl << mean << std::endl;

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

  //
  // Calculate the covariance matrix
  //

  Eigen::Matrix8f cov = Eigen::Matrix8f::Zero();

  log << cgi::log::Priority::INFO << "eigen_dmp" << "Calculating covariance for..." << cgi::log::flush;

  for(int i=0; i<m.getTileCount(); ++i)
    {
      if(0 != (i % everyNthTile))
	continue;

      log << cgi::log::Priority::INFO << "eigen_dmp" << "Tile " << i << " of " << m.getTileCount() << cgi::log::flush;

      const cgi::Tile<unsigned short> t(m.getTile<unsigned short>(i));
      const boost::numeric::ublas::matrix< bool > mask (t.getValidMask(cgi::valid_mask::ALL));

      for(int r=0; r<numRows; ++r)
	{
	  for(int c=0; c<numCols; ++c)
	    {
	      if(mask(r,c))
		{

		  const Eigen::Vector8f eVec(v2v(t(r,c)));
		  cov += (eVec - mean) * (eVec - mean).transpose();
		}
	    }
	}
    }

  cov /= validPixelCount;
  //cov -= mean * mean.transpose();
  std::cout << "cov = " << std::endl << cov << std::endl;

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

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix8f> eigensolver(cov);

  if (eigensolver.info() != Eigen::Success)
    abort();

  std::cout << "The eigenvalues of A are:" << std::endl << eigensolver.eigenvalues() << std::endl;
  std::cout << "Here's a matrix whose columns are eigenvectors of A " << std::endl
	    << "corresponding to these eigenvalues:" << std::endl
	    << eigensolver.eigenvectors() << std::endl;

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

  Eigen::Vector8f evals = eigensolver.eigenvalues();
  evals /= evals(evals.rows()-1, 0);
  std::cout << "Cummulative energy content for each eigenvector: " << std::endl
	    << evals << std::endl;

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

  //const Eigen::Vector8f z = eigensolver.eigenvectors().col(nBands-1);
  Eigen::MatrixXf zz(nBands,l);
  zz = eigensolver.eigenvectors().block(0,nBands-l, nBands,l);

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

  Eigen::VectorXf meanOfEigenImage = zz.transpose() * mean;
  std::cout << "meanOfEigneImage = " << std::endl
	    << meanOfEigenImage << std::endl;

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

  const cgi::Tile<unsigned short> t(m.getTile<unsigned short>(m.getTileIndex1D(m.getRowCount()/2, m.getColumnCount()/2)));
  cgi::Tile<float> tt(t.getSize(), l);

  for(int r=0; r<numRows; ++r)
    {
      for(int c=0; c<numCols; ++c)
        {
	  const Eigen::Vector8f eVec(v2v(t(r,c)));

	  //const float pp = z.transpose() * (eVec - mean);
	  const Eigen::VectorXf ppv (zz.transpose() * (eVec - mean));

	  for(int b=0; b < ppv.rows(); ++b)
	    tt[b](r,c) = ppv(b,0);
        }
    }

  cgi::MosaicUtility::writeImage<unsigned short>(t, "dmp.tif");
  cgi::MosaicUtility::writeImage<float>(tt, "out.tif");

  cgi::MosaicUtility::writeImage<unsigned char>(threshold(tt, 3*meanOfEigenImage), "thresh.tif");

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

  return 0;

  /*
  Eigen::Matrix8f A;

  //srand(time(NULL));

  const int n = nBands;
  for(int i=0; i<n; ++i)
    for(int j=i; j<n; ++j)
      A(i,j) = A(j,i) = (-1 + (2 * (rand()%2))) * (rand() / static_cast<float>(RAND_MAX));

  cout << "Here is the matrix A:\n" << A << endl;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix8f> eigensolver(A);
  if (eigensolver.info() != Eigen::Success) abort();
  cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
  cout << "Here's a matrix whose columns are eigenvectors of A \n"
       << "corresponding to these eigenvalues:\n"
       << eigensolver.eigenvectors() << endl;

  */
}


//#define BOOST_MATH_ASSERT_UNDEFINED_POLICY false
//#define BOOST_MATH_OVERFLOW_ERROR_POLICY errno_on_error

#include <boost/math/distributions/bernoulli.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/cauchy.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/extreme_value.hpp>
#include <boost/math/distributions/fisher_f.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/geometric.hpp>
#include <boost/math/distributions/laplace.hpp>
#include <boost/math/distributions/lognormal.hpp>
#include <boost/math/distributions/negative_binomial.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/triangular.hpp>
#include <boost/math/distributions/uniform.hpp>
#include <boost/math/distributions/weibull.hpp>

#include <boost/log/trivial.hpp>
#include <boost/log/attributes/scoped_attribute.hpp>

#include "GenericDistribution.h"

namespace Distribution
{
namespace Detail
{
    namespace bm = boost::math;

    struct pdf_visitor : boost::static_visitor<>
    {
        pdf_visitor(QVector<double> const& x, QVector<double>& y) :
            _x(x), _y(y) {}

        // Uniform Distributions
        void operator()(UniformInt const& d) const {
            auto m = bm::uniform_distribution<>(d.a(), d.b());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(UniformReal const& d) const {
            auto m = bm::uniform_distribution<>(d.a(), d.b());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Binomial const& d) const {
            auto m = bm::binomial_distribution<>(d.t(), d.p());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Geometric const& d) const {
            auto m = bm::geometric_distribution<>(d.p());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(NegativeBinomial const& d) const {
            auto m = bm::negative_binomial_distribution<>(d.k(), d.p());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        // Poisson Distributions
        void operator()(Poisson const& d) const {
            auto m = bm::poisson_distribution<>(d.mean());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Exponential const& d) const {
            auto m = bm::exponential_distribution<>(d.lambda());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Gamma const& d) const {
            auto m = bm::gamma_distribution<>(d.alpha(), d.beta());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Weibull const& d) const {
            auto m = bm::weibull_distribution<>(d.a(), d.b());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(ExtremeValue const& d) const {
            auto m = bm::extreme_value_distribution<>(d.a(), d.b());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Beta const& d) const {
            auto m = bm::beta_distribution<>(d.alpha(), d.beta());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Laplace const& d) const {
            auto m = bm::laplace_distribution<>(d.mean(), d.beta());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        // Normal Distributions
        void operator()(Normal const& d) const {
            auto m = bm::normal_distribution<>(d.mean(), d.sigma());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Lognormal const& d) const {
            auto m = bm::lognormal_distribution<>(d.m(), d.s());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(ChiSquared const& d) const {
            auto m = bm::chi_squared_distribution<>(d.n());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(NCChiSquared const& d) const {
            auto m = bm::non_central_chi_squared_distribution<>(d.k(), d.lambda());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Cauchy const& d) const {
            auto m = bm::cauchy_distribution<>(d.median(), d.sigma());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(FisherF const& d) const {
            auto m = bm::fisher_f_distribution<>(d.m(), d.n());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(StudentT const& d) const {
            auto m = bm::students_t_distribution<>(d.n());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        // Sampling Distributions
        void operator()(Discrete const& d) const {
            _y = QVector<double>::fromStdVector(d.probabilities());
        }
        void operator()(PiecewiseConstant const& d) const {
            _y = QVector<double>::fromStdVector(d.densities());
        }
        void operator()(PiecewiseLinear const& d) const {
            _y = QVector<double>::fromStdVector(d.densities());
        }
        // Miscellaneous Distributions
        void operator()(Triangle const& d) const {
            auto m = bm::triangular_distribution<>(d.a(), d.b(), d.c());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        // Placeholder
        void operator()(Constant const& d) const {
            for(const auto &x : _x) _y.push_back(d.a());
        }

    private:
        const QVector<double> &_x;
        QVector<double> &_y;
    };

} // namespace Distribution
} // namespace Detail

void GenericDistribution::pdf(QVector<double> const& x, QVector<double> &y) const
{
    BOOST_LOG_SCOPED_THREAD_TAG("Tag", "Distribution");

    y.reserve(x.size());
    try {
        boost::apply_visitor(Distribution::Detail::pdf_visitor(x, y), *this);
    } catch (const std::exception &e) {
        BOOST_LOG_TRIVIAL(warning) << e.what();
        return;
    }
}


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

/*! \file InvertMatrix.hpp

The following code inverts the matrix input using LU-decomposition with backsubstitution of unit vectors. Reference: Numerical Recipies in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery.

you can solve Ax=b using three lines of ublas code:

permutation_matrix<> piv;
lu_factorize(A, piv);
lu_substitute(A, piv, x);

*/
#ifndef INVERT_MATRIX_HPP
#define INVERT_MATRIX_HPP

// REMEMBER to update "lu.hpp" header includes from boost-CVS
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

/** Matrix inversion routine.

    Uses lu_factorize and lu_substitute in uBLAS to invert a matrix

    \param input source matrix
    \param output inverted matrix.

*/
template<class T, class U, class V>
bool InvertMatrix(const boost::numeric::ublas::matrix<T, U, V>& input,
                  boost::numeric::ublas::matrix<T, U, V>& inverse)
{
  typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
  // create a working copy of the input
  boost::numeric::ublas::matrix<T, U, V> A(input);
  // create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());

  // perform LU-factorization
  int res = lu_factorize(A,pm);
  if(res != 0) return false;

  // create identity matrix of "inverse"
  inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));

// backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);

  return true;
}
#endif //INVERT_MATRIX_HPP


//
// Copyright (c) 2016-2019 CNRS, INRIA
//

#ifndef __pinocchio_math_quaternion_hpp__
#define __pinocchio_math_quaternion_hpp__

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/comparison-operators.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/utils/static-if.hpp"
#include <boost/type_traits.hpp>

#include <Eigen/Geometry>

namespace pinocchio
{
  namespace quaternion
  {
    ///
    /// \brief Compute the minimal angle between q1 and q2.
    ///
    /// \param[in] q1 input quaternion.
    /// \param[in] q2 input quaternion.
    ///
    /// \return angle between the two quaternions
    ///
    template<typename D1, typename D2>
    typename D1::Scalar
    angleBetweenQuaternions(const Eigen::QuaternionBase<D1> & q1,
                            const Eigen::QuaternionBase<D2> & q2)
    {
      typedef typename D1::Scalar Scalar;
      const Scalar innerprod = q1.dot(q2);
      Scalar theta = math::acos(innerprod);
      static const Scalar PI_value = PI<Scalar>();

      theta = internal::if_then_else(internal::LT, innerprod, Scalar(0),
                                     PI_value - theta,
                                     theta);
      return theta;
    }

    ///
    /// \brief Check if two quaternions define the same rotations.
    /// \note Two quaternions define the same rotation iff q1 == q2 OR q1 == -q2.
    ///
    /// \param[in] q1 input quaternion.
    /// \param[in] q2 input quaternion.
    ///
    /// \return Return true if the two input quaternions define the same rotation.
    ///
    template<typename D1, typename D2>
    bool defineSameRotation(const Eigen::QuaternionBase<D1> & q1,
                            const Eigen::QuaternionBase<D2> & q2,
                            const typename D1::RealScalar & prec = Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
    {
      return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
    }

    /// Approximately normalize by applying the first order limited development
    /// of the normalization function.
    ///
    /// Only additions and multiplications are required. Neither square root nor
    /// division are used (except a division by 2). Let \f$ \delta = ||q||^2 - 1 \f$.
    /// Using the following limited development:
    /// \f[ \frac{1}{||q||} = (1 + \delta)^{-\frac{1}{2}} = 1 - \frac{\delta}{2} + \mathcal{O}(\delta^2) \f]
    ///
    /// The output is
    /// \f[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \f]
    ///
    /// The output quaternion is guaranted to statisfy the following:
    /// \f[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \f]
    /// where \f$ M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \f$
    /// and \f$ \epsilon \f$ is the maximum tolerance of \f$ ||q_{in}||^2 - 1 \f$.
    ///
    /// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero.
    ///
    /// \note See
    /// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3
    /// to know the reason why the argument is const.
    template<typename D>
    void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
    {
      typedef typename D::Scalar Scalar;
      const Scalar N2 = q.squaredNorm();
#ifndef NDEBUG
      const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
      typedef apply_op_if<less_than_or_equal_to_op,boost::is_floating_point<Scalar>::value,true> static_leq;
      assert(static_leq::op(math::fabs(N2-1.), epsilon));
#endif
      const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
      PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha;
#ifndef NDEBUG
      const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
      assert(static_leq::op(math::fabs(q.norm() - Scalar(1)),
                            math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision())));
#endif
    }

    /// Uniformly random quaternion sphere.
    template<typename Derived>
    void uniformRandom(const Eigen::QuaternionBase<Derived> & q)
    {
      typedef typename Derived::Scalar Scalar;

      // Rotational part
      const Scalar u1 = (Scalar)rand() / RAND_MAX;
      const Scalar u2 = (Scalar)rand() / RAND_MAX;
      const Scalar u3 = (Scalar)rand() / RAND_MAX;

      const Scalar mult1 = sqrt(Scalar(1)-u1);
      const Scalar mult2 = sqrt(u1);

      static const Scalar PI_value = PI<Scalar>();
      Scalar s2,c2; SINCOS(Scalar(2)*PI_value*u2,&s2,&c2);
      Scalar s3,c3; SINCOS(Scalar(2)*PI_value*u3,&s3,&c3);

      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).w() = mult1 * s2;
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).x() = mult1 * c2;
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).y() = mult2 * s3;
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).z() = mult2 * c3;
    }

    namespace internal
    {

      template<typename Scalar, bool value = boost::is_floating_point<Scalar>::value>
      struct quaternionbase_assign_impl;

      template<Eigen::DenseIndex i>
      struct quaternionbase_assign_impl_if_t_negative
      {
        template<typename Scalar, typename Matrix3, typename QuaternionDerived>
        static inline void run(Scalar t,
                               Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          Eigen::DenseIndex j = (i+1)%3;
          Eigen::DenseIndex k = (j+1)%3;

          t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
          q.coeffs().coeffRef(i) = Scalar(0.5) * t;
          t = Scalar(0.5)/t;
          q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
          q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
          q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
        }
      };

      struct quaternionbase_assign_impl_if_t_positive
      {
        template<typename Scalar, typename Matrix3, typename QuaternionDerived>
        static inline void run(Scalar t,
                               Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          t = sqrt(t + Scalar(1.0));
          q.w() = Scalar(0.5)*t;
          t = Scalar(0.5)/t;
          q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
          q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
          q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
        }
      };

      template<typename Scalar>
      struct quaternionbase_assign_impl<Scalar, true>
      {
        template<typename Matrix3, typename QuaternionDerived>
        static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          Scalar t = mat.trace();
          if (t > Scalar(0.))
            quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
          else
          {
            Eigen::DenseIndex i = 0;
            if (mat.coeff(1,1) > mat.coeff(0,0))
              i = 1;
            if (mat.coeff(2,2) > mat.coeff(i,i))
              i = 2;

            if(i==0)
              quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
            else if(i==1)
              quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
            else
              quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
          }
        }
      };

    } // namespace internal

    template<typename D, typename Matrix3>
    void assignQuaternion(Eigen::QuaternionBase<D> & quat,
                          const Eigen::MatrixBase<Matrix3> & R)
    {
      internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(PINOCCHIO_EIGEN_CONST_CAST(D,quat),
                                                                          R.derived());
    }

  } // namespace quaternion

}
#endif //#ifndef __pinocchio_math_quaternion_hpp__


// Copyright Maarten L. Hekkelman, Radboud University 2008-2011.
//   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)
//
// 3d routines

#include "mas.h"

#include <valarray>
#include <cmath>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include "primitives-3d.h"
#include "matrix.h"

using namespace std;

const double
	kPI = 4 * std::atan(1.0);

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

MQuaternion Normalize(MQuaternion q)
{
	valarray<double> t(4);

	t[0] = q.R_component_1();
	t[1] = q.R_component_2();
	t[2] = q.R_component_3();
	t[3] = q.R_component_4();

	t *= t;

	double length = sqrt(t.sum());

	if (length > 0.001)
		q /= length;
	else
		q = MQuaternion(1, 0, 0, 0);

	return q;
}

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

double MPoint::Normalize()
{
	double length = mX * mX + mY * mY + mZ * mZ;
	if (length > 0)
	{
		length = sqrt(length);
		mX /= length;
		mY /= length;
		mZ /= length;
	}
	return length;
}

MPoint operator+(const MPoint& lhs, const MPoint& rhs)
{
	return MPoint(lhs.mX + rhs.mX, lhs.mY + rhs.mY, lhs.mZ + rhs.mZ);
}

MPoint operator-(const MPoint& lhs, const MPoint& rhs)
{
	return MPoint(lhs.mX - rhs.mX, lhs.mY - rhs.mY, lhs.mZ - rhs.mZ);
}

MPoint operator-(const MPoint& pt)
{
	return MPoint(-pt.mX, -pt.mY, -pt.mZ);
}

MPoint operator*(const MPoint& pt, double f)
{
	MPoint result(pt);
	result *= f;
	return result;
}

MPoint operator/(const MPoint& pt, double f)
{
	MPoint result(pt);
	result /= f;
	return result;
}

ostream& operator<<(ostream& os, const MPoint& pt)
{
	os << '(' << pt.mX << ',' << pt.mY << ',' << pt.mZ << ')';
	return os;
}

ostream& operator<<(ostream& os, const vector<MPoint>& pts)
{
	uint32 n = pts.size();
	os << '[' << n << ']';

	foreach (const MPoint& pt, pts)
	{
		os << pt;
		if (n-- > 1)
			os << ',';
	}

	return os;
}

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

double DihedralAngle(const MPoint& p1, const MPoint& p2, const MPoint& p3, const MPoint& p4)
{
	MPoint v12 = p1 - p2;	// vector from p2 to p1
	MPoint v43 = p4 - p3;	// vector from p3 to p4

	MPoint z = p2 - p3;		// vector from p3 to p2

	MPoint p = CrossProduct(z, v12);
	MPoint x = CrossProduct(z, v43);
	MPoint y = CrossProduct(z, x);

	double u = DotProduct(x, x);
	double v = DotProduct(y, y);

	double result = 360;
	if (u > 0 and v > 0)
	{
		u = DotProduct(p, x) / sqrt(u);
		v = DotProduct(p, y) / sqrt(v);
		if (u != 0 or v != 0)
			result = atan2(v, u) * 180 / kPI;
	}

	return result;
}

double CosinusAngle(const MPoint& p1, const MPoint& p2, const MPoint& p3, const MPoint& p4)
{
	MPoint v12 = p1 - p2;
	MPoint v34 = p3 - p4;

	double result = 0;

	double x = DotProduct(v12, v12) * DotProduct(v34, v34);
	if (x > 0)
		result = DotProduct(v12, v34) / sqrt(x);

	return result;
}

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

tuple<double,MPoint> QuaternionToAngleAxis(MQuaternion q)
{
	if (q.R_component_1() > 1)
		q = Normalize(q);

	// angle:
	double angle = 2 * acos(q.R_component_1());
	angle = angle * 180 / kPI;

	// axis:
	double s = sqrt(1 - q.R_component_1() * q.R_component_1());
	if (s < 0.001)
		s = 1;

	MPoint axis(q.R_component_2() / s, q.R_component_3() / s, q.R_component_4() / s);

	return make_tuple(angle, axis);
}

MPoint CenterPoints(vector<MPoint>& points)
{
	MPoint t;

	foreach (MPoint& pt, points)
	{
		t.mX += pt.mX;
		t.mY += pt.mY;
		t.mZ += pt.mZ;
	}

	t.mX /= points.size();
	t.mY /= points.size();
	t.mZ /= points.size();

	foreach (MPoint& pt, points)
	{
		pt.mX -= t.mX;
		pt.mY -= t.mY;
		pt.mZ -= t.mZ;
	}

	return t;
}

MPoint Centroid(vector<MPoint>& points)
{
	MPoint result;

	foreach (MPoint& pt, points)
		result += pt;

	result /= points.size();

	return result;
}

double RMSd(const vector<MPoint>& a, const vector<MPoint>& b)
{
	double sum = 0;
	for (uint32 i = 0; i < a.size(); ++i)
	{
		valarray<double> d(3);

		d[0] = b[i].mX - a[i].mX;
		d[1] = b[i].mY - a[i].mY;
		d[2] = b[i].mZ - a[i].mZ;

		d *= d;

		sum += d.sum();
	}

	return sqrt(sum / a.size());
}

// The next function returns the largest solution for a quartic equation
// based on Ferrari's algorithm.
// A depressed quartic is of the form:
//
//   x^4 + ax^2 + bx + c = 0
//
// (since I'm too lazy to find out a better way, I've implemented the
//  routine using complex values to avoid nan's as a result of taking
//  sqrt of a negative number)
double LargestDepressedQuarticSolution(double a, double b, double c)
{
	complex<double> P = - (a * a) / 12 - c;
	complex<double> Q = - (a * a * a) / 108 + (a * c) / 3 - (b * b) / 8;
	complex<double> R = - Q / 2.0 + sqrt((Q * Q) / 4.0 + (P * P * P) / 27.0);

	complex<double> U = pow(R, 1 / 3.0);

	complex<double> y;
	if (U == 0.0)
		y = -5.0 * a / 6.0 + U - pow(Q, 1.0 / 3.0);
	else
		y = -5.0 * a / 6.0 + U - P / (3.0 * U);

	complex<double> W = sqrt(a + 2.0 * y);

	// And to get the final result:
	// result = (±W + sqrt(-(3 * alpha + 2 * y ± 2 * beta / W))) / 2;
	// We want the largest result, so:

	valarray<double> t(4);

	t[0] = (( W + sqrt(-(3.0 * a + 2.0 * y + 2.0 * b / W))) / 2.0).real();
	t[1] = (( W + sqrt(-(3.0 * a + 2.0 * y - 2.0 * b / W))) / 2.0).real();
	t[2] = ((-W + sqrt(-(3.0 * a + 2.0 * y + 2.0 * b / W))) / 2.0).real();
	t[3] = ((-W + sqrt(-(3.0 * a + 2.0 * y - 2.0 * b / W))) / 2.0).real();

	return t.max();
}

MQuaternion AlignPoints(const vector<MPoint>& pa, const vector<MPoint>& pb)
{
	// First calculate M, a 3x3 matrix containing the sums of products of the coordinates of A and B
	matrix<double> M(3, 3, 0);

	for (uint32 i = 0; i < pa.size(); ++i)
	{
		const MPoint& a = pa[i];
		const MPoint& b = pb[i];

		M(0, 0) += a.mX * b.mX;	M(0, 1) += a.mX * b.mY;	M(0, 2) += a.mX * b.mZ;
		M(1, 0) += a.mY * b.mX;	M(1, 1) += a.mY * b.mY;	M(1, 2) += a.mY * b.mZ;
		M(2, 0) += a.mZ * b.mX;	M(2, 1) += a.mZ * b.mY;	M(2, 2) += a.mZ * b.mZ;
	}

	// Now calculate N, a symmetric 4x4 matrix
	symmetric_matrix<double> N(4);

	N(0, 0) =  M(0, 0) + M(1, 1) + M(2, 2);
	N(0, 1) =  M(1, 2) - M(2, 1);
	N(0, 2) =  M(2, 0) - M(0, 2);
	N(0, 3) =  M(0, 1) - M(1, 0);

	N(1, 1) =  M(0, 0) - M(1, 1) - M(2, 2);
	N(1, 2) =  M(0, 1) + M(1, 0);
	N(1, 3) =  M(0, 2) + M(2, 0);

	N(2, 2) = -M(0, 0) + M(1, 1) - M(2, 2);
	N(2, 3) =  M(1, 2) + M(2, 1);

	N(3, 3) = -M(0, 0) - M(1, 1) + M(2, 2);

	// det(N - λI) = 0
	// find the largest λ (λm)
	//
	// Aλ4 + Bλ3 + Cλ2 + Dλ + E = 0
	// A = 1
	// B = 0
	// and so this is a so-called depressed quartic
	// solve it using Ferrari's algorithm

	double C = -2 * (
		M(0, 0) * M(0, 0) + M(0, 1) * M(0, 1) + M(0, 2) * M(0, 2) +
		M(1, 0) * M(1, 0) + M(1, 1) * M(1, 1) + M(1, 2) * M(1, 2) +
		M(2, 0) * M(2, 0) + M(2, 1) * M(2, 1) + M(2, 2) * M(2, 2));

	double D = 8 * (M(0, 0) * M(1, 2) * M(2, 1) +
					M(1, 1) * M(2, 0) * M(0, 2) +
					M(2, 2) * M(0, 1) * M(1, 0)) -
			   8 * (M(0, 0) * M(1, 1) * M(2, 2) +
					M(1, 2) * M(2, 0) * M(0, 1) +
					M(2, 1) * M(1, 0) * M(0, 2));

	double E =
		(N(0,0) * N(1,1) - N(0,1) * N(0,1)) * (N(2,2) * N(3,3) - N(2,3) * N(2,3)) +
		(N(0,1) * N(0,2) - N(0,0) * N(2,1)) * (N(2,1) * N(3,3) - N(2,3) * N(1,3)) +
		(N(0,0) * N(1,3) - N(0,1) * N(0,3)) * (N(2,1) * N(2,3) - N(2,2) * N(1,3)) +
		(N(0,1) * N(2,1) - N(1,1) * N(0,2)) * (N(0,2) * N(3,3) - N(2,3) * N(0,3)) +
		(N(1,1) * N(0,3) - N(0,1) * N(1,3)) * (N(0,2) * N(2,3) - N(2,2) * N(0,3)) +
		(N(0,2) * N(1,3) - N(2,1) * N(0,3)) * (N(0,2) * N(1,3) - N(2,1) * N(0,3));

	// solve quartic
	double lm = LargestDepressedQuarticSolution(C, D, E);

	// calculate t = (N - λI)
	matrix<double> li = identity_matrix<double>(4) * lm;
	matrix<double> t = N - li;

	// calculate a matrix of cofactors for t
	matrix<double> cf(4, 4);

	const uint32 ixs[4][3] =
	{
		{ 1, 2, 3 },
		{ 0, 2, 3 },
		{ 0, 1, 3 },
		{ 0, 1, 2 }
	};

	uint32 maxR = 0;
	for (uint32 r = 0; r < 4; ++r)
	{
		const uint32* ir = ixs[r];

		for (uint32 c = 0; c < 4; ++c)
		{
			const uint32* ic = ixs[c];

			cf(r, c) =
				t(ir[0], ic[0]) * t(ir[1], ic[1]) * t(ir[2], ic[2]) +
				t(ir[0], ic[1]) * t(ir[1], ic[2]) * t(ir[2], ic[0]) +
				t(ir[0], ic[2]) * t(ir[1], ic[0]) * t(ir[2], ic[1]) -
				t(ir[0], ic[2]) * t(ir[1], ic[1]) * t(ir[2], ic[0]) -
				t(ir[0], ic[1]) * t(ir[1], ic[0]) * t(ir[2], ic[2]) -
				t(ir[0], ic[0]) * t(ir[1], ic[2]) * t(ir[2], ic[1]);
		}

		if (r > maxR and cf(r, 0) > cf(maxR, 0))
			maxR = r;
	}

	// NOTE the negation of the y here, why? Maybe I swapped r/c above?
	MQuaternion q(cf(maxR, 0), cf(maxR, 1), -cf(maxR, 2), cf(maxR, 3));
	q = Normalize(q);

	return q;
}


#include "phase_space.hpp"
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <gsl/gsl_sf_bessel.h>

using MomentaType = std::array<phase_space::LVector<double>, 4>;

/// Struct holding whatever parameters are needed
struct ModelParameters {
  double mx;
  std::array<double, 4> fsp_masses;
};

/// Function computing squared matrix element
auto msqrd(const MomentaType &momenta, const ModelParameters &params)
    -> double {
  return 1.0;
}

/// Compute zero-temperature cross section
auto cross_section(double cme, const ModelParameters &params) -> double {
  auto msqrd_ = [&params](const MomentaType &momenta) {
    return msqrd(momenta, params);
  };
  return phase_space::cross_section(msqrd_, cme, params.mx, params.mx,
                                    params.fsp_masses)
      .first;
}

/// Compute the thermal cross section for X + X -> A + B + C + D
///
/// Usual integal is:
///     <σv> = (2 π² T) ∫ ds σ[√s] (s - 4 m²) √s K₁[√s/T] / (4π m² T K₂[m/T])
/// Define: s = z² m², x = m /T. Then this becomes:
///     <σv> = x ∫ dz z² (z² - 4) σ[m z] K₁[x * z] / (4 K₂[x]²)
/// Next, define scaled bessel functions: K₁[x] = exp(-x) k₁[x]
/// and K₂[x] = exp(-x) k₂[x]. Then:
///     <σv> = x / (4 k₂[x]²) ∫ dz z² (z² - 4) σ[m z] k₁[x * z] exp(-x (z-2))
double thermal_cross_section(const double x, const ModelParameters &params) {
  using boost::math::quadrature::gauss_kronrod;

  const double den = 2.0 * gsl_sf_bessel_Kn_scaled(2, x);
  const double pre = x / (den * den);
  const double mx = params.mx;

  auto f = [x, &params, mx](double z) -> double {
    const double z2 = z * z;
    const double sig = cross_section(z * mx, params);
    const double ker =
        z2 * (z2 - 4.0) * gsl_sf_bessel_K1_scaled(x * z) * exp(-x * (z - 2.0));
    return sig * ker;
  };

  const double integral = gauss_kronrod<double, 15>::integrate(
      f, 4.0, std::numeric_limits<double>::infinity(), 5, 1e-8);

  return pre * integral;
}

int main() {

  ModelParameters params = {100.0, {1.0, 1.0, 1.0, 1.0}};
  std::cout << thermal_cross_section(1.0, params);
  return 0.0;
}


#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include "../include/ply.h"

// contruct the stiffness matrix of the ply in the ply coordinates. P
Eigen::Matrix3d build_Q(const Properties& p);

double to_radian(double angle_degree);

using std::cin; using std::cout; using std::endl;
using Eigen::Matrix3d;

ply::ply(const std::string material_label,
    const Properties material_properties,
    const double theta, const double thickness):
    material_label_(material_label),
    material_properties_(material_properties), theta_(theta),
    thickness_(thickness) {

    double angle_radian = to_radian(theta);
    double s = sin(angle_radian);
    double c = cos(angle_radian);
    Matrix3d T_stress_inv;
    Matrix3d T_strain;

    T_stress_inv << pow(c,2), pow(s,2), -2*s*c,
                    pow(s,2), pow(c,2),  2*s*c,
                    s*c     , -s*c    ,  pow(c,2) - pow(s,2);

    T_strain << pow(c,2), pow(s,2),  s*c,
                pow(s,2), pow(c,2), -s*c,
                -2*s*c     , 2*s*c  ,  pow(c,2) - pow(s,2);

    Matrix3d Q = build_Q(material_properties_);

    // Transformation from ply coordinates to laminate coordinates.
    Qbar_ = T_stress_inv * Q * T_strain;

}

Matrix3d build_Q(const Properties& p) {
    double D = 1 - p.E2 / p.E1 * pow(p.nu12, 2);
    Matrix3d Q;

    Q << p.E1/D       , p.nu12*p.E2/D, 0,
         p.nu12*p.E2/D, p.E2/D       , 0,
         0            , 0            , p.G12;

    return Q;
}

double to_radian(double angle_degree) {
    return angle_degree * M_PI/180;
}


#include <HElib/FHE.h>
#include <HElib/FHEContext.h>
#include <HElib/EncryptedArray.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <iostream>
#include <fstream>
void generate_poly(std::vector<NTL::ZZX> &polys,
                   const EncryptedArray *ea,
                   const long upper) {
    polys.resize(ea->size());
    long d = ea->getDegree();
    for (size_t i = 0; i < polys.size(); i++) {
        polys[i].SetLength(d);
        for (long j = 0; j < d; j++)
            NTL::SetCoeff(polys[i], j, NTL::RandomBnd(upper));
        // std::cout << NTL::deg(polys[i]) << "=?" << d << std::endl;
    }
}

long inner_prod(NTL::ZZX const& v, NTL::ZZX const& u,
                const long d,
                const long upper) {
    NTL::ZZ ip(0);
    // std::cout << v << " " << u << std::endl;
    for (long i = 0; i < d; i++) {
        // std::cout << NTL::coeff(v, i)  << "*" << NTL::coeff(u, d - i - 1) << std::endl;
        ip = ip + NTL::coeff(v, i) * NTL::coeff(u, d - i - 1);
    }
    return ip % upper;
}

int main(int argc, char *argv[]) {
    // long m = 32;
    // long p = 113;
    long m = NTL::RandomPrime_long(5, 20);
    long p = NTL::RandomPrime_long(13, 20);
    do {
          p = NTL::RandomPrime_long(13, 20);
     } while ((m % p) == 0);
    FHEcontext context(m, p, 1);
    auto G = context.alMod.getFactorsOverZZ()[0];
    // EncryptedArray *ea = new EncryptedArray(context, G);
    auto ea = context.ea;
    std::cout << m << " " << p << std::endl;
    std::cout << ea->size() << " " << ea->getDegree() << std::endl;
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);

    std::vector<NTL::ZZX> slots, slots2;
    generate_poly(slots, ea, p);
    generate_poly(slots2, ea, p);
    for (size_t i = 0; i < slots.size(); i++) {
        std::cout << inner_prod(slots[i], slots2[i], ea->getDegree(), p) << std::endl;
    }
    std::cout << "\n";
    Ctxt ctx(sk), ctx2(sk);
    ea->skEncrypt(ctx, sk, slots);
    ea->skEncrypt(ctx2, sk, slots2);
    ctx *= ctx2;
    ea->decrypt(ctx, sk, slots);
    for (auto &ss : slots)
        std::cout << NTL::coeff(ss, ea->getDegree() - 1) << std::endl;
    return 0;
}


#include <iostream>
#include <random>
#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#define pi 3.14159

using namespace std;

static boost::random::mt19937 gen;

int gen_data(float bounds[8], int index, char thread);
float rand_gen(float lim[2]);

float rand_gen(float lim[2]){
  static boost::random::uniform_real_distribution<> dist(0, 1);
  return lim[0] + dist(gen)*(lim[1] - lim[0]);
}

int gen_data(float bounds[8], int index, char thread){
  string states_nm, parameters_nm, control_nm;
  bool flag = false;

  USING_NAMESPACE_ACADO

  DifferentialState        q1,q2,qd1,qd2;   // the differential states
  Control                  tau1,tau2;       // the control input u
  Parameter                T;
  DifferentialEquation     f( 0.0, 0.5 );

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

  OCP ocp( 0.0, 0.5,5 );
  ocp.minimizeLagrangeTerm(1+tau1*tau1 + tau2*tau2);
  //ocp.minimizeMayerTerm(q1*q1 + q2*q2 + qd1*qd1 + qd2*qd2 );

  f << dot(q1) == qd1;
  f << dot(q2) == qd2;
  f << dot(qd1) == -(48*tau1 - 48*tau2 + 24*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 18*qd1*qd1*sin(2*q2) - 72*tau2*cos(q2) + 48*qd1*qd2*sin(q2))/(36*cos(q2)*cos(q2) - 64);
  f << dot(qd2) == (48*tau1 - 240*tau2 + 120*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 36*qd1*qd1*sin(2*q2) + 18*qd2*qd2*sin(2*q2) + 72*tau1*cos(q2) - 144*tau2*cos(q2) + 48*qd1*qd2*sin(q2) + 36*qd1*qd2*sin(2*q2))/(18*cos(2*q2) - 46);

  ocp.subjectTo(f);
  ocp.subjectTo(AT_START, q1 ==  bounds[0] );

  ocp.subjectTo(AT_START, q2 ==  bounds[1] );

  ocp.subjectTo(AT_START, qd1 ==  bounds[2] );
  ocp.subjectTo(AT_START, qd2 ==  bounds[3] );

  ocp.subjectTo(AT_END, q1 == bounds[4]);
  ocp.subjectTo(AT_END, q2 == bounds[5]);
  ocp.subjectTo(AT_END, qd1 == bounds[6]);
  ocp.subjectTo(AT_END, qd2 == bounds[7]);

  // bounds on the control input u
  ocp.subjectTo(-400 <= tau1 <= 400);
  ocp.subjectTo(-400 <= tau2 <= 400);

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

  // Optimization algorithm
  OptimizationAlgorithm algorithm(ocp);
  algorithm.set( DISCRETIZATION_TYPE , SINGLE_SHOOTING);
  algorithm.set( INTEGRATOR_TYPE , INT_RK45);
  algorithm.set( HESSIAN_APPROXIMATION   , BLOCK_BFGS_UPDATE);
  algorithm.set( KKT_TOLERANCE   , 1e-1);
  algorithm.set( ABSOLUTE_TOLERANCE, 1e-1);
  algorithm.set( INTEGRATOR_TOLERANCE, 1e-1);
  algorithm.set( MAX_NUM_ITERATIONS, 1000);
  algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 10000);

  algorithm.set(PRINT_COPYRIGHT,BT_FALSE);
  algorithm.set(PRINTLEVEL,LOW);
  algorithm.set(PRINT_INTEGRATOR_PROFILE,BT_FALSE);
  algorithm.set(PRINT_SCP_METHOD_PROFILE,BT_FALSE);
  bool return_code = algorithm.solve();

  // VariablesGrid grid;
  // algorithm.getDifferentialStates(grid);
  // DVector final_state(4), diff(4);
  // final_state = grid.getLastVector();

  // diff(0) = final_state[0] - bounds[4];
  // diff(1) = final_state[1] - bounds[5];
  // diff(2) = final_state[2] - bounds[6];
  // diff(3) = final_state[3] - bounds[7];

  // if (diff.getNorm(VN_L2) < 0.001){

  if (return_code == 0){
    flag = true;
    // cout << thread;
    states_nm = "states_" + to_string(index) + "_" + thread + ".txt";
    parameters_nm = "parameters_" + to_string(index) + "_" + thread + ".txt";
    control_nm = "control_" + to_string(index) + "_" + thread + ".txt";
    algorithm.getDifferentialStates(states_nm.c_str());
    algorithm.getObjectiveValue(parameters_nm.c_str());
    algorithm.getControls(control_nm.c_str());
  }

  clearAllStaticCounters();
  return flag;
}

int main(int argc, char const *argv[])
{
  float q_lims[2] = {0.0,2*pi};
  float qd_lims[2] = {-30.0,30.0};

  if(argc != 3){
    cout << "Incorrect number of arguments." << endl;
  }
  else{
    int num_iter = atoi(argv[1]);

    gen.seed(time(0) + int(*argv[2]));
    int i = 0;
    while(i < num_iter){
      float bounds[8] = {rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims),rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims)};
      cout << i << endl;
      cout << bounds[0] << " " << bounds[1] << " " << bounds[2] << " " << bounds[3] << " " << bounds[4] << " " << bounds[5] << " " << bounds[6] << " " << bounds[7] << endl;
      bool success = gen_data(bounds, i, *argv[2]);
      if(success){
        i++;
      }
    }
  }
  return 0;
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example qr.cpp
*
*   This tutorial shows how the QR factorization of matrices from ViennaCL or Boost.uBLAS can be computed.
*
**/

// Activate ublas support in ViennaCL
#define VIENNACL_WITH_UBLAS

//
// Include necessary system headers
//
#include <iostream>

//
// ViennaCL includes
//
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/qr.hpp"

//
// Boost includes
//
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

/**
* A helper function comparing two matrices and returning the maximum entry-wise relative error encountered.
**/
template<typename MatrixType>
double check(MatrixType const & qr, MatrixType const & ref)
{
  bool do_break = false;
  double max_error = 0;
  for (std::size_t i=0; i<ref.size1(); ++i)
  {
    for (std::size_t j=0; j<ref.size2(); ++j)
    {
      if (qr(i,j) != 0.0 && ref(i,j) != 0.0)
      {
        double rel_err = fabs(qr(i,j) - ref(i,j)) / fabs(ref(i,j) );

        if (rel_err > max_error)
          max_error = rel_err;
      }

      /* Uncomment the following if you also want to check for NaNs.
      if (qr(i,j) != qr(i,j))
      {
        std::cout << "!!!" << std::endl;
        std::cout << "!!! NaN detected at i=" << i << " and j=" << j << std::endl;
        std::cout << "!!!" << std::endl;
        do_break = true;
        break;
      }*/
    }
    if (do_break)
      break;
  }
  return max_error;
}

/**
*  We set up a random matrix using Boost.uBLAS and use it to initialize a ViennaCL matrix.
*  Then we compute the QR factorization directly for the uBLAS matrix as well as the ViennaCL matrix.
**/
int main (int, const char **)
{
  typedef double               ScalarType;     //feel free to change this to 'double' if supported by your hardware
  typedef boost::numeric::ublas::matrix<ScalarType>              MatrixType;
  typedef viennacl::matrix<ScalarType, viennacl::column_major>   VCLMatrixType;

  std::size_t rows = 113;   // number of rows in the matrix
  std::size_t cols = 54;    // number of columns

  /**
  * Create uBLAS matrices with some random input data.
  **/
  MatrixType ublas_A(rows, cols);
  MatrixType Q(rows, rows);
  MatrixType R(rows, cols);

  // Some random data with a bit of extra weight on the diagonal
  for (std::size_t i=0; i<rows; ++i)
  {
    for (std::size_t j=0; j<cols; ++j)
    {
      ublas_A(i,j) = ScalarType(-1.0) + ScalarType((i+1)*(j+1))
                     + ScalarType( (rand() % 1000) - 500.0) / ScalarType(1000.0);

      if (i == j)
        ublas_A(i,j) += ScalarType(10.0);

      R(i,j) = 0.0;
    }

    for (std::size_t j=0; j<rows; ++j)
      Q(i,j) = ScalarType(0.0);
  }

  // keep initial input matrix for comparison
  MatrixType ublas_A_backup(ublas_A);

  /**
  *   Setup the matrix in ViennaCL and copy the data from the uBLAS matrix:
  **/
  VCLMatrixType vcl_A(ublas_A.size1(), ublas_A.size2());

  viennacl::copy(ublas_A, vcl_A);

  /**
  *
QR Factorization with Boost.uBLAS Matrices

  * Compute QR factorization of A. A is overwritten with Householder vectors. Coefficients are returned and a block size of 3 is used.
  * Note that at the moment the number of columns of A must be divisible by the block size
  **/

  std::cout << "--- Boost.uBLAS ---" << std::endl;
  std::vector<ScalarType> ublas_betas = viennacl::linalg::inplace_qr(ublas_A);  //computes the QR factorization

  /**
  *  Let us check for the correct result:
  **/
  viennacl::linalg::recoverQ(ublas_A, ublas_betas, Q, R);
  MatrixType ublas_QR = prod(Q, R);
  double ublas_error = check(ublas_QR, ublas_A_backup);
  std::cout << "Maximum relative error (ublas): " << ublas_error << std::endl;

  /**
  *
QR Factorization with Boost.uBLAS Matrices

  *  We now compute the QR factorization from a ViennaCL matrix. Internally it uses Boost.uBLAS for the panel factorization.
  **/
  std::cout << "--- Hybrid (default) ---" << std::endl;
  viennacl::copy(ublas_A_backup, vcl_A);
  std::vector<ScalarType> hybrid_betas = viennacl::linalg::inplace_qr(vcl_A);

  /**
  *  Let us check for the correct result:
  **/
  viennacl::copy(vcl_A, ublas_A);
  Q.clear(); R.clear();
  viennacl::linalg::recoverQ(ublas_A, hybrid_betas, Q, R);
  double hybrid_error = check(ublas_QR, ublas_A_backup);
  std::cout << "Maximum relative error (hybrid): " << hybrid_error << std::endl;

  /**
  *  That's it. Print a success message and exit.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


// To run this script, `cd` to the `./test/fixtures` directory and then execute in the terminal `runWandbox --file --compiler gcc-head --output output.json ./runner.cpp`.

#include <random>
#include <algorithm>
#include <iterator>
#include <vector>
#include <iostream>
#include <boost/math/special_functions/gamma.hpp>

using namespace std;

vector<double> linspace( double start, double end, int num ) {
	double delta = (end - start) / (num - 1);
	vector<double> arr( num - 1 );
	for ( int i = 0; i < num - 1; ++i ){
		arr[ i ] = start + delta * i;
	}
	arr.push_back( end );
	return arr;
}

void print_vector( vector<double> vec, bool last = false ) {
	cout << "[";
	for ( vector<double>::iterator it = vec.begin(); it != vec.end(); ++it ) {
		if ( vec.end() != it+1 ) {
			cout << setprecision (16) << *it;
			cout << ",";
		} else {
			cout << setprecision (16) << *it;
			cout << "]";
			if ( last == false ) {
				cout << ",";
			}
		}
	}
	return;
}

void print_results(
	vector<double> x,
	vector<double> s,
	vector<double> lower_regularized,
	vector<double> upper_regularized,
	vector<double> lower_unregularized,
	vector<double> upper_unregularized
) {
	cout << "{" << endl;
	cout << "  \"x\": ";
	print_vector( x );
	cout << "  \"s\": ";
	print_vector( s );
	cout << "  \"lower_regularized\": ";
	print_vector( lower_regularized );
	cout << "  \"upper_regularized\": ";
	print_vector( upper_regularized );
	cout << "  \"lower_unregularized\": ";
	print_vector( lower_unregularized );
	cout << "  \"upper_unregularized\": ";
	print_vector( upper_unregularized, true );
	cout << "}" << endl;
	return;
}

int main() {
	random_device rd;
	mt19937 g(rd());

	vector<double> x = linspace( 1.0, 40.0, 100 );
	shuffle( x.begin(), x.end(), g );
	vector<double> s = linspace( 1.0, 40.0, 100 );
	shuffle( s.begin(), s.end(), g );
	vector<double> lower_regularized;
	vector<double> upper_regularized;
	vector<double> lower_unregularized;
	vector<double> upper_unregularized;

	for ( int i = 0; i < 100; i++  ) {
		double arg1 = s[ i ];
		double arg2 = x[ i ];
		lower_regularized.push_back( boost::math::gamma_p( arg1, arg2 ) );
		upper_regularized.push_back( boost::math::gamma_q( arg1, arg2 ) );
		lower_unregularized.push_back( boost::math::tgamma_lower( arg1, arg2 ) );
		upper_unregularized.push_back( boost::math::tgamma( arg1, arg2 ) );
	}

	print_results( x, s, lower_regularized, upper_regularized, lower_unregularized, upper_unregularized );
	return 0;
}


/**
 * @file
 * @brief We build a tensor product mesh on the unit square and use pointwise
 * refinement to refine it.
 * @author Anian Ruoss
 * @date   2018-10-06 16:37:17
 * @copyright MIT License
 */

#include <boost/program_options.hpp>
#include <functional>
#include <iostream>
#include <vector>

#include <lf/refinement/mesh_hierarchy.h>
#include <lf/refinement/refutils.h>
#include "lf/io/io.h"
#include "lf/mesh/utils/utils.h"

using CodimMeshDataSet_t =
    std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<bool>>;

bool PointInTriangle(const Eigen::MatrixXd &tria_coords,
                     const Eigen::Vector2d &point) {
  // calculate barycentric coordinates of point using affine transformation
  // from reference triangle: phi(x_hat) = alpha + beta * x_hat
  Eigen::Vector2d alpha = tria_coords.col(0);
  Eigen::Matrix2d beta;
  beta << tria_coords.col(1) - tria_coords.col(0),
      tria_coords.col(2) - tria_coords.col(0);

  Eigen::Vector2d loc_coords = beta.inverse() * (point - alpha);

  return (0 <= loc_coords(0) && 0 <= loc_coords(1) && loc_coords.sum() <= 1);
}

CodimMeshDataSet_t MarkMesh(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh_ptr,
    const Eigen::MatrixXd &point) {
  // we use CodimMeshDataSet to store whether an edge has to be marked or not
  CodimMeshDataSet_t marked =
      lf::mesh::utils::make_CodimMeshDataSet<bool>(mesh_ptr, 1, false);

  // loop through all cells to check if it contains the point
  for (const lf::mesh::Entity &cell : mesh_ptr->Entities(0)) {
    const lf::geometry::Geometry *geom_ptr = cell.Geometry();
    lf::base::RefEl ref_el = cell.RefEl();
    const Eigen::MatrixXd &ref_el_coords(ref_el.NodeCoords());
    const Eigen::MatrixXd vtx_coords(geom_ptr->Global(ref_el_coords));

    // mark all edges if the point lies inside the cell
    if (ref_el == lf::base::RefEl::kTria()) {
      if (PointInTriangle(vtx_coords, point)) {
        for (const lf::mesh::Entity &edge : cell.SubEntities(1)) {
          marked->operator()(edge) = true;
        }
      }
    } else if (ref_el == lf::base::RefEl::kQuad()) {
      // split quadrilateral into two triangles and check each separately
      Eigen::MatrixXd tria1 = vtx_coords.block(0, 0, 2, 3);
      Eigen::MatrixXd tria2(2, 3);
      tria2 << vtx_coords.col(0), vtx_coords.col(2), vtx_coords.col(3);

      if (PointInTriangle(tria1, point) || PointInTriangle(tria2, point)) {
        for (const lf::mesh::Entity &edge : cell.SubEntities(1)) {
          marked->operator()(edge) = true;
        }
      }
    } else {
      std::cerr << "unknown cell geometry" << std::endl;
    }
  }

  return marked;
}

int main(int argc, char **argv) {
  // define allowed command line arguments:
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  desc.add_options()("help", "Produce this help message")(
      "num_steps", po::value<size_t>()->default_value(5),
      "Number of refinement steps")(
      "pointwise", po::value<bool>()->default_value(true),
      "Whether to use pointwise refinement or not")(
      "point",
      po::value<std::vector<double>>()->multitoken()->default_value(
          std::vector<double>{.5, .5}, ".5, .5"),
      "Point coordinates in unit square (ignored if pointwise=false)");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help") != 0u) {
    std::cout << desc << std::endl;
    return 1;
  }

  size_t num_steps = vm["num_steps"].as<size_t>();
  bool pointwise = vm["pointwise"].as<bool>();
  std::vector<double> point_coords = vm["point"].as<std::vector<double>>();
  Eigen::Vector2d point(point_coords.data());

  using size_type = lf::base::size_type;
  using lf::mesh::utils::TikzOutputCtrl;

  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // build single-cell tensor product mesh on unit square
  lf::mesh::hybrid2d::TPQuadMeshBuilder builder(mesh_factory_ptr);
  builder.setBottomLeftCorner(Eigen::Vector2d{0, 0});
  builder.setTopRightCorner(Eigen::Vector2d{1, 1});
  builder.setNoXCells(1);
  builder.setNoYCells(1);
  std::shared_ptr<lf::mesh::Mesh> mesh_ptr = builder.Build();

  // output mesh information
  const lf::mesh::Mesh &mesh = *mesh_ptr;
  lf::mesh::utils::PrintInfo(mesh, std::cout);
  std::cout << std::endl;

  // build mesh hierarchy
  lf::refinement::MeshHierarchy multi_mesh(mesh_ptr, mesh_factory_ptr);

  // mark edges of cells containing point
  auto marker = [](const lf::mesh::Mesh &mesh, const lf::mesh::Entity &edge,
                   CodimMeshDataSet_t mesh_data) -> bool {
    return mesh_data->operator()(edge);
  };

  for (int step = 0; step < num_steps; ++step) {
    // obtain pointer to mesh on finest level
    const size_type n_levels = multi_mesh.NumLevels();
    std::shared_ptr<const lf::mesh::Mesh> mesh_fine =
        multi_mesh.getMesh(n_levels - 1);

    // print number of entities of various co-dimensions
    std::cout << "Mesh on level " << n_levels - 1 << ": " << mesh_fine->Size(2)
              << " nodes, " << mesh_fine->Size(1) << " edges, "
              << mesh_fine->Size(0) << " cells," << std::endl;

    lf::mesh::utils::writeTikZ(
        *mesh_fine,
        std::string("refinement_mesh") + std::to_string(step) + ".txt",
        TikzOutputCtrl::RenderCells | TikzOutputCtrl::CellNumbering |
            TikzOutputCtrl::VerticeNumbering | TikzOutputCtrl::NodeNumbering |
            TikzOutputCtrl::EdgeNumbering);

    lf::io::writeMatplotlib(*mesh_fine, std::string("refinement_mesh") +
                                            std::to_string(step) + ".csv");

    if (pointwise) {
      CodimMeshDataSet_t marked_mesh = MarkMesh(mesh_fine, point);
      multi_mesh.MarkEdges(std::bind(marker, std::placeholders::_1,
                                     std::placeholders::_2, marked_mesh));
      multi_mesh.RefineMarked();
    } else {
      multi_mesh.RefineRegular();
    }
  }

  // generate MATLAB functions describing all levels of mesh hierarchy
  lf::refinement::WriteMatlab(multi_mesh, "pointwise_refinement");

  return 0;
}