#include <cmath>
#include <functional>
#include <iostream>
#include <vector>
#include <catch2/catch.hpp>
#include <Eigen/Dense>
#include "nelder_mead.h"

TEST_CASE("Test Nelder-Mead Optimization", "[Helpers][Optimization]") {

  SECTION("Test ability to optimize Rosenbrock function") {
    optimization::NelderMead optimizer(1e-6);

    std::function<double(const std::vector<double>&)> rosenbrock =
        [](const std::vector<double> &points) -> double {
      double a = 1.0, b = 100.0;
      return std::pow(a - points[0], 2) +
             b * std::pow(points[1] - points[0] * points[0], 2);
    };

    std::vector<double> initial_point = {-10.0, -1.0};
    double delta = 0.1;

    auto calced_min_location =
        optimizer.minimize(initial_point, delta, rosenbrock);

    REQUIRE(optimizer.get_minimum() + 1.0 == Approx(1.0).epsilon(0.01));
    REQUIRE(calced_min_location[0] == Approx(1.0).epsilon(0.01));
    REQUIRE(calced_min_location[1] == Approx(1.0).epsilon(0.01));
  }

  SECTION("Test ability to optimize Rosenbrock function") {
    optimization::NelderMead optimizer(1e-6);

    std::function<double(const std::vector<double>&)> rosenbrock =
        [](const std::vector<double> &points) -> double {
      double a = 1.0, b = 100.0;
      return std::pow(a - points[0], 2) +
             b * std::pow(points[1] - points[0] * points[0], 2);
    };

    std::vector<double> initial_point = {1000.0, -2000.0};
    std::vector<double> delta = {0.1, 0.2};

    auto calced_min_location =
        optimizer.minimize(initial_point, delta, rosenbrock);

    REQUIRE(optimizer.get_minimum() + 1.0 == Approx(1.0).epsilon(0.01));
    REQUIRE(calced_min_location[0] == Approx(1.0).epsilon(0.01));
    REQUIRE(calced_min_location[1] == Approx(1.0).epsilon(0.01));
  }
}


#include "QuinticPolynomial.h"

#include <Eigen/LU>
#include <cmath>

using namespace Eigen;

QuinticPolynomial::QuinticPolynomial(double xs, double vxs, double axs,
        double xe, double vxe, double axe, double t):
        a0(xs), a1(vxs) {
    a2 = axs / 2.0;
    Matrix3d A;
    Vector3d B;
    A << pow(t, 3), pow(t, 4), pow(t, 5), 3 * pow(t, 2),
    4 * pow(t, 3), 5 * pow(t, 4), 6 * t, 12 * pow(t, 2),
    20 * pow(t, 3);
    B << xe - a0 - a1 * t - a2 * pow(t, 2), vxe - a1 - 2 * a2 * t,
    axe - 2 * a2;
    Matrix3d A_inv = A.inverse();
    Vector3d x = A_inv * B;
    a3 = x[0];
    a4 = x[1];
    a5 = x[2];
}

double QuinticPolynomial::calc_point(double t) {
    return a0 + a1 * t + a2 * pow(t, 2) + a3 * pow(t, 3) +
    a4 * pow(t, 4) + a5 * pow(t, 5);
}

double QuinticPolynomial::calc_first_derivative(double t) {
    return a1 + 2 * a2 * t + 3 * a3 * pow(t, 2) + 4 * a4 * pow(t, 3) +
    5 * a5 * pow(t, 4);
}

double QuinticPolynomial::calc_second_derivative(double t) {
    return 2 * a2 + 6 * a3 * t + 12 * a4 * pow(t, 2) + 20 * a5 * pow(t, 3);
}

double QuinticPolynomial::calc_third_derivative(double t) {
    return 6 * a3 + 24 * a4 * t + 60 * a5 * pow(t, 2);
}


/*
 DepSpawn: Data Dependent Spawn library
 Copyright (C) 2012-2021 Carlos H. Gonzalez, Basilio B. Fraguela. Universidade da Coruna

 Distributed under the MIT License. (See accompanying file LICENSE)
*/

///
/// \author   Carlos H. Gonzalez  <cgonzalezv@udc.es>
/// \author   Basilio B. Fraguela <basilio.fraguela@udc.es>
///

#include <cstdlib>
#include <iostream>
#include <chrono>
#include <blitz/array.h>
#include "depspawn/depspawn.h"
#include "common_io.cpp"  // This is only for serializing parallel prints

using namespace depspawn;
using namespace blitz;

typedef float Type;

#define N 1000

int CHUNKS;

Array<Type , 2> result(N,N), a(N, N), b(N,N);

//template<typename Type> //IT DOES NOT WORK WITH A TEMPLATED FUNCTION; AND IT SHOULD!
void mxm(Array<Type , 2>& result, const Array<Type , 2>& a, const Array<Type , 2>& b)
{
  const int nrows = result.rows();
  const int ncols = result.cols();
  const int kdim = a.cols();

  for(int i = 0; i < nrows; i++) {
    for(int j = 0; j < ncols; j++) {
      Type f = (Type)0;
      for(int k = 0; k < kdim; k++)
	f += a(i, k) * b(k, j);
      result(i, j) = f;
    }
  }
}

bool test()
{
  const Type *ap = a.data();
  const Type *bp = b.data();
  const Type *rp = result.data();

  for (int i = 0; i < N; i++) {
    for(int j = 0; j < N; j++) {
      Type f = (Type)0;
      for(int k = 0; k < N; k++)
	f += ap[i * N + k] * bp[k * N + j];
      if (f != rp[i * N + j])
	return false;
    }
  }

  return true;
}

int main(int argc, char **argv)
{ int i, j;

  CHUNKS = (argc == 1) ? 4 : atoi(argv[1]);

  for (i = 0; i < N; i++) {
    for( j = 0; j < N; j++) {
      result(i, j) = (Type)0;
      a(i, j) = i + j;
      b(i, j) = (i > j) ? i - j : j - i;
    }
  }

  std::chrono::time_point<std::chrono::high_resolution_clock> t0 = std::chrono::high_resolution_clock::now();

  mxm(result, a, b);

  std::chrono::time_point<std::chrono::high_resolution_clock> t1 = std::chrono::high_resolution_clock::now();

  //matrix - vector product using CHUNKS chunks
  for(i = 0; i < N; i += N / CHUNKS) {
    int limi = (i + N / CHUNKS) >= N ? N : (i + N / CHUNKS);
    Range rows(i, limi - 1);
    for(j = 0; j < N; j += N / CHUNKS) {
      int limj = (j + N / CHUNKS) >= N ? N : (j + N / CHUNKS);
      Range cols(j, limj - 1);
      spawn(mxm, result(rows, cols), a(rows, Range::all()), b(Range::all(), cols));
    }
  }

  wait_for_all();

  std::chrono::time_point<std::chrono::high_resolution_clock> t2 = std::chrono::high_resolution_clock::now();

  double serial_time = std::chrono::duration<double>(t1-t0).count();
  double parallel_time = std::chrono::duration<double>(t2-t1).count();

  std::cout << "Serial time: " << serial_time << "s.  Parallel time: " << parallel_time << "s.\n";
  std::cout << "Speedup (using " << CHUNKS << " X " << CHUNKS << " chunks): " <<  (serial_time / parallel_time) << std::endl;

  const bool test_ok = test();

  std::cout << "TEST " << (test_ok ? "SUCCESSFUL" : "UNSUCCESSFUL") << std::endl;

  return !test_ok;
}


/*
 *  Distributed under our modified Boost Software License.
 *  Version 1.0 (see accompanying file LICENSE)
 */
/* This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at https://mozilla.org/MPL/2.0/. */
/**
 *  @file       Robot.cpp
 *  @author     Lydia Zoghbi, Ari Kupferberg, Ryan Bates
 *  @copyright  Copyright ARL 2019
 *  @date       10/19/2019
 *  @version    1.0
 *
 *  @brief      Definitions for Robot.hpp
 *
 */
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/SVD>
#include <math.h>
#include <cmath>
#include <vector>
#include <iostream>
#include "Robot.hpp"
#include <RobotPosition.hpp>
#include <RobotPath.hpp>
#include <Point.hpp>
#include <boost/numeric/ublas/matrix.hpp>

Robot::Robot(const Point& startingPos): initialEEPosition(startingPos) {
}

boost::numeric::ublas::matrix<double> Robot::computeATransform(std::vector<double> dhRow) {

  // Initializing parameters
  double a = dhRow[0];
  double alpha = dhRow[1];
  double d = dhRow[2];
  double theta = dhRow[3];
  boost::numeric::ublas::matrix<double> aTransform(4, 4);

  // Initialize 'A' matrix variables
  std::vector<double> aTerms = {cos(theta), -sin(theta)*cos(alpha),
     sin(theta)*sin(alpha), a*cos(theta), sin(theta), cos(theta)*cos(alpha),
     -cos(theta)*sin(alpha), a*sin(theta), 0, sin(alpha), cos(alpha), d, 0,
     0, 0, 1};

  int i = 0;
  int j = 0;

  // Populate the A transformation matrix
  for (auto& term: aTerms) {
    aTransform(i, j) = term;
    j++;
    if (j == 4) {
      i++;
      j = 0;
    }
  }
  return aTransform;
}

std::vector<Point> Robot::computeFk(std::vector<double> jointAngles) {
  int i = 0;
  for (auto& element : jointAngles) {
    dhParams[i][3] = element + dhParams[i][3];
    i++;
  }

  // Initialize Matrices
  boost::numeric::ublas::matrix<double> aTransform(4, 4);
  boost::numeric::ublas::matrix<double> tTransform(4, 4);
  boost::numeric::ublas::matrix<double> previousTransform(4, 4);
  boost::numeric::ublas::identity_matrix<double> identity(4, 4);
  previousTransform = identity;

  // Loop through A matrices to get T matrices
  std::vector<boost::numeric::ublas::matrix<double>> tTransforms;
  for (auto& row : dhParams) {
    aTransform = computeATransform(row);
      tTransform = prod(previousTransform, aTransform);
      previousTransform = tTransform;
      tTransforms.push_back(tTransform);
  }

  // Pull points from T matrices
  Point initialPoint(0.0, 0.0, 0.0);
  std::vector<Point> points = {initialPoint};
  for (auto& p : tTransforms) {
     Point point(p(0, 3), p(1, 3), p(2, 3));
     points.push_back(point);
  }
  return points;
}

boost::numeric::ublas::vector<double> Robot::crossProduct(boost::numeric::ublas::vector<double> vector1, boost::numeric::ublas::vector<double> vector2) {

  // Cross product function because there was no easy way to get it done through Boost
  boost::numeric::ublas::vector<double> resultVector(3);
  resultVector(0) = vector1(1)*vector2(2)-vector1(2)*vector2(1);
  resultVector(1) = vector1(2)*vector2(0)-vector1(0)*vector2(2);
  resultVector(2) = vector1(0)*vector2(1)-vector1(1)*vector2(0);
  return resultVector;
}

std::vector<boost::numeric::ublas::matrix<double>> Robot::computeTransformationMatrices(std::vector<double> jointAngles) {

 int i = 0;
  for (auto& element : jointAngles) {
    dhParams[i][3] = element + dhParams[i][3];
    i++;
  }

  // Initialize Matrices
  boost::numeric::ublas::matrix<double> aTransform(4, 4);
  boost::numeric::ublas::matrix<double> tTransform(4, 4);
  boost::numeric::ublas::matrix<double> previousTransform(4, 4);
  boost::numeric::ublas::identity_matrix<double> identity(4, 4);
  previousTransform = identity;

  // Loop through A matrices to get T matrices
  std::vector<boost::numeric::ublas::matrix<double>> tTransforms;
  tTransforms.push_back(identity);
  for (auto& row : dhParams) {
    aTransform = computeATransform(row);
    tTransform = prod(previousTransform, aTransform);
    previousTransform = tTransform;
    tTransforms.push_back(tTransform);
  }
 return tTransforms;
}

boost::numeric::ublas::matrix<double> Robot::computeJacobian(RobotPosition robotPosition, std::vector<boost::numeric::ublas::matrix<double>> tTransforms) {

  // Get all the joint positions in Point
  std::vector<Point> joints = robotPosition.getJoints();
  boost::numeric::ublas::vector<double> zAxes(3);
  zAxes(0) = 0;
  zAxes(1) = 0;
  zAxes(2) = 1;

  // Initialize some parameteres
  boost::numeric::ublas::matrix<double> jacobian(6,6);
  std::vector<double> iterator = {0, 1, 2};
  boost::numeric::ublas::matrix<double> trans(3,3);

  int index = 0;
  Point robotEEPosition = joints[6];
  joints.pop_back();

  // Loop through the joints to compute the Jacobian
  for (auto& joint : joints) {
    Point robotJointPosition = joint;
    double differenceInX = robotEEPosition.getX() - robotJointPosition.getX();
    double differenceInY = robotEEPosition.getY() - robotJointPosition.getY();
    double differenceInZ = robotEEPosition.getZ() - robotJointPosition.getZ();
    boost::numeric::ublas::vector<double> differenceVector(3);
    differenceVector(0) = differenceInX;
    differenceVector(1) = differenceInY;
    differenceVector(2) = differenceInZ;

    trans(0,0) = tTransforms[index](0,0); trans(0,1) = tTransforms[index](0,1);
    trans(0,2) = tTransforms[index](0,2); trans(1,0) = tTransforms[index](1,0);
    trans(1,1) = tTransforms[index](1,1); trans(1,2) = tTransforms[index](1,2);
    trans(2,0) = tTransforms[index](2,0); trans(2,1) = tTransforms[index](2,1);
    trans(2,2) = tTransforms[index](2,2);
    boost::numeric::ublas::vector<double> transformedZAxes = prod(trans,zAxes);
    boost::numeric::ublas::vector<double> crossProductVector = crossProduct(transformedZAxes, differenceVector);

    // Populate the Jacobian's columns iteratively
    for (auto& element : iterator) {
      jacobian(element, index) = crossProductVector(element);
      jacobian(element+3, index) = transformedZAxes(element);
    }
    index++;
  }
  return jacobian;
}

// https://gist.github.com/javidcf/25066cf85e71105d57b6 used as reference
boost::numeric::ublas::matrix<double> Robot::penroseInverseMatrix(boost::numeric::ublas::matrix<double> mat) {
  boost::numeric::ublas::matrix<double> result(mat.size1(), mat.size2());

  Eigen::MatrixXd convertedInput(mat.size1(), mat.size2());

  // Convert from boost matrix to eigen matrix
  // we permit ourselves to use int for loops
  // because the indexing is useful for referencing between datatypes
  for (unsigned int rowIndex = 0; rowIndex < mat.size1(); rowIndex++) {
    for (unsigned int columnIndex = 0; columnIndex < mat.size2(); columnIndex++) {
      convertedInput(rowIndex, columnIndex) = mat(rowIndex, columnIndex);
    }
  }

  // Now for the actual math
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(convertedInput, Eigen::ComputeFullU | Eigen::ComputeFullV);

  // We can safely use adjoint instead of transpose because this is a real matrix
  Eigen::MatrixXd sigma = svd.singularValues().asDiagonal();

  double tolerance = 0.001;
  // Take the reciprocal of each element, if possible,
  // as per https://en.wikipedia.org/wiki/Moore%E2%80%93Penrose_inverse
  // We only hit the diagonal, since this is a diagonal matrix.
  // We allow this loop type because the indexing is useful.
  for (int i = 0; i < sigma.rows(); i++) {
    if (fabs(sigma(i, i)) < tolerance) {
      sigma(i, i) = 0;
    } else {
      sigma(i, i) = 1 / sigma(i, i);
    }
  }

  Eigen::MatrixXd sigmaTransposed = sigma.transpose();
  Eigen::MatrixXd eigenResult = svd.matrixV() * sigmaTransposed * svd.matrixU().adjoint();

  // Convert eigen matrix to boost matrix
  // We permit ourselves to use int for loops
  // because the indexing is useful for referencing between datatypes
  for (unsigned int rowIndex = 0; rowIndex < mat.size1(); rowIndex++) {
    for (unsigned int columnIndex = 0; columnIndex < mat.size2(); columnIndex++) {
      result(rowIndex, columnIndex) = static_cast<double>(eigenResult(rowIndex, columnIndex));
    }
  }
  return result;
}

std::vector<RobotPosition> Robot::computeIK(Point targetPoint, Environment environment) {

  // Initialize the angles vector theta
  std::vector<RobotPosition> allRobotPositions;
  boost::numeric::ublas::vector<double> velocity(6);
  boost::numeric::ublas::vector<double> theta(6);
  theta(0) = initialJointAngles[0]; theta(1) = initialJointAngles[1]; theta(2) = initialJointAngles[2];
  theta(3) = initialJointAngles[3]; theta(4) = initialJointAngles[4]; theta(5) = initialJointAngles[5];

  std::vector<double> jointAngles = initialJointAngles;

  std::vector<Point> robotJointPosition = computeFk(jointAngles);
  Point eePosition = robotJointPosition.back();
  PathPlanner pathPlanner;
  std::vector<Point> getPath = pathPlanner.findStraightPath(eePosition, targetPoint);

  double eeX; double eeY; double eeZ;
  double targetX; double targetY; double targetZ;
  double norm;
  int counter = 0;

  for(auto& pathPoints:getPath) {

    // Extact each point from a discretized path to find joint angles
    robotJointPosition = computeFk(jointAngles);
    eePosition = robotJointPosition.back();
    eeX = eePosition.getX(); eeY = eePosition.getY(); eeZ = eePosition.getZ();
    targetX = pathPoints.getX(); targetY = pathPoints.getY(); targetZ = pathPoints.getZ();
    velocity(0) = targetX - eeX; velocity(1) = targetY - eeY; velocity(2) = targetZ - eeZ;
    velocity(3) = 0.0; velocity(4) = 0.0; velocity(5) = 0.0;
    norm = norm_2(velocity);
    counter++;

    // Loop through each point to compute the inverse kinematics using the pseudo-inverse Jacobian
    while (norm > 0.01) {

      robotJointPosition = computeFk(jointAngles);
      RobotPosition robotPosition(robotJointPosition, jointAngles);
      std::vector<boost::numeric::ublas::matrix<double>> tTrans = computeTransformationMatrices(jointAngles);
      boost::numeric::ublas::matrix<double> jacobian = computeJacobian(robotPosition, tTrans);
      boost::numeric::ublas::matrix<double> pseudoInverse = penroseInverseMatrix(jacobian);
      Point eePosition = robotJointPosition.back();

      eeX = eePosition.getX(); eeY = eePosition.getY(); eeZ = eePosition.getZ();
      targetX = pathPoints.getX(); targetY = pathPoints.getY(); targetZ = pathPoints.getZ();
      velocity(0) = targetX - eeX; velocity(1) = targetY - eeY; velocity(2) = targetZ - eeZ;
      velocity(3) = 0.0; velocity(4) = 0.0; velocity(5) = 0.0;
      norm = norm_2(velocity);

      boost::numeric::ublas::vector<double> deltaTheta = prod(pseudoInverse, velocity);
      theta = theta + 0.0000001*deltaTheta;
      jointAngles[0] = theta(0); jointAngles[1] = theta(1); jointAngles[2] = theta(2);
      jointAngles[3] = theta(3); jointAngles[4] = theta(4); jointAngles[5] = theta(5);

    }

    RobotPosition robotPosition(robotJointPosition, jointAngles);

    // Check for any collision
    if (robotPosition.checkCollision(environment)) {
      allRobotPositions.push_back(robotPosition);
    } else {break;}
  }

return allRobotPositions;

}


#include "common.h"
#include <cmath>
#include <ctime>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>

boost::random::mt19937 rng(time(0));
boost::random::uniform_01<float_type> dist01;

// calculate digamma(x+n) - digamma(x) = \sum 1/(x+k), k=0,1,...n-1
float_type digammaDiff(float_type x, int n)
{
    float_type ret = 0.0;

    for (int i = n - 1; i >= 0; i--) {
        ret += 1.0/(x + i);
    }

    return ret;
}

float_type GegenbauerDAlphaAt0(int n, float_type x)
{
    if (n <= 0) {
        return 0.0;
    } else if (n == 1) {
        return 2 * x;
    } else if (n == 2) {
        return 2 * x * x - 1;
    }

    // for -1 < x < 1 it actually equals
    // return 2 * cos(n * acos(x)) / n;

    float_type a = 2 * x, b = 2 * x * x - 1;
    float_type c;
    for (int i = 3; i <= n; i++) {
        c = (2 * (i - 1) * x * b - (i - 2) * a) / i;
        a = b;
        b = c;
    }

    return c;
}

bool IsInteger(float_type n)
{
    return std::abs(n - floor(n + EPS)) < EPS;
}

float_type random(float_type min, float_type max)
{
    return min + (max - min) * dist01(rng);
}

int randomint(int min, int max)
{
    boost::random::uniform_int_distribution<int> dist(min,max);
    return dist(rng);
}

cpx_t RandomComplex(float_type maxNorm)
{
    float_type r = random(0.0, maxNorm);
    float_type theta = random(0.0, 2 * acos(-1.0));
    return cpx_t(r * cos(theta), r * sin(theta));
}

std::string NowToString()
{
    time_t rawtime;
    struct tm * timeinfo;
    char buffer[80];

    time (&rawtime);
    timeinfo = localtime(&rawtime);

    strftime(buffer,sizeof(buffer),"%Y-%m-%d %I:%M:%S",timeinfo);
    return std::string(buffer);
}


//
//  seqols.cpp
//
//
// Created by Jose V. Alcala Burgos on 7/30/13.
//  Copyright [2013] Jose V. Alcala Burgos
//
//

#include "base/seqols.h"

// System
#include <armadillo>

#include <iostream>
#include <cmath>

using namespace arma;
using namespace std;

void SeqOls::useObservations(mat X, mat Y  // A n x p matrix with n responses,
                                           // one response per row
                             ) {
    if (X.size() != Y.size()) {
        cout << " The predictor/response matrices should have the same size";
        cout << endl;
    }

    n = X.n_rows;
    p = X.n_cols;
    X = join_rows(X, ones(n, 1));
    P = inv((X.t()) * X);
    B = P * (X.t() * Y);
    G = inv(B.rows(0, p - 1));
}

void SeqOls::addObservation(mat x,
                            mat y
                            ) {
    if (x.size() != y.size()) {
        cout << " The predictor/response matrices should have the same size";
        cout << endl;
    }

    if (x.n_cols != p) {
        cout << " The new observation should have " << p << "columns" << endl;
    }

    if (x.n_rows != 1) {
        cout << " Add only ONE observation. " << endl;
    }

    n = n + 1;
    x = join_rows(x, ones(1, 1));
    double alpha;
    alpha = as_scalar((1.0 / (1.0 + x * P * x.t())));
    mat u = P * x.t();
    u = u.rows(0, p - 1);
    u = alpha * u;
    mat v = y - x * B;
    v = v.t();
    B = B + alpha * (P * x.t() * (y - x * B));
    P = P - alpha * (P * x.t() * x * P.t());
    double beta;
    beta = as_scalar(1.0 / (1.0 + v.t() * G * u));
    G = G - beta * (G * u * v.t() * G);
}

// Prints the Ols estimator
void SeqOls::printEstimator() {
    B.print(" B_estimator : ");
    G.print("G_estimator (Hessian inverse):");
}

// Tests that the inverse of the first p rows of B is equal to G.
void SeqOls::testInverse() {
    double tolerance = 0.00000001;
    mat error = (B.rows(0, p - 1) * G - eye<mat>(p, p));
    if (norm(error, 2) < tolerance) {
        cout << "The inverse of the estimator is correct." << endl;
    } else {
        cout << "The inverse of the estimator is incorrect." << endl;
    }
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/core/math/math_eigen_system_decomposition.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

template<typename vector3_type,typename matrix3x3_type>
void eigen_value_decomposition_test(vector3_type d,matrix3x3_type R)
{
  using std::fabs;

  typedef typename vector3_type::value_type    real_type;
  typedef typename vector3_type::value_traits  value_traits;

  real_type tol = 0.0001;

  vector3_type r1 = vector3_type(R(0,0),R(1,0),R(2,0));
  vector3_type r2 = vector3_type(R(0,1),R(1,1),R(2,1));
  vector3_type r3 = vector3_type(R(0,2),R(1,2),R(2,2));
  real_type    d0 = d(0);
  real_type    d1 = d(1);
  real_type    d2 = d(2);

  matrix3x3_type D = diag(d);
  matrix3x3_type A = R*D*trans(R);
  BOOST_CHECK( is_symmetric(A,tol) );

  matrix3x3_type V;
  OpenTissue::math::eigen(A, V, d);

  vector3_type e1 = vector3_type(V(0,0),V(1,0),V(2,0));
  vector3_type e2 = vector3_type(V(0,1),V(1,1),V(2,1));
  vector3_type e3 = vector3_type(V(0,2),V(1,2),V(2,2));

  real_type determinant = det(V);
  BOOST_CHECK_CLOSE( fabs( determinant ), value_traits::one(), tol );

  real_type epsilon = 10e-7;

  matrix3x3_type Itest = trans(V)*V;

  BOOST_CHECK_CLOSE( Itest(0,0), value_traits::one() , tol );
  BOOST_CHECK_CLOSE( Itest(1,1), value_traits::one() , tol );
  BOOST_CHECK_CLOSE( Itest(2,2), value_traits::one() , tol );
  //--- The check close versin behaves strange when rhs is exactly
  //--- zero?, so I use the check-version instead...
  //BOOST_CHECK_CLOSE(      Itest(0,1) , value_traits::zero(), tol );
  //BOOST_CHECK_CLOSE(      Itest(0,2) , value_traits::zero(), tol );
  //BOOST_CHECK_CLOSE(      Itest(1,0) , value_traits::zero(), tol );
  //BOOST_CHECK_CLOSE(      Itest(1,2) , value_traits::zero(), tol );
  //BOOST_CHECK_CLOSE(      Itest(2,0) , value_traits::zero(), tol );
  //BOOST_CHECK_CLOSE(      Itest(2,1) , value_traits::zero(), tol );
  BOOST_CHECK( fabs(Itest(0,1))<epsilon );
  BOOST_CHECK( fabs(Itest(0,2))<epsilon );
  BOOST_CHECK( fabs(Itest(1,0))<epsilon );
  BOOST_CHECK( fabs(Itest(1,2))<epsilon );
  BOOST_CHECK( fabs(Itest(2,0))<epsilon );
  BOOST_CHECK( fabs(Itest(2,1))<epsilon );

  matrix3x3_type Atest = A - V*diag(d)*trans(V);

  BOOST_CHECK( fabs(Atest(0,0))<epsilon );
  BOOST_CHECK( fabs(Atest(0,1))<epsilon );
  BOOST_CHECK( fabs(Atest(0,2))<epsilon );
  BOOST_CHECK( fabs(Atest(1,0))<epsilon );
  BOOST_CHECK( fabs(Atest(1,1))<epsilon );
  BOOST_CHECK( fabs(Atest(1,2))<epsilon );
  BOOST_CHECK( fabs(Atest(2,0))<epsilon );
  BOOST_CHECK( fabs(Atest(2,1))<epsilon );
  BOOST_CHECK( fabs(Atest(2,2))<epsilon );

  bool match1 = fabs( d0 - d(0) ) < epsilon &&
                fabs( d1 - d(1) ) < epsilon &&
                fabs( d2 - d(2) ) < epsilon ;

  bool match2 = fabs( d0 - d(0) )< epsilon &&
                fabs( d1 - d(2) )< epsilon &&
                fabs( d2 - d(1) )< epsilon ;

  bool match3 = fabs( d0 - d(1) )< epsilon &&
                fabs( d1 - d(0) )< epsilon &&
                fabs( d2 - d(2) )< epsilon ;

  bool match4 = fabs( d0 - d(1) )< epsilon &&
                fabs( d1 - d(2) )< epsilon &&
                fabs( d2 - d(0) )< epsilon ;

  bool match5 = fabs( d0 - d(2) )< epsilon &&
                fabs( d1 - d(0) )< epsilon &&
                fabs( d2 - d(1) )< epsilon ;

  bool match6 = fabs( d0 - d(2) )< epsilon &&
                fabs( d1 - d(1) )< epsilon &&
                fabs( d2 - d(0) )< epsilon ;

  BOOST_CHECK( match1 || match2 || match3 || match4 || match5 || match6 );
}

BOOST_AUTO_TEST_SUITE(opentissue_math_eigen);

  BOOST_AUTO_TEST_CASE(random_testing)
  {
    typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
    typedef math_types::vector3_type                         vector3_type;
    typedef math_types::matrix3x3_type                       matrix3x3_type;
    typedef math_types::real_type                            real_type;

    matrix3x3_type R;
    vector3_type d;

    for(int i= 0;i<100;++i)
    {
      //--- non-negative eigen-values
      random(d,0,1);
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- non-positive eigen-values
      random(d,-1,0);
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- one zero eigen-values
      random(d,0,1);
      d(0) = 0;
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- one zero eigen-values
      random(d,0,1);
      d(1) = 0;
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- one zero eigen-values
      random(d,0,1);
      d(2) = 0;
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- two zero eigen-values
      random(d,0,1);
      d(0) = 0;
      d(1) = 0;
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- two zero eigen-values
      random(d,0,1);
      d(0) = 0;
      d(2) = 0;
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- two zero eigen-values
      random(d,0,1);
      d(1) = 0;
      d(2) = 0;
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- three zero eigen-values
      d.clear();
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- multiplicity of 3
      random(d,0,1);
      d(1) = d(0);
      d(2) = d(0);
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- multiplicity of 2
      random(d,0,1);
      d(1) = d(0);
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- multiplicity of 2
      random(d,0,1);
      d(2) = d(0);
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);

      //--- multiplicity of 2
      random(d,0,1);
      d(2) = d(1);
      random(R);
      R = ortonormalize(R);
      eigen_value_decomposition_test(d,R);
    }
  }

BOOST_AUTO_TEST_SUITE_END();


#include <NTL/HNF.h>

#include <NTL/new.h>

NTL_START_IMPL

// This implements a variation of an algorithm in
// [P. Domich, R. Kannan and L. Trotter, Math. Oper. Research 12:50-59, 1987].
// I started with the description in Henri Cohen's book, but had to modify
// that because Cohen does not actually keep the numbers reduced modulo
// the determinant, which leads to larger than necessary numbers.
// This modifiaction was put in place in v3.9b.

static
void EuclUpdate(vec_ZZ& u, vec_ZZ& v,
                const ZZ& a, const ZZ& b, const ZZ& c, const ZZ& d,
                const ZZ& M)

{
   long m = u.length();
   long i;

   ZZ M1;
   RightShift(M1, M, 1);

   ZZ t1, t2, t3;

   for (i = 1; i <= m; i++) {
      mul(t1, u(i), a);
      mul(t2, v(i), b);
      add(t1, t1, t2);
      rem(t1, t1, M);
      if (t1 > M1)
         sub(t1, t1, M);

      t3 = t1;

      mul(t1, u(i), c);
      mul(t2, v(i), d);
      add(t1, t1, t2);
      rem(t1, t1, M);
      if (t1 > M1)
         sub(t1, t1, M);

      u(i) = t3;
      v(i) = t1;
   }
}

static
void FixDiag(vec_ZZ& u, const ZZ& a, const vec_ZZ& v, const ZZ& M, long m)
{
   long i;
   ZZ t1;

   for (i = 1; i <= m; i++) {
      mul(t1, a, v(i));
      rem(u(i), t1, M);
   }
}

static
void ReduceW(vec_ZZ& u, const ZZ& a, const vec_ZZ& v, const ZZ& M, long m)
{
   long i;
   ZZ t1, t2;

   for (i = 1; i <= m; i++) {
      mul(t1, a, v(i));
      sub(t2, u(i), t1);
      rem(u(i), t2, M);
   }
}

void HNF(mat_ZZ& W, const mat_ZZ& A_in, const ZZ& D_in)
{
   mat_ZZ A = A_in;

   long n = A.NumRows();
   long m = A.NumCols();

   ZZ D = D_in;
   if (D < 0)
      negate(D, D);

   if (n == 0 || m == 0 || D == 0)
      LogicError("HNF: bad input");

   W.SetDims(m, m);
   clear(W);

   long i, j, k;
   ZZ d, u, v, c1, c2;

   k = n;

   for (i = m; i >= 1; i--) {
      for (j = k-1; j >= 1; j--) {
         if (A(j, i) != 0) {
            XGCD(d, u, v, A(k, i), A(j, i));
            div(c1, A(k, i), d);
            div(c2, A(j, i), d);
            negate(c2, c2);
            EuclUpdate(A(j), A(k), c1, c2, v, u, D);
         }
      }

      XGCD(d, u, v, A(k, i), D);
      FixDiag(W(i), u, A(k), D, i);
      if (W(i, i) == 0) W(i, i) = D;

      for (j = i+1; j <= m; j++) {
         div(c1, W(j, i), W(i, i));
         ReduceW(W(j), c1, W(i), D, i);
      }

      div(D, D, d);
      k--;
   }
}

NTL_END_IMPL


#include <Core/Core.h>
#include <Eigen/Eigen.h>
#include "Sundials.h"
#include "Mooring.h"

namespace Upp {

MooringStatus Catenary(double rho_m, double rho_m3, double rho_water, double moorlen, double BL,
			  double xanchorvessel, double zanchor, double zvessel,
			  double &Fhanchorvessel, double &Fvanchor, double &Fvvessel,
			  double &xonfloor, Vector<double> &x, Vector<double> &z, int num) {
	if (zanchor < 0 && zvessel < 0)
		throw Exc("Catenary. Anchor and vessel z positions have to be positive");
	if (xanchorvessel < 0)
		throw Exc("Catenary. x distance between anchor and vessel has to be positive");

	const double g = 9.81;

	double lmb = rho_m*(rho_m3 - rho_water)/rho_m3;

  	Buffer<double> udata(1, 1);
	SolveNonLinearEquationsSun(udata, 1, [&](const double y[], double residuals[])->bool {
		double Blimit = y[0];
		residuals[0] =  sqrt(zanchor*(zanchor + 2*Blimit)) + sqrt(zvessel*(zvessel + 2*Blimit)) - moorlen;
		return true;
	});
	double Blimit = udata[0];

    double xanchorvessel_limit = Blimit*(acosh(zanchor/Blimit + 1) + acosh(zvessel/Blimit + 1));

	double delta = xanchorvessel/(num - 1);
	x.SetCount(num);
	z.SetCount(num);

	MooringStatus status;

	if (xanchorvessel < moorlen - zanchor - zvessel) {
		status = LOOSE_ON_FLOOR;

		if (!IsNull(rho_m)) {
	        Fhanchorvessel = 0;
	        Fvanchor = lmb*g*zanchor;
	        Fvvessel = lmb*g*zvessel;
		} else
			Fhanchorvessel = Fvanchor = Fvvessel = Null;

     	xonfloor = xanchorvessel;

		x.SetCount(4);
		z.SetCount(4);
		x[0] = 0;				z[0] = zanchor;
		x[1] = 0;				z[1] = 0;
		x[2] = xanchorvessel;	z[2] = 0;
		x[3] = xanchorvessel;	z[3] = zvessel;
	} else if (xanchorvessel < xanchorvessel_limit) {
        status = CATENARY_ON_FLOOR;

	  	Buffer<int> consdata(1, 2);		// B > 0

	  	Buffer<double> udata(1, Blimit/2.);
		SolveNonLinearEquationsSun(udata, 1, [&](const double y[], double residuals[])->bool {
			double B = y[0];
			residuals[0] = xanchorvessel - B*acosh(zanchor/B + 1) - B*acosh(zvessel/B + 1) + sqrt(zanchor*(zanchor + 2*B)) + sqrt(zvessel*(zvessel + 2*B)) - moorlen;
			return true;
		}, consdata);
		double B = udata[0];

        double xcatanchor = B*acosh(zanchor/B + 1);
        double xcatvessel = B*acosh(zvessel/B + 1);
		xonfloor = xanchorvessel - xcatanchor - xcatvessel;

		if (!IsNull(rho_m)) {
	        Fhanchorvessel = lmb*g*B;
	        Fvanchor = Fhanchorvessel*sinh(xcatanchor/B);
	        Fvvessel = Fhanchorvessel*sinh(xcatvessel/B);
		} else
			Fhanchorvessel = Fvanchor = Fvvessel = Null;

        for (int i = 0; i < x.size(); ++i) {
            x[i] = i*delta;
            if (x[i] < xcatanchor)
        		z[i] = B*(cosh((xcatanchor - x[i])/B) - 1);
            else if (x[i] > (xanchorvessel - xcatvessel))
                z[i] = B*(cosh((max(0., x[i] - (xanchorvessel - xcatvessel)))/B) - 1);
            else
                z[i] = 0;
        }
    } else if (xanchorvessel < sqrt(sqr(moorlen) - sqr(zvessel - zanchor))) {
        status = CATENARY;

		int neq = 2;
        double deltaz = zvessel - zanchor;

	  	Buffer<double> udata(neq);
	  	Buffer<int> consdata(2);
	  	consdata[0] = 2;			// B > 0
	  	consdata[1] = 0;

	  	double x1_0, B_0, x1, B;
	  	bool done = false;
	  	for (x1_0 = 0; x1_0 < abs(xanchorvessel) && !done; x1_0 += abs(xanchorvessel)/4) {
	  		for (B_0 = abs(Blimit); B_0 < 10*B_0 && !done; B_0 += 5*abs(Blimit)/4) {
		  		udata[0] = B_0;
		  		udata[1] = x1_0;
				SolveNonLinearEquationsSun(udata, neq, [&](const double y[], double residuals[])->bool {
					double B = y[0];
					double x1 = y[1];

					residuals[0] = B*(sinh((x1 + xanchorvessel)/B) - sinh(x1/B)) - moorlen;
					residuals[1] = B*(cosh((x1 + xanchorvessel)/B) - cosh(x1/B)) - deltaz;

					return true;
				}, consdata);
				B = udata[0];
				x1 = udata[1];
				if (abs((x1 + xanchorvessel)/B) < 3)
					done = true;
	  		}
	  	}
	  	if (!done)
			throw Exc("Catenary. Solving problem");

		if (!IsNull(rho_m)) {
	        Fhanchorvessel = lmb*g*abs(B);
	        Fvanchor = -Fhanchorvessel*sinh(x1/B);
	        Fvvessel = Fhanchorvessel*sinh((x1 + xanchorvessel)/B);
		} else
			Fhanchorvessel = Fvanchor = Fvvessel = Null;

        xonfloor = 0;

        for (int i = 0; i < x.size(); ++i) {
            x[i] = i*delta;
        	z[i] = B*(cosh((x[i] + x1)/B) - 1) - B*(cosh(x1/B) - 1) + zanchor;
        }
    } else {
		status = BROKEN;
		if (!IsNull(rho_m))
			Fhanchorvessel = Fvanchor = Fvvessel = xonfloor = 0;
		else
			Fhanchorvessel = Fvanchor = Fvvessel = xonfloor = Null;

		x.Clear();
		z.Clear();
    }

	if (!IsNull(BL))
    	if (max(sqrt(sqr(Fhanchorvessel) + sqr(Fvanchor)), sqrt(sqr(Fhanchorvessel) + sqr(Fvvessel))) >= BL)
        	status = BL_EXCEDEED;

	return status;
}

MooringStatus Catenary(double rho_m, double rho_m3, double rho_water, double moorlen, double BL,
			  double xanchorvessel, double zanchor, double zvessel,
			  double &Fhanchorvessel, double &Fvanchor, double &Fvvessel, double &xonfloor) {
	Vector<double> x, z;

	return Catenary(rho_m, rho_m3, rho_water, moorlen, BL, xanchorvessel,
			zanchor, zvessel, Fhanchorvessel, Fvanchor, Fvvessel, xonfloor, x, z, 0);
}

MooringStatus Catenary(double moorlen, double xanchorvessel, double zanchor, double zvessel, double &xonfloor,
				Vector<double> &x, Vector<double> &z, int num) {
	double rho_m = Null,  rho_m3 = Null, rho_water = Null, BL = Null, Fhanchorvessel, Fvanchor, Fvvessel;

	return Catenary(rho_m, rho_m3, rho_water, moorlen, BL, xanchorvessel,
			zanchor, zvessel, Fhanchorvessel, Fvanchor, Fvvessel, xonfloor, x, z, num);
}

MooringStatus Catenary(double moorlen, double xanchorvessel, double zanchor, double zvessel, double &xonfloor) {
	double rho_m = Null,  rho_m3 = Null, rho_water = Null, BL = Null, Fhanchorvessel, Fvanchor, Fvvessel;
	Vector<double> x, z;
	return Catenary(rho_m, rho_m3, rho_water, moorlen, BL, xanchorvessel,
			zanchor, zvessel, Fhanchorvessel, Fvanchor, Fvvessel, xonfloor, x, z, 0);
}

bool CatenaryGetLen0(double xonfloor, double xanchorvessel, double zanchor, double zvessel,
					double &moorlen) {
	if (xonfloor >= xanchorvessel) {
		moorlen = xanchorvessel + 2*(xonfloor - xanchorvessel) + zanchor + zvessel;
		return false;
	}

	Buffer<int> consdata(2, 2);		// B > 0 && moorlen > 0
  	Buffer<double> udata(2);
  	udata[0] = 1;
  	udata[1] = sqrt(sqr(xanchorvessel) + sqr(zanchor - zvessel));
	SolveNonLinearEquationsSun(udata, 2, [&](const double y[], double residuals[])->bool {
		double B = y[0];
		double moorlen = y[1];
		residuals[0] = xanchorvessel - B*acosh(zanchor/B + 1) - B*acosh(zvessel/B + 1) + sqrt(zanchor*(zanchor + 2*B)) + sqrt(zvessel*(zvessel + 2*B)) - moorlen;
    	double xcatanchor = B*acosh(zanchor/B + 1);
    	double xcatvessel = B*acosh(zvessel/B + 1);
		residuals[1] = xanchorvessel - xcatanchor - xcatvessel - xonfloor;
		return true;
	}, consdata);
	//double B = udata[0];
	moorlen = udata[1];

	return true;
					}

MooringStatus CatenaryGetLen(double xonfloor, double xanchorvessel, double zanchor, double zvessel,
			double &moorlen) {
	if (!CatenaryGetLen0(xonfloor, xanchorvessel, zanchor, zvessel, moorlen))
		return LOOSE_ON_FLOOR;

	return Catenary(moorlen, xanchorvessel, zanchor, zvessel, xonfloor);
}

MooringStatus CatenaryGetLen(double rho_m, double rho_m3, double rho_water, double xonfloor,
			double BL, double xanchorvessel, double zanchor, double zvessel,
			double &Fhanchorvessel, double &Fvanchor, double &Fvvessel, double &moorlen) {
	if (!CatenaryGetLen0(xonfloor, xanchorvessel, zanchor, zvessel, moorlen))
		return LOOSE_ON_FLOOR;

	return Catenary(rho_m, rho_m3, rho_water, moorlen, BL, xanchorvessel, zanchor, zvessel,
					Fhanchorvessel, Fvanchor, Fvvessel, xonfloor);
}

const char *MooringStatusStr(MooringStatus status) {
	const char *str[5] = {"loose on floor", "catenary on floor", "catenary",
						 "line length exceeded", "break load exdeeded"};
	ASSERT(int(status) < 5);
	return str[int(status)];
}

bool IsOK(MooringStatus status) {
	return status < BROKEN;
}

}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Astrodynamics/Gravitation/triAxialEllipsoidGravity.h"

namespace tudat
{

namespace unit_tests
{

using namespace tudat::gravitation;

//! Function to get theoretical values of zonal gravity field coefficient of ellipsoid of revolution
double getZonalTermForEllipsoidOfRevolution(
        const double axisA, const double axisC, const double referenceRadius, const int degree )
{
    double zonalTerm = 0.0;
    if( degree % 2 == 0 )
    {
        zonalTerm = 3.0 / std::pow( referenceRadius, degree ) *
                std::pow( axisA * axisA - axisC * axisC, degree / 2 ) *
                std::pow( -1.0, degree / 2 ) /
                ( ( static_cast< double >( degree ) + 1.0 ) *
                  ( static_cast< double >( degree ) + 3.0 ) );
    }

    return zonalTerm;
}

BOOST_AUTO_TEST_SUITE( test_tri_axial_ellipsoid_gravity )

//! Test gravity field coefficients of homogeneous triaxial ellipsoid
BOOST_AUTO_TEST_CASE( testTriAxialEllipsoidGravity )
{
    double axisA = 26.8E3;
    double axisB = 22.4E3;
    double axisC = 18.4E3;

    // Compute coefficients
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > sphericalHarmonicCoefficients =
            createTriAxialEllipsoidSphericalHarmonicCoefficients( axisA, axisB, axisC, 4, 4 );
    double referenceRadius = calculateTriAxialEllipsoidReferenceRadius( axisA, axisB, axisC );

    // Compute expected low-degree coefficients
    double expectedC20 = 1.0 / ( 5.0 * referenceRadius * referenceRadius ) *
            ( axisC * axisC - ( axisA * axisA + axisB * axisB ) / 2.0 );
    double expectedC22 = 1.0 / ( 20.0 * referenceRadius * referenceRadius ) *
            ( axisA * axisA - axisB * axisB );
    double expectedC40 = 15.0 / 7.0 * ( expectedC20 * expectedC20 +
                                        2.0 * expectedC22 * expectedC22 );
    double expectedC42 = 5.0 / 7.0 * ( expectedC20 * expectedC22 );
    double expectedC44 = 5.0 / 28.0 * ( expectedC22 * expectedC22 );

    BOOST_CHECK_CLOSE_FRACTION(
                expectedC20, sphericalHarmonicCoefficients.first( 2, 0 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC22, sphericalHarmonicCoefficients.first( 2, 2 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC40, sphericalHarmonicCoefficients.first( 4, 0 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC42, sphericalHarmonicCoefficients.first( 4, 2 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION(
                expectedC44, sphericalHarmonicCoefficients.first( 4, 4 ),
                2.0 * std::numeric_limits< double >::epsilon( ) );

    // Check whether C-coefficients with odd degree or order, and all S-coefficients, are zero.
    for( unsigned int i = 0; i < 5; i++ )
    {
        for( unsigned int j = 0; j < 5; j++ )
        {
            if( ( i % 2 != 0 ) || ( j % 2 ) != 0 )
            {
                BOOST_CHECK_EQUAL( sphericalHarmonicCoefficients.first( i, j ), 0.0 );
            }
            BOOST_CHECK_EQUAL( sphericalHarmonicCoefficients.second( i, j ), 0.0 );

        }
    }

    // Compute gravity field coefficients for ellipsoid of revolution
    axisB = axisA;
    referenceRadius = calculateTriAxialEllipsoidReferenceRadius( axisA, axisB, axisC );
    sphericalHarmonicCoefficients = createTriAxialEllipsoidSphericalHarmonicCoefficients( axisA, axisB, axisC, 20, 1 );

    // Check computation against theoretical values.
    for( int i = 2; i < 21; i += 2 )
    {
        BOOST_CHECK_CLOSE_FRACTION(
                    sphericalHarmonicCoefficients.first( i, 0 ),
                    getZonalTermForEllipsoidOfRevolution( axisA, axisC, referenceRadius, i ),
                    2.0 * std::numeric_limits< double >::epsilon( ) );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Advancing_front_surface_reconstruction.h>
#include <CGAL/tuple.h>
#include <boost/lexical_cast.hpp>

typedef CGAL::Simple_cartesian<double> K;
typedef K::Point_3  Point_3;

typedef std::array<std::size_t,3> Facet;

namespace std {
  std::ostream&
  operator<<(std::ostream& os, const Facet& f)
  {
    os << "3 " << f[0] << " " << f[1] << " " << f[2];
    return os;
  }
}

struct Perimeter {

  double bound;

  Perimeter(double bound)
    : bound(bound)
  {}

  template <typename AdvancingFront, typename Cell_handle>
  double operator() (const AdvancingFront& adv, Cell_handle& c,
                     const int& index) const
  {
    // bound == 0 is better than bound < infinity
    // as it avoids the distance computations
    if(bound == 0){
      return adv.smallest_radius_delaunay_sphere (c, index);
    }

    // If perimeter > bound, return infinity so that facet is not used
    double d  = 0;
    d = sqrt(squared_distance(c->vertex((index+1)%4)->point(),
                              c->vertex((index+2)%4)->point()));
    if(d>bound) return adv.infinity();
    d += sqrt(squared_distance(c->vertex((index+2)%4)->point(),
                               c->vertex((index+3)%4)->point()));
    if(d>bound) return adv.infinity();
    d += sqrt(squared_distance(c->vertex((index+1)%4)->point(),
                               c->vertex((index+3)%4)->point()));
    if(d>bound) return adv.infinity();

    // Otherwise, return usual priority value: smallest radius of
    // delaunay sphere
    return adv.smallest_radius_delaunay_sphere (c, index);
  }
};

int main(int argc, char* argv[])
{
  std::ifstream in((argc>1)?argv[1]:"data/half.xyz");
  double per = (argc>2)?boost::lexical_cast<double>(argv[2]):0;
  std::vector<Point_3> points;
  std::vector<Facet> facets;

  std::copy(std::istream_iterator<Point_3>(in),
            std::istream_iterator<Point_3>(),
            std::back_inserter(points));

  Perimeter perimeter(per);
  CGAL::advancing_front_surface_reconstruction(points.begin(),
                                               points.end(),
                                               std::back_inserter(facets),
                                               perimeter);

  std::cout << "OFF\n" << points.size() << " " << facets.size() << " 0\n";
  std::copy(points.begin(),
            points.end(),
            std::ostream_iterator<Point_3>(std::cout, "\n"));
  std::copy(facets.begin(),
            facets.end(),
            std::ostream_iterator<Facet>(std::cout, "\n"));

  return 0;
}


#ifndef CANNON_MATH_FUNC_ROSENBROCK_H
#define CANNON_MATH_FUNC_ROSENBROCK_H

/*!
 * \file cannon/math/func/rosenbrock.hpp
 * \brief File containing Rosenbrock function and gradient definitions.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace math {

    /*!
     * \brief Compute the value of the Rosenbrock function at the input point.
     * See https://en.wikipedia.org/wiki/Rosenbrock_function
     *
     * \param x The point at which to evaluate the Rosenbrock function.
     *
     * \returns The value of the Rosenbrock function at the input point.
     */
    double rosenbrock(const Vector2d& x);

    /*!
     * \brief Compute the gradient of the Rosenbrock function at the input point.
     *
     * \param x The point at which to compute the gradient.
     *
     * \returns Rosenbrock function gradient at the input point.
     */
    Vector2d rosenbrock_grad(const Vector2d& x);

  } // namespace math
} // namespace cannon

#endif /* ifndef CANNON_MATH_FUNC_ROSENBROCK_H */


#include "problemes.h"
#include "numerique.h"
#include "puissance.h"

#include <boost/algorithm/string.hpp>

ENREGISTRER_PROBLEME(162, "Hexadecimal numbers") {
    // In the hexadecimal number system numbers are represented using 16 different digits:
    //
    //                           0,1,2,3,4,5,6,7,8,9,A,B,C,D,E,F
    //
    // The hexadecimal number AF when written in the decimal number system equals 10x16+15=175.
    //
    // In the 3-digit hexadecimal numbers 10A, 1A0, A10, and A01 the digits 0,1 and A are all present.
    // Like numbers written in base ten we write hexadecimal numbers without leading zeroes.
    //
    // How many hexadecimal numbers containing at most sixteen hexadecimal digits exist with all of the digits 0,1, and
    // A present at least once?
    // Give your answer as a hexadecimal number.
    //
    // (A,B,C,D,E and F in upper case, without any leading or trailing code that marks the number as hexadecimal and
    // without leading zeroes , e.g. 1A3F and not: 1a3f and not 0x1a3f and not $1A3F and not #1A3F and not 0000001A3F)
    uint128_t resultat = 0;
    for (size_t n = 3; n < 17; ++n) {
        resultat += 15 * puissance::puissance<uint128_t>(16, n - 1);
        resultat += 41 * puissance::puissance<uint128_t>(14, n - 1);
        resultat -= 43 * puissance::puissance<uint128_t>(15, n - 1);
        resultat -= puissance::puissance<uint128_t>(13, n);
    }

    std::ostringstream oss;
    oss << std::hex << resultat;

    std::string str = oss.str();
    boost::to_upper(str);

    return str;
}


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2009
//
//============================================================================

#ifndef __Thea_Algorithms_LinearLeastSquares3_hpp__
#define __Thea_Algorithms_LinearLeastSquares3_hpp__

#include "../Common.hpp"
#include "../Array.hpp"
#include "../Line3.hpp"
#include "../MatVec.hpp"
#include "../Plane3.hpp"
#include "CentroidN.hpp"
#include "Iterators.hpp"
#include "PointTraitsN.hpp"
#include <Eigen/Eigenvalues>

namespace Thea {
namespace Algorithms {

/** Fitting linear models to 3D data by minimizing sum-of-squared-errors. */
template <typename T, typename Enable = void>
class /* THEA_API */ LinearLeastSquares3
{
  public:
    /**
     * Linear least-squares fitting of a line to a set of 3D objects. InputIterator must dereference to type T or pointer-to-T.
     *
     * @param begin The first object in the set.
     * @param end One position beyond the last object in the set.
     * @param line Used to return the best-fit line.
     * @param centroid If non-null, used to return the centroid of the objects, which is computed in the process of finding the
     *   best-fit line.
     *
     * @return The sum of squared fitting errors.
     */
    template <typename InputIterator>
    static double fitLine(InputIterator begin, InputIterator end, Line3 & line, Vector3 * centroid = nullptr);

    /**
     * Linear least-squares fitting of a plane to a set of 3D objects. InputIterator must dereference to type T or pointer-to-T.
     *
     * @param begin The first object in the set.
     * @param end One position beyond the last object in the set.
     * @param plane Used to return the best-fit plane.
     * @param centroid If non-null, used to return the centroid of the objects, which is computed in the process of finding
     *   the best-fit plane.
     *
     * @return The fitting quality: 0 (worst) to 1 (perfect).
     */
    template <typename InputIterator>
    static double fitPlane(InputIterator begin, InputIterator end, Plane3 & plane, Vector3 * centroid = nullptr);

}; // class LinearLeastSquares3

// Fitting linear models to sets of objects that map to single points in 3-space.
template <typename T>
class LinearLeastSquares3<T, typename std::enable_if< IsNonReferencedPointN<T, 3>::value >::type>
{
  public:
    template <typename InputIterator>
    static double fitLine(InputIterator begin, InputIterator end, Line3 & line, Vector3 * centroid = nullptr)
    {
      Vector3d center;
      Matrix3d cov = covMatrix(begin, end, center);

      double sum = 0;
      for (auto iter = makeRefIterator(begin); iter != makeRefIterator(end); ++iter)
      {
        Vector3d diff = PointTraitsN<T, 3>::getPosition(*iter).template cast<double>() - center;
        sum += (diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]);
      }

      Matrix3d m = sum * Matrix3d::Identity() - cov;
      Eigen::SelfAdjointEigenSolver<Matrix3d> eigensolver;
      if (eigensolver.computeDirect(m).info() != Eigen::Success)
        throw Error("LinearLeastSquares3: Could not eigensolve matrix");

      // Eigenvalues are in increasing order
      line = Line3::fromPointAndDirection(center.cast<Real>(), eigensolver.eigenvectors().col(0).cast<Real>());
      if (centroid) *centroid = center.cast<Real>();
      return eigensolver.eigenvalues()[0];
    }

    template <typename InputIterator>
    static double fitPlane(InputIterator begin, InputIterator end, Plane3 & plane, Vector3 * centroid = nullptr)
    {
      Vector3d center;
      Matrix3d cov = covMatrix(begin, end, center);

      Eigen::SelfAdjointEigenSolver<Matrix3d> eigensolver;
      if (eigensolver.computeDirect(cov).info() != Eigen::Success)
        throw Error("LinearLeastSquares3: Could not eigensolve covariance matrix");

      // Eigenvalues are non-negative (covariance matrix is non-negative definite) and in increasing order
      plane = Plane3::fromPointAndNormal(center.cast<Real>(), eigensolver.eigenvectors().col(0).cast<Real>());
      if (centroid) *centroid = center.cast<Real>();
      return eigensolver.eigenvalues()[0];
    }

  private:
    /** Compute the covariance matrix between the coordinates of a set of 3D points. */
    template <typename InputIterator>
    static Matrix3d covMatrix(InputIterator begin, InputIterator end, Vector3d & centroid)
    {
      centroid = CentroidN<T, 3>::compute(begin, end).template cast<double>();

      Matrix3d m = Matrix3d::Zero();
      for (auto iter = makeRefIterator(begin); iter != makeRefIterator(end); ++iter)
      {
        Vector3d diff = PointTraitsN<T, 3>::getPosition(*iter).template cast<double>() - centroid;
        m += diff * diff.transpose();  // outer product
      }

      return m;
    }

}; // class LinearLeastSquares3<Point3>

} // namespace Algorithms
} // namespace Thea

#endif


#include "rotation.h"

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <iostream>

namespace cpt {

Eigen::Matrix3f get_angle_axis_rotation_matrix(const Eigen::Vector3f& angleAxis) {
    return Eigen::AngleAxisf(angleAxis.norm(), angleAxis.normalized()).toRotationMatrix();
}

Eigen::Matrix3f get_euler_xyz_rotation_matrix(const Eigen::Vector3f& xyz) {
    return get_angle_axis_rotation_matrix(xyz(2) * Eigen::Vector3f::UnitZ()) *
        get_angle_axis_rotation_matrix(xyz(1) * Eigen::Vector3f::UnitY()) *
        get_angle_axis_rotation_matrix(xyz(0) * Eigen::Vector3f::UnitX());
}

void decompose_scale_rotation(const Eigen::Matrix3f& rotScaleMat, Eigen::Matrix3f& rot, Eigen::Vector3f& scale)
{
    scale = rotScaleMat.colwise().norm();
    rot = rotScaleMat.array().rowwise() / scale.transpose().array();

    if (rot.determinant() < 0.f) {
        scale *= -1.f;
        rot *= -1.f;
    }
}

}


#pragma once

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

#include <util/util.hpp>

enum ActivationFunction { None = 0, Tanh = 1, ReLU = 2, LeakyReLU = 3 };

class Layer {
public:
  Layer(Eigen::MatrixXd weight, Eigen::MatrixXd bias,
        ActivationFunction act_fn);
  virtual ~Layer();
  Eigen::MatrixXd GetOutput(const Eigen::MatrixXd &input);
  int GetNumInput() { return num_input_; }
  int GetNumOutput() { return num_output_; }
  Eigen::MatrixXd GetWeight() { return weight_; }
  Eigen::MatrixXd GetBias() { return bias_; }
  ActivationFunction GetActivationFunction() { return act_fn_; }

private:
  Eigen::MatrixXd weight_;
  Eigen::MatrixXd bias_;
  int num_input_;
  int num_output_;
  ActivationFunction act_fn_;
};

class MLPModel {
public:
  MLPModel(std::vector<Layer> layers);
  MLPModel(const YAML::Node &node);

  virtual ~MLPModel();

  std::vector<Layer> GetLayers() { return layers_; }
  Eigen::MatrixXd GetOutput(const Eigen::MatrixXd &input);
  Eigen::MatrixXd GetOutput(const Eigen::MatrixXd &input, int idx);

  int GetNumInput() { return num_input_; }
  int GetNumOutput() { return num_output_; }
  void PrintInfo();

private:
  void Initialize_(std::vector<Layer> layers);
  int num_input_;
  int num_output_;
  int num_layer_;
  std::vector<Layer> layers_;
};


//
//  LR.hpp
//  LR-x
//
//  Created by zhuangqh on 2017/6/16.
//  Copyright © 2017年 zhuangqh. All rights reserved.
//

#pragma once

#include <Eigen/Core>

namespace LR {

  using Eigen::VectorXd;
  using Eigen::MatrixXd;

  class LogisticRegression {
  private:
    double alpha;
    double lambda;
    double epsilon;
    int maxIter;

    VectorXd theta;

    long m;
    long n;

    bool verbose;

    void regularize(VectorXd &);
  public:
    LogisticRegression(double alpha, int maxIter, double lambda, bool verbose=false, double epsilon=1e-5)
        : alpha(alpha), maxIter(maxIter), lambda(lambda), epsilon(epsilon), verbose(verbose) {}

    void fit_vec(std::pair<MatrixXd, VectorXd>);

    void fit_naive(std::pair<MatrixXd, VectorXd>);

    void fit_parallel(std::pair<MatrixXd, VectorXd>);

    VectorXd predict(MatrixXd);
  };

}


// A simple quickref for Eigen. Add anything that's missing.
// Main author: Keir Mierle

#include <Eigen/Dense>

Matrix<double, 3, 3> A;               // Fixed rows and cols. Same as Matrix3d.
Matrix<double, 3, Dynamic> B;         // Fixed rows, dynamic cols.
Matrix<double, Dynamic, Dynamic> C;   // Full dynamic. Same as MatrixXd.
Matrix<double, 3, 3, RowMajor> E;     // Row major; default is column-major.
Matrix3f P, Q, R;                     // 3x3 float matrix.
Vector3f x, y, z;                     // 3x1 float matrix.
RowVector3f a, b, c;                  // 1x3 float matrix.
VectorXd v;                           // Dynamic column vector of doubles
double s;

// Basic usage
// Eigen          // Matlab           // comments
x.size()          // length(x)        // vector size
C.rows()          // size(C,1)        // number of rows
C.cols()          // size(C,2)        // number of columns
x(i)              // x(i+1)           // Matlab is 1-based
C(i,j)            // C(i+1,j+1)       //

A.resize(4, 4);   // Runtime error if assertions are on.
B.resize(4, 9);   // Runtime error if assertions are on.
A.resize(3, 3);   // Ok; size didn't change.
B.resize(3, 9);   // Ok; only dynamic cols changed.

A << 1, 2, 3,     // Initialize A. The elements can also be
     4, 5, 6,     // matrices, which are stacked along cols
     7, 8, 9;     // and then the rows are stacked.
B << A, A, A;     // B is three horizontally stacked A's.
A.fill(10);       // Fill A with all 10's.

// Eigen                            // Matlab
MatrixXd::Identity(rows,cols)       // eye(rows,cols)
C.setIdentity(rows,cols)            // C = eye(rows,cols)
MatrixXd::Zero(rows,cols)           // zeros(rows,cols)
C.setZero(rows,cols)                // C = ones(rows,cols)
MatrixXd::Ones(rows,cols)           // ones(rows,cols)
C.setOnes(rows,cols)                // C = ones(rows,cols)
MatrixXd::Random(rows,cols)         // rand(rows,cols)*2-1        // MatrixXd::Random returns uniform random numbers in (-1, 1).
C.setRandom(rows,cols)              // C = rand(rows,cols)*2-1
VectorXd::LinSpaced(size,low,high)   // linspace(low,high,size)'
v.setLinSpaced(size,low,high)        // v = linspace(low,high,size)'

// Matrix slicing and blocks. All expressions listed here are read/write.
// Templated size versions are faster. Note that Matlab is 1-based (a size N
// vector is x(1)...x(N)).
// Eigen                           // Matlab
x.head(n)                          // x(1:n)
x.head<n>()                        // x(1:n)
x.tail(n)                          // x(end - n + 1: end)
x.tail<n>()                        // x(end - n + 1: end)
x.segment(i, n)                    // x(i+1 : i+n)
x.segment<n>(i)                    // x(i+1 : i+n)
P.block(i, j, rows, cols)          // P(i+1 : i+rows, j+1 : j+cols)
P.block<rows, cols>(i, j)          // P(i+1 : i+rows, j+1 : j+cols)
P.row(i)                           // P(i+1, :)
P.col(j)                           // P(:, j+1)
P.leftCols<cols>()                 // P(:, 1:cols)
P.leftCols(cols)                   // P(:, 1:cols)
P.middleCols<cols>(j)              // P(:, j+1:j+cols)
P.middleCols(j, cols)              // P(:, j+1:j+cols)
P.rightCols<cols>()                // P(:, end-cols+1:end)
P.rightCols(cols)                  // P(:, end-cols+1:end)
P.topRows<rows>()                  // P(1:rows, :)
P.topRows(rows)                    // P(1:rows, :)
P.middleRows<rows>(i)              // P(:, i+1:i+rows)
P.middleRows(i, rows)              // P(:, i+1:i+rows)
P.bottomRows<rows>()               // P(:, end-rows+1:end)
P.bottomRows(rows)                 // P(:, end-rows+1:end)
P.topLeftCorner(rows, cols)        // P(1:rows, 1:cols)
P.topRightCorner(rows, cols)       // P(1:rows, end-cols+1:end)
P.bottomLeftCorner(rows, cols)     // P(end-rows+1:end, 1:cols)
P.bottomRightCorner(rows, cols)    // P(end-rows+1:end, end-cols+1:end)
P.topLeftCorner<rows,cols>()       // P(1:rows, 1:cols)
P.topRightCorner<rows,cols>()      // P(1:rows, end-cols+1:end)
P.bottomLeftCorner<rows,cols>()    // P(end-rows+1:end, 1:cols)
P.bottomRightCorner<rows,cols>()   // P(end-rows+1:end, end-cols+1:end)

// Of particular note is Eigen's swap function which is highly optimized.
// Eigen                           // Matlab
R.row(i) = P.col(j);               // R(i, :) = P(:, i)
R.col(j1).swap(mat1.col(j2));      // R(:, [j1 j2]) = R(:, [j2, j1])

// Views, transpose, etc; all read-write except for .adjoint().
// Eigen                           // Matlab
R.adjoint()                        // R'
R.transpose()                      // R.' or conj(R')
R.diagonal()                       // diag(R)
x.asDiagonal()                     // diag(x)
R.transpose().colwise().reverse(); // rot90(R)
R.conjugate()                      // conj(R)

// All the same as Matlab, but matlab doesn't have *= style operators.
// Matrix-vector.  Matrix-matrix.   Matrix-scalar.
y  = M*x;          R  = P*Q;        R  = P*s;
a  = b*M;          R  = P - Q;      R  = s*P;
a *= M;            R  = P + Q;      R  = P/s;
                   R *= Q;          R  = s*P;
                   R += Q;          R *= s;
                   R -= Q;          R /= s;

// Vectorized operations on each element independently
// Eigen                  // Matlab
R = P.cwiseProduct(Q);    // R = P .* Q
R = P.array() * s.array();// R = P .* s
R = P.cwiseQuotient(Q);   // R = P ./ Q
R = P.array() / Q.array();// R = P ./ Q
R = P.array() + s.array();// R = P + s
R = P.array() - s.array();// R = P - s
R.array() += s;           // R = R + s
R.array() -= s;           // R = R - s
R.array() < Q.array();    // R < Q
R.array() <= Q.array();   // R <= Q
R.cwiseInverse();         // 1 ./ P
R.array().inverse();      // 1 ./ P
R.array().sin()           // sin(P)
R.array().cos()           // cos(P)
R.array().pow(s)          // P .^ s
R.array().square()        // P .^ 2
R.array().cube()          // P .^ 3
R.cwiseSqrt()             // sqrt(P)
R.array().sqrt()          // sqrt(P)
R.array().exp()           // exp(P)
R.array().log()           // log(P)
R.cwiseMax(P)             // max(R, P)
R.array().max(P.array())  // max(R, P)
R.cwiseMin(P)             // min(R, P)
R.array().min(P.array())  // min(R, P)
R.cwiseAbs()              // abs(P)
R.array().abs()           // abs(P)
R.cwiseAbs2()             // abs(P.^2)
R.array().abs2()          // abs(P.^2)
(R.array() < s).select(P,Q);  // (R < s ? P : Q)

// Reductions.
int r, c;
// Eigen                  // Matlab
R.minCoeff()              // min(R(:))
R.maxCoeff()              // max(R(:))
s = R.minCoeff(&r, &c)    // [s, i] = min(R(:)); [r, c] = ind2sub(size(R), i);
s = R.maxCoeff(&r, &c)    // [s, i] = max(R(:)); [r, c] = ind2sub(size(R), i);
R.sum()                   // sum(R(:))
R.colwise().sum()         // sum(R)
R.rowwise().sum()         // sum(R, 2) or sum(R')'
R.prod()                  // prod(R(:))
R.colwise().prod()        // prod(R)
R.rowwise().prod()        // prod(R, 2) or prod(R')'
R.trace()                 // trace(R)
R.all()                   // all(R(:))
R.colwise().all()         // all(R)
R.rowwise().all()         // all(R, 2)
R.any()                   // any(R(:))
R.colwise().any()         // any(R)
R.rowwise().any()         // any(R, 2)

// Dot products, norms, etc.
// Eigen                  // Matlab
x.norm()                  // norm(x).    Note that norm(R) doesn't work in Eigen.
x.squaredNorm()           // dot(x, x)   Note the equivalence is not true for complex
x.dot(y)                  // dot(x, y)
x.cross(y)                // cross(x, y) Requires #include <Eigen/Geometry>

//// Type conversion
// Eigen                           // Matlab
A.cast<double>();                  // double(A)
A.cast<float>();                   // single(A)
A.cast<int>();                     // int32(A)
A.real();                          // real(A)
A.imag();                          // imag(A)
// if the original type equals destination type, no work is done

// Note that for most operations Eigen requires all operands to have the same type:
MatrixXf F = MatrixXf::Zero(3,3);
A += F;                // illegal in Eigen. In Matlab A = A+F is allowed
A += F.cast<double>(); // F converted to double and then added (generally, conversion happens on-the-fly)

// Eigen can map existing memory into Eigen matrices.
float array[3];
Vector3f::Map(array).fill(10);            // create a temporary Map over array and sets entries to 10
int data[4] = {1, 2, 3, 4};
Matrix2i mat2x2(data);                    // copies data into mat2x2
Matrix2i::Map(data) = 2*mat2x2;           // overwrite elements of data with 2*mat2x2
MatrixXi::Map(data, 2, 2) += mat2x2;      // adds mat2x2 to elements of data (alternative syntax if size is not know at compile time)

// Solve Ax = b. Result stored in x. Matlab: x = A \ b.
x = A.ldlt().solve(b));  // A sym. p.s.d.    #include <Eigen/Cholesky>
x = A.llt() .solve(b));  // A sym. p.d.      #include <Eigen/Cholesky>
x = A.lu()  .solve(b));  // Stable and fast. #include <Eigen/LU>
x = A.qr()  .solve(b));  // No pivoting.     #include <Eigen/QR>
x = A.svd() .solve(b));  // Stable, slowest. #include <Eigen/SVD>
// .ldlt() -> .matrixL() and .matrixD()
// .llt()  -> .matrixL()
// .lu()   -> .matrixL() and .matrixU()
// .qr()   -> .matrixQ() and .matrixR()
// .svd()  -> .matrixU(), .singularValues(), and .matrixV()

// Eigenvalue problems
// Eigen                          // Matlab
A.eigenvalues();                  // eig(A);
EigenSolver<Matrix3d> eig(A);     // [vec val] = eig(A)
eig.eigenvalues();                // diag(val)
eig.eigenvectors();               // vec
// For self-adjoint matrices use SelfAdjointEigenSolver<>


/*
Copyright (C) 2015-present CompatibL

Performance test results and finance-specific examples are available at:

http://www.tapescript.org

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.
*/

#ifndef cl_tape_examples_impl_polynomial_regression_hpp
#define cl_tape_examples_impl_polynomial_regression_hpp

#define CL_BASE_SERIALIZER_OPEN
#include <cl/tape/tape.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace cl
{
    // Class for polynomial regression calculation.
    class polynomial_regression
    {
    private:
        // Order of polynomial regression.
        const int order_;
        // Size of matrixes m = order + 1.
        const int m_;

        // Input data vectors.
        const tobject data_x_;
        const tobject data_y_;

        // Intermediate regression struuctures:
        // Vectors of powers of x from 0 to 2 * (order + 1).
        std::vector<tobject> x_power_;
        // Matrix X * X^T
        std::vector<std::vector<tobject>> mat_X_XT_;
        // Inverse matrix (X * X^T)^-1
        std::vector<std::vector<tobject>> mat_X_XT_inv_;
        // Vector X^T * y.
        std::vector<tobject> vec_XT_y_;

        // Regression coefficients and their derivatives.
        std::vector<tobject> coef_;

        // Status of calculation.
        bool is_coef_calculated;
        bool is_estim_calculated;

    public:
        // Construct using provided data and order of polynomial regression.
        polynomial_regression(tobject& x, tobject& y, int order) : data_x_(x), data_y_(y), order_(order), m_(order + 1)
        {
            is_coef_calculated = false;
            is_estim_calculated = false;
        }

        // Calculate polynomial regression coefficients.
        std::vector<tobject> calculate_coefficients()
        {
            // Create vectors of powers of x from 0 to 2m.
            x_power_.resize(2 * m_);
            x_power_[0] = 1 + data_x_ - data_x_;
            std::vector<tobject> x_power_sum_vec(2 * m_);
            x_power_sum_vec[0] = tapescript::sum_vec(x_power_[0]);
            for (int i = 1; i < 2 * m_; i++)
            {
                x_power_[i] = x_power_[i - 1] * data_x_;
                x_power_sum_vec[i] = tapescript::sum_vec(x_power_[i]);
            }

            // Calculate matrix X * X^T.
            mat_X_XT_.resize(m_);
            for (int i = 0; i < m_; i++)
                mat_X_XT_[i].resize(m_);

            // Calculate lower triangular matrix elements.
            for (int i = 0; i < m_; i++)
                for (int j = 0; j <= i; j++)
                    mat_X_XT_[i][j] = x_power_sum_vec[i + j];

            // Calculate the rest of matrix elements using symmetry.
            for (int i = 0; i < m_; i++)
                for (int j = i + 1; j < m_; j++)
                    mat_X_XT_[i][j] = mat_X_XT_[j][i];

            // Calculate inverse matrix (X * X^T)^-1.
            mat_X_XT_inv_ = invert_sym_matrix_boost(mat_X_XT_);

            // Calculate vector X^T * y.
            vec_XT_y_.resize(m_);
            for (int i = 0; i < m_; i++)
                vec_XT_y_[i] = tapescript::sum_vec(x_power_[i] * data_y_);

            // Calculate regression coefficients.
            coef_.resize(m_);
            for (int i = 0; i < m_; i++)
            {
                coef_[i] = 0;
                for (int j = 0; j < m_; j++)
                    coef_[i] += mat_X_XT_inv_[i][j] * vec_XT_y_[j];
            }

            is_coef_calculated = true;
            return coef_;
        }

        // Calculate polynomial regression coefficients.
        tobject calculate_estimation()
        {
            if (!is_coef_calculated)
                throw std::runtime_error("Regression coefficients are not calculated.");
            tobject estim_y = tobject(0);
            for (int i = 0; i < m_; i++)
                estim_y += coef_[i] * x_power_[i];
            is_estim_calculated = true;
            return estim_y;
        }

        // Calculate polynomial regression coefficients derivatives w.r.t data points (y).
        std::vector<std::vector<double>> calculate_coefficients_derivatives()
        {
            if (!is_coef_calculated)
                throw std::runtime_error("Regression coefficients are not calculated.");
            std::vector<std::vector<double>> d_coef_d_y(m_);
            std::vector<tvalue> x_power_tvalue(m_);
            for (int i = 0; i < m_; i++)
                x_power_tvalue[i] = (tvalue)x_power_[i];
            int npoints = x_power_tvalue[0].size();
            for (int i = 0; i < m_; i++)
            {
                d_coef_d_y[i].resize(npoints);
                for (int j = 0; j < npoints; j++)
                    for (int k = 0; k < m_; k++)
                        d_coef_d_y[i][j] += ((tvalue)mat_X_XT_inv_[i][k]).scalar_value_ * x_power_tvalue[k].element_at(j);
            }
            return d_coef_d_y;
        }

        // Calculate polynomial regression coefficients derivatives w.r.t data points (y).
        std::vector<std::vector<double>> calculate_estimation_derivatives()
        {
            std::vector<std::vector<double>> d_coef_d_y = calculate_coefficients_derivatives();
            std::vector<tvalue> x_power_tvalue(m_);
            for (int i = 0; i < m_; i++)
                x_power_tvalue[i] = (tvalue)x_power_[i];
            int npoints = x_power_tvalue[0].size();
            std::vector<std::vector<double>> estim_y_deriv(npoints);
            for (int i = 0; i < npoints; i++)
            {
                estim_y_deriv[i].resize(npoints);
                for (int j = 0; j < npoints; j++)
                    for (int k = 0; k < m_; k++)
                        estim_y_deriv[i][j] += d_coef_d_y[k][i] * x_power_tvalue[k].element_at(j);
            }
            return estim_y_deriv;
        }

        // Invert symmetric matrix using Cholesky decomposition from the Boost library.
        // This template method works with both element_type = tobject and element_type = tdouble.
        template<typename element_type>
        static std::vector<std::vector<element_type>> invert_sym_matrix_boost(const std::vector<std::vector<element_type>>& mat)
        {
            // Matrix size.
            int m = mat.size();
            boost::numeric::ublas::matrix<element_type> input(m, m);
            for (int i = 0; i < m; i++)
                for (int j = 0; j < m; j++)
                    input.operator ()(i, j) = mat[i][j];
            // Create a permutation matrix for the LU-factorization
            boost::numeric::ublas::permutation_matrix<std::size_t> pm(m);
            // Perform LU-factorization
            if(boost::numeric::ublas::lu_factorize(input, pm))
                throw std::runtime_error("Singular matrix");
            // Create identity matrix of for inverse
            boost::numeric::ublas::matrix<element_type> inverse(m, m);
            inverse.assign(boost::numeric::ublas::identity_matrix<element_type>(m));
            boost::numeric::ublas::lu_substitute(input, pm, inverse);
            std::vector<std::vector<element_type>> mat_inv = mat;
            for (int i = 0; i < m; i++)
                for (int j = 0; j < m; j++)
                    mat_inv[i][j] = inverse(i, j);
            return mat_inv;
        }
    };
}

#endif // cl_tape_examples_impl_polynomial_regression_hpp


/** @file
 * @brief NPDE ZienkiewiczZhuEstimator
 * @author Erick Schulz
 * @date 25/07/2019
 * @copyright Developed at ETH Zurich
 */

#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>

#include "zienkiewiczzhuestimator.h"
// Eigen includes
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
// Lehrfem++ includes
#include <lf/assemble/assemble.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

using namespace ZienkiewiczZhuEstimator;

int main(int /*argc*/, const char ** /*argv*/) {
  std::cout << "\n" << std::endl;
  std::cout << "PROBLEM - ZienkiewiczZhuEstimator " << std::endl;
  progress_bar progress{std::clog, 70u, "Computing"};
  double progress_pourcentage;
  // Tools and data
  int N_meshes = 4;             // num. of meshes
  Eigen::VectorXd approx_sol;   // basis ceoff expansion of scalar approx sol
  Eigen::VectorXd approx_grad;  // basis ceoff expansion of approx grad
  Eigen::VectorXd L2errors(N_meshes);     // L2 errors of scalar approx sol
  Eigen::VectorXd H1errors(N_meshes);     // H1 errors of scalar approx sol
  Eigen::VectorXd errors_grad(N_meshes);  // deviation of grad (delta error)
  Eigen::VectorXd errors_diff(N_meshes);  // error difference (epsilon error)
  Eigen::VectorXd mesh_sizes(N_meshes);
  Eigen::VectorXd interpolated_uExact;
  std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space_p;
  std::shared_ptr<const lf::mesh::Mesh> mesh_p;

  // Analytic solution
  auto uExact = [](coord_t x) -> double {
    return (0.2 / (M_PI * M_PI)) * sin(M_PI * x[0]) * sin(2 * M_PI * x[1]);
  };
  lf::mesh::utils::MeshFunctionGlobal mf_uExact{uExact};
  auto grad_uExact = [](coord_t x) -> Eigen::Vector2d {
    return Eigen::Vector2d(
        (0.2 / M_PI) * cos(M_PI * x[0]) * sin(2 * M_PI * x[1]),
        (0.4 / M_PI) * sin(M_PI * x[0]) * cos(2 * M_PI * x[1]));
  };
  lf::mesh::utils::MeshFunctionGlobal mf_grad_uExact{grad_uExact};

  for (int i = 0; i < N_meshes; i++) {  // for each mesh
    std::string idx_str = std::to_string(i);
    // Load mesh into a Lehrfem++ object
    auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
    const lf::io::GmshReader reader(
        std::move(mesh_factory),
        CURRENT_SOURCE_DIR "/../meshes/unitsquare" + idx_str + ".msh");
    mesh_p = reader.mesh();
    mesh_sizes[i] = getMeshSize(mesh_p);
    fe_space_p =
        std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
    // Obtain reference to scalar dofh
    const lf::assemble::DofHandler &dofh{fe_space_p->LocGlobMap()};
    // Produce a dof handler for the vector-valued finite element space
    lf::assemble::UniformFEDofHandler vec_dofh(
        mesh_p, {{lf::base::RefEl::kPoint(), 2},
                 {lf::base::RefEl::kSegment(), 0},
                 {lf::base::RefEl::kTria(), 0},
                 {lf::base::RefEl::kQuad(), 0}});

    LF_VERIFY_MSG(2 * dofh.NumDofs() == vec_dofh.NumDofs(),
                  "Number of degrees of freedom mismatch!");

    // Solve Poisson BVP with essential BCs
    approx_sol = solveBVP(fe_space_p);
    // Compute L2 and H1 errors
    auto mf_approx_sol = lf::fe::MeshFunctionFE(fe_space_p, approx_sol);
    L2errors[i] = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh_p, lf::uscalfe::squaredNorm(mf_uExact - mf_approx_sol), 2));
    auto mf_approx_grad_sol =
        lf::fe::MeshFunctionGradFE(fe_space_p, approx_sol);
    H1errors[i] = std::sqrt(lf::fe::IntegrateMeshFunction(
        *mesh_p, lf::uscalfe::squaredNorm(mf_grad_uExact - mf_approx_grad_sol),
        2));

    // Solve gradient VP and compute L2 deviation
    approx_grad = solveGradVP(fe_space_p, approx_sol, vec_dofh);
    // approx_grad = computeLumpedProjection(dofh, approx_sol, vec_dofh);
    // Compute deviation error (delta error)
    errors_grad[i] =
        computeL2Deviation(dofh, approx_sol, vec_dofh, approx_grad);

    // Compute difference error (epsilon error)
    errors_diff[i] = std::abs(H1errors[i] - errors_grad[i]);

    progress_pourcentage = ((double)i + 1.0) / N_meshes * 100.0;
    progress.write(progress_pourcentage / 100.0);
  }

  // Computing rates of convergence of approx solutions to BVP and gradient VP
  double ratesL2[N_meshes - 1];
  double ratesH1[N_meshes - 1];
  double rates_grad[N_meshes - 1];
  double rates_diff[N_meshes - 1];
  double log_denum;
  for (int k = 0; k < N_meshes - 1; k++) {
    log_denum = log(mesh_sizes[k] / mesh_sizes[k + 1]);
    ratesL2[k] = log(L2errors[k] / L2errors[k + 1]) / log_denum;
    ratesH1[k] = log(H1errors[k] / H1errors[k + 1]) / log_denum;
    rates_grad[k] = log(errors_grad[k] / errors_grad[k + 1]) / log_denum;
    rates_diff[k] = log(errors_diff[k] / errors_diff[k + 1]) / log_denum;
  }

  std::cout << "\n Done." << std::endl;
  std::cout << "Outputing results." << std::endl;

  std::cout << "\n" << std::endl;
  std::cout << "*********************************************************"
            << std::endl;
  std::cout << "      ERRORS AND CONVERGENCE RATES FOR PROBLEM        "
            << std::endl;
  std::cout << "             ZIENKIEWICZ ZHU ESTIMATOR                   "
            << std::endl;
  std::cout << "*********************************************************"
            << std::endl;

  std::cout << "\n" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  std::cout << "    L2 errors of approximate scalar solutions to the     "
            << std::endl;
  std::cout << "   Poisson Problem with essential boundary conditions    "
            << std::endl;
  std::cout << "--------------------- RESULTS ---------------------------"
            << std::endl;
  std::cout << "Iteration"
            << "\t| L2 error"
            << "\t\t| rates" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  for (int k = 0; k < N_meshes; k++) {
    std::cout << k << "\t"
              << "\t|" << L2errors[k];
    if (k > 0) {
      std::cout << "\t \t|" << ratesL2[k - 1];
    }
    std::cout << "\n";
  }
  std::cout << "---------------------------------------------------------"
            << std::endl;

  std::cout << "\n" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  std::cout << "    H1 errors of approximate scalar solutions to the     "
            << std::endl;
  std::cout << "   Poisson Problem with essential boundary conditions    "
            << std::endl;
  std::cout << "--------------------- RESULTS ---------------------------"
            << std::endl;
  std::cout << "Iteration"
            << "\t| H1 error"
            << "\t\t| rates" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  for (int k = 0; k < N_meshes; k++) {
    std::cout << k << "\t"
              << "\t|" << H1errors[k];
    if (k > 0) {
      std::cout << "\t \t|" << ratesH1[k - 1];
    }
    std::cout << "\n";
  }
  std::cout << "---------------------------------------------------------"
            << std::endl;

  std::cout << "\n" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  std::cout << "      Convergence rates of the approximate gradients     "
            << std::endl;
  std::cout << "                     (delta error)                       "
            << std::endl;
  std::cout << "--------------------- RESULTS ---------------------------"
            << std::endl;
  std::cout << "Iteration"
            << "\t| delta"
            << "\t\t| rates" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  for (int k = 0; k < N_meshes; k++) {
    std::cout << k << "\t"
              << "\t|" << errors_grad[k];
    if (k > 0) {
      std::cout << "\t\t|" << rates_grad[k - 1];
    }
    std::cout << "\n";
  }
  std::cout << "---------------------------------------------------------"
            << std::endl;

  std::cout << "\n" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  std::cout << "      Difference error between the H1 error of the       "
            << std::endl;
  std::cout << "   approximate solution to the BVP and the deviation     "
            << std::endl;
  std::cout << "                    (epsilon error)                      "
            << std::endl;
  std::cout << "--------------------- RESULTS ---------------------------"
            << std::endl;
  std::cout << "Iteration"
            << "\t| epsilon"
            << "\t\t| rates" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  for (int k = 0; k < N_meshes; k++) {
    std::cout << k << "\t"
              << "\t|" << errors_diff[k];
    if (k > 0) {
      std::cout << "\t \t|" << rates_diff[k - 1];
    }
    std::cout << "\n";
  }
  std::cout << "---------------------------------------------------------"
            << std::endl;

  // Output results for epsilon and delta errors to csv file
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::string errors_file_name_delta = "delta_errors.csv";
  std::ofstream file_delta(errors_file_name_delta.c_str());
  if (file_delta.is_open()) {
    file_delta << errors_grad.format(CSVFormat);
  }
  std::string errors_file_name_epsilon = "epsilon_errors.csv";
  std::ofstream file_epsilon(errors_file_name_epsilon.c_str());
  if (file_epsilon.is_open()) {
    file_epsilon << errors_diff.format(CSVFormat);
  }

  std::cout << "\n The delta and epsilon errors were written to:" << std::endl;
  std::cout << ">> delta_errors.csv and epsilon_errors.csv\n" << std::endl;

  // Save approximate solution in VTK format
  // Output results to vtk file
  const lf::assemble::DofHandler &dofh{fe_space_p->LocGlobMap()};
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());
  lf::io::VtkWriter vtk_writer(mesh_p, "ZienkiewiczZhuEstimator_solution.vtk");
  // Write nodal data taking the values of the discrete solution at the
  // vertices
  auto nodal_data = lf::mesh::utils::make_CodimMeshDataSet<double>(mesh_p, 2);
  for (int global_idx = 0; global_idx < N_dofs; global_idx++) {
    nodal_data->operator()(dofh.Entity(global_idx)) = approx_sol[global_idx];
  };
  vtk_writer.WritePointData("ZienkiewiczZhuEstimator_solution", *nodal_data);

  std::cout << "\n The ZienkiewiczZhuEstimator_solution was written to:"
            << std::endl;
  std::cout << ">> ZienkiewiczZhuEstimator_solution.vtk\n" << std::endl;

}  // main


// Modified from NetKet source for YANNQ Project
// <Chae-Yeun Park>(chae.yeun.park@gmail.com)
// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef YANNQ_ACTIVATIONS_HPP
#define YANNQ_ACTIVATIONS_HPP

#include <Eigen/Dense>
#include <complex>
#include <iostream>
#include <random>
#include <vector>

#include <Utilities/type_traits.hpp>
#include <Utilities/Utility.hpp>

#include <nlohmann/json.hpp>

namespace yannq {

/**
  Abstract class for Activations.
*/
namespace activation
{
template<typename T>
class Identity
{
public:
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	// A = Z
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		A.noalias() = Z;
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = Z
	// J = dA / dZ = I
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef /*Z*/, VectorConstRef /*A*/,
			VectorConstRef F,
			VectorRef G) const  {
		G.noalias() = F;
	}

	std::string name() const {
		return "Identity";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}
};

template<typename T>
class LnCosh
{
public:
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	// A = Lncosh(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		for (int i = 0; i < A.size(); ++i) {
			A(i) = logCosh(Z(i));
		}
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = Lncosh(Z)
	// J = dA / dZ
	// G = J * F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F,
			VectorRef G) const  {
		G.array() = F.array() * Z.array().tanh();
	}

	std::string name() const {
		return "LnCosh";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}
};

template<typename T>
class Tanh
{
public:
	using Scalar = T;
	using RealScalar = remove_complex_t<Scalar>;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

private:
	RealScalar constant_;

public:

	explicit Tanh(const RealScalar constant = 1.0) //constant*tanh(z/constant)
		: constant_{constant}
	{
	}

	// A = a*Tanh(Z/a)
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		A.array() = constant_ * (Z.array() / constant_).tanh();
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = a*Tanh(Z/a)
	// J = dA / dZ
	// G = J * F
	inline void ApplyJacobian(VectorConstRef /*Z*/, VectorConstRef A,
			VectorConstRef F,
			VectorRef G) const  {
		G.array() = F.array() * (1. - A.array() * A.array() / (constant_*constant_));
	}

	std::string name() const
	{
		return "Tanh";
	}

	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(constant_);
	}
};

template<typename T>
class WeakTanh
{
public:
	using Scalar = T;
	using RealScalar = remove_complex_t<Scalar>;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

private:

public:

	explicit WeakTanh() //x - tanh(x)/2
	{
	}

	// A = a*Tanh(Z/a)
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		A.array() = Z.array();
		A.array() -= Z.array().tanh()/2.0;
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = Tanh(Z)
	// J = dA / dZ
	// G = J * F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F,
			VectorRef G) const  {
		Eigen::Array<T, Eigen::Dynamic, 1> tanh = Z.array().tanh();
		G.array() = F.array() * (1.0 +  tanh * tanh)/2.0;
	}

	std::string name() const
	{
		return "WeakTanh";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}
};

template<typename T>
class Sigmoid
{
	static_assert(!is_complex_type<T>::value, "Sigmoid now only supports real parameters");
public:
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	// A = Sigmoid(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		A.array() = Z.array().exp();
		A.array().inverse();
		A.array() += 1.;
		A.array().inverse();
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = Sigmoid(Z)
	// J = dA / dZ
	// G = J * F
	inline void ApplyJacobian(VectorConstRef /*Z*/, VectorConstRef A,
			VectorConstRef /*F*/,
			VectorRef G) const  {
		G.array() = (1. - A.array() ) * A.array();
	}

	std::string name() const
	{
		return "Sigmoid";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}
};

template<typename T, typename Enable = void>
class ReLU;

//for complex T
template<typename T>
class ReLU<T, typename std::enable_if<is_complex_type<T>::value>::type >
{
public:
	using Scalar = T;
	using RealScalar = remove_complex_t<Scalar>;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	constexpr static RealScalar theta1_ = 3*M_PI/4;
	constexpr static RealScalar theta2_ = -M_PI/4;

	// A = ReLU(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		for (int i = 0; i < Z.size(); ++i) {
			A(i) =
				(std::arg(Z(i)) < theta1_) && (std::arg(Z(i)) > theta2_) ? Z(i) : 0.0;
		}
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = ReLU(Z)
	// J = dA / dZ
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F,
			VectorRef G) const  {
		for (int i = 0; i < Z.size(); ++i) {
			G(i) =
				(std::arg(Z(i)) < theta1_) && (std::arg(Z(i)) > theta2_) ? F(i) : 0.0;
		}
	}

	std::string name() const
	{
		return "ReLU";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}
};
//For real T
template<typename T>
class ReLU<T, typename std::enable_if<!is_complex_type<T>::value>::type >
{
public:
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	// A = ReLU(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		for (int i = 0; i < Z.size(); ++i) {
			A(i) = std::max(Z(i), T{0.0});
		}
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = ReLU(Z)
	// J = dA / dZ
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F,
			VectorRef G) const
	{
		for (int i = 0; i < Z.size(); ++i) {
			G(i) = Z(i)>0?F(i):T{0.0};
		}
	}

	std::string name() const
	{
		return "ReLU";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}
};

template<typename T, typename Enable = void>
class LeakyReLU;

//for complex T
template<typename T>
class LeakyReLU<T, typename std::enable_if<is_complex_type<T>::value>::type>
{
public:
	using Scalar = T;
	using RealScalar = remove_complex_t<Scalar>;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	constexpr static RealScalar theta1_ = 3*M_PI/4;
	constexpr static RealScalar theta2_ = -M_PI/4;
	RealScalar negativeSlope_;

	LeakyReLU(const RealScalar negativeSlope = 0.1)
		: negativeSlope_{negativeSlope}
	{
	}

	// A = LeakyReLU(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		for (int i = 0; i < Z.size(); ++i) {
			A(i) =
				(std::arg(Z(i)) < theta1_) && (std::arg(Z(i)) > theta2_) ? Z(i) : negativeSlope_*Z(i);
		}
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = LeakyReLU(Z)
	// J = dA / dZ
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F,
			VectorRef G) const
	{
		for (int i = 0; i < Z.size(); ++i) {
			G(i) =
				(std::arg(Z(i)) < theta1_) && (std::arg(Z(i)) > theta2_) ? F(i) : negativeSlope_*F(i);
		}
	}

	std::string name() const
	{
		return "LeakyReLU";
	}

	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(negativeSlope_);
	}
};

//For real T
template<typename T>
class LeakyReLU<T, typename std::enable_if<!is_complex_type<T>::value>::type>
{
public:
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	Scalar negativeSlope_;

	LeakyReLU(const Scalar negativeSlope = 0.1)
		: negativeSlope_{negativeSlope}
	{
	}
	// A = LeakyReLU(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const  {
		for (int i = 0; i < Z.size(); ++i) {
			A(i) = Z(i)>0?Z(i):T{negativeSlope_*Z(i)};
		}
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = LeakyReLU(Z)
	// J = dA / dZ
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F,
			VectorRef G) const
	{
		for (int i = 0; i < Z.size(); ++i) {
			G(i) = Z(i)>0?F(i):T{negativeSlope_*F(i)};
		}
	}

	std::string name() const
	{
		return "LeakyReLU";
	}
	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(negativeSlope_);
	}
};

//For real T
template<typename T>
class HardTanh
{
public:
	static_assert(!is_complex_type<T>::value, "T must be a real type for HardTanh");
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	HardTanh()
	{
	}

	// A = hardtanh(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const
	{
		using std::abs;
		for (int i = 0; i < Z.size(); ++i)
		{
			A(i) = abs(Z(i))>1.0?copysign(1.0, Z(i)):Z(i);
		}
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = hardtanh(Z)
	// J = dA / dZ
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F, VectorRef G) const
	{
		using std::abs;
		for (int i = 0; i < Z.size(); ++i) {
			G(i) = abs(Z(i))>1.0?0.0:F(i);
		}
	}

	std::string name() const
	{
		return "HardTanh";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}
};

template<typename T>
class SoftShrink
{
public:
	static_assert(!is_complex_type<T>::value, "T must be a real type for SoftShrink");
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	SoftShrink()
	{
	}

	// A = softshrink(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const
	{
		using std::abs;
		for (int i = 0; i < Z.size(); ++i)
		{
			A(i) = abs(Z(i))<1.0?0.0:(Z(i)-copysign(1.0,Z(i)));
		}
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = softshrink(Z)
	// J = dA / dZ
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F, VectorRef G) const
	{
		using std::abs;
		for (int i = 0; i < Z.size(); ++i) {
			G(i) = abs(Z(i))<1.0?0.0:F(i);
		}
	}

	std::string name() const
	{
		return "SoftShrink";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}

};

template<typename T>
class LeakyHardTanh
{
public:
	static_assert(!is_complex_type<T>::value, "T must be a real type for LeakyHardTanh");
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	Scalar outsideSlope_;

	LeakyHardTanh(Scalar outsideSlope = 0.01)
		: outsideSlope_{outsideSlope}
	{
	}

	// A = leakyhardtanh(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const
	{
		using std::abs;
		for (int i = 0; i < Z.size(); ++i)
		{
			T val = abs(Z(i))>1.0?copysign(1.0, Z(i)):Z(i);
			val += abs(Z(i))<1.0?0.0:outsideSlope_*(Z(i)-copysign(1.0,Z(i)));
			A(i) = val;
		}
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = leakyhardtanh(Z)
	// J = dA / dZ
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F, VectorRef G) const
	{
		using std::abs;
		for (int i = 0; i < Z.size(); ++i) {
			G(i) = abs(Z(i))>1.0?outsideSlope_*F(i):F(i);
		}
	}

	std::string name() const
	{
		return "LeakyHardTanh";
	}

	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(outsideSlope_);
	}
};

template<typename T>
class SoftSign
{
public:
	static_assert(!is_complex_type<T>::value, "T must be a real type for SoftSign");
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	SoftSign()
	{
	}

	// A = softsign(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const
	{
		A.array() = Z.array()/(Z.array().abs()+1.0);
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = softsign(Z)
	// J = dA / dZ = I
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F, VectorRef G) const
	{
		G.array() = (1.0+Z.array().abs()).square().inverse()*F.array();
	}

	std::string name() const
	{
		return "SoftSign";
	}

	template<class Archive>
	void serialize(Archive& /*ar*/)
	{
	}
};

template<typename T>
class Cos
{
private:
	T a_;
public:
	static_assert(!is_complex_type<T>::value, "T must be a real type for Cos");
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	Cos(T a = 1.0)
		: a_{a}
	{
	}

	// A = cos(Z)
	inline void operator()(VectorConstRef Z, VectorRef A) const
	{
		A.array() = a_*cos(Z.array()*M_PI/a_);
	}

	// Apply the (derivative of activation function) matrix J to a vector F
	// A = a*cos(\pi Z/a)
	// J = dA / dZ = \pi sin(\pi Z/ a)
	// G = J * F = F
	inline void ApplyJacobian(VectorConstRef Z, VectorConstRef /*A*/,
			VectorConstRef F, VectorRef G) const
	{
		G.array() = -M_PI*sin(M_PI*Z.array()/a_)*F.array();
	}

	std::string name() const
	{
		return "Cos";
	}

	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(a_);
	}
};
}//namsepace activation

}// namespace yannq

#endif