#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;

}


//
//  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;
    }
}


#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;
}

}


#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


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


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

using Eigen::MatrixXd;
using namespace Eigen;
using namespace std;

int main()
{
  string name = "Juan pablo mallarino \n github.com";
  int n = 0, m = 0;
  // Bloque de codigo que lee el archivo y determina filas y columnas
  MatrixXd A(n,m);
  // Bloque de codigo que carga el archivo a la matriz
  MatrixXd m = MatrixXd::Random(10,10); // # reales PSEUDO-aleatorios entre -1 y 1
  cout << "Checkpoint 1: \n" << m << endl;
  cout << "Determinant of m:\n" << m.determinant() << endl;
  MatrixXd u_m = m.inverse();
  cout << "Inverse of m:\n" << u_m << endl;
  cout << "Test de valicez m^-1*m:\n" << u_m*m << endl;
  // No existe tal cosa como exactitud en computacion -> decimales
  // el cero en precision doble (double) -> 10^-15
  // -> es un reto!
  // LISP + Mathematica
  m = (m + MatrixXd::Constant(10,10,1.2)) * 50; // # reales aleatorios entre 10 y 110
  cout << "Checkpoint 2: m =\n" << m << endl; // endl hace flush...
  VectorXd v(10);
  v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
  cout << "Checkpoint 3: v =" << v << endl;
  cout << "m * v =" << endl << m * v << endl;
  cout << "terminando... "<<name<<endl;
  return 0;
}


#include <Eigen/Dense>

#include <iostream>
#include <iomanip>

#include <cmath>

//! \brief One step of autonomous IVP y'' + y' + y = 0, [y(0), y'(0)] = z0 using SDIRK method
//! Use SDIRK method for first order ode z' = f(z). Steps of size h.
//! \tparam StateType type of solution space y and initial data y0
//! \param[in] z0 initial data z(0)
//! \param[in] h size of the step
//! \param[in] gamma parameter
//! \return next step z1
template <class StateType>
StateType sdirkStep(const StateType & z0, double h, double gamma) {
    // TODO: implement one step of SDIRK method
}

//! \brief Solve autonomous IVP y'' + y' + y = 0, [y(0), y'(0)] = z0 using SDIRK method
//! Use SDIRK method for first order ode z' = f(z), with N equidistant steps
//! \tparam StateType type of solution space z = [y,y']! and initial data z0 = [y(0), y'(0)]
//! \param[in] z0 initial data z(0)
//! \param[in] N number of equidistant steps
//! \param[in] T final time of simulation
//! \param[in] gamma parameter
//! \return vector containing each step of z_k (y and y')
template <class StateType>
std::vector<StateType> sdirkSolve(const StateType & z0, unsigned int N, double T, double gamma) {
    // TODO: implement solution with SDIRK method
}

int main() {
    // Initial data z0 = [y(0), y'(0)]
    Eigen::Vector2d z0;
    z0 << 1,0;
    // Final time
    const double T = 10;
    // Parameter
    const double gamma = (3.+std::sqrt(3.)) / 6.;
    // Mesh sizes
    std::vector<int> N = {20, 40, 80, 160, 320, 640, 1280, 2560, 5120, 10240};
    // Exact solution (only y(t)) given z0 = [y(0), y'(0)] and t
    auto yex = [&z0] (double t) {
        return 1./3.*std::exp(-t/2.) * ( 3.*z0(0) * std::cos( std::sqrt(3.)*t/2. ) +
        std::sqrt(3.)*z0(0) * std::sin( std::sqrt(3.)*t/2. ) +
        2.*std::sqrt(3.)*z0(1) * std::sin( std::sqrt(3.)*t/2. ) );
    };

    // TODO: compute solution and output error/approximate order
}

Nummerical Methods for CSE/PS14/templates_ps14/sdirk_template.cpp


#include <unistd.h>
#include <Eigen/Dense>
#include <string>
#include <fstream>
#include "common.h"

#define TINYEXR_IMPLEMENTATION
#include "tinyexr.h"

using namespace std;

float L(float cosTheta, float rough, float f0, int s)
{
	if (rough == 0) return schlick(cosTheta, f0);
	cosTheta = std::max(cosTheta, 0.001f); // avoid issues at exact grazing

	float sinTheta = std::sqrt(1 - sqr(cosTheta));
	Vector3 incoming(sinTheta, 0, cosTheta);
	float alpha = sqr(rough);
	MicrofacetLobe lobe(alpha, f0, incoming);
	float total = 0;

	for (int i = 0; i < s; i++)
		for (int j = 0; j < s; j++)
		{
			// stratified sampling
			float r1 = (randf() + i) / s;
			float r2 = (randf() + j) / s;
			Vector3 dir;
			float weight;
			bool ok = lobe.sample(r1, r2, dir, weight);
			if (!ok) continue;
			total += weight;
		}

	total /= s * s;
	return total;
}

float T(int i, const FloatImage& img)
{
	// computing the hemispherical integral in solid angle measure:
	// I = 1/pi * int[f(cos theta) * cos theta d omega]

	// this is turned into spherical coordinates, resulting in:
	// I = 2 int[f(cos theta) * cos theta * sin theta d theta] from 0 to pi/2

	// substitute t := cos theta, obtaining
	// I = 2 int[f(t) * t dt]  from 0 to 1
	// compute the latter using trapezoid rule

	int n = img.cols();
	float total = 0;

	for (int j = 0; j < n; j++)
	{
		float t = float(j) / (n - 1);
		float w = j == 0 || j == n-1 ? 0.5f : 1;
		total += 2 * t * img(i, j) * w / (n - 1);
	}

	return total;
}

void writeExr(FloatImage& img, string filename)
{
	int m = img.rows(), n = img.cols();
	ColorImage tmp(m, n);
	for (int i = 0; i < m; i++)
		for (int j = 0; j < n; j++)
			tmp(i, j) = Color::Constant(img(i, j));
	SaveEXR((float*) &tmp(0,0), n, m, 3, 1, filename.c_str(), 0);
}

int main(int argc, char** argv)
{
	int s = 100;  // use s^2 stratified samples
	int n = 256;  // result table size n x n
	float f0 = 0; // Schlick Fresnel normal incidence

	int tmp;
	while ((tmp = getopt(argc, argv, "n:s:f:")) != -1)
	{
		switch (tmp)
		{
			case 'n': n = atoi(optarg); break;
			case 's': s = atoi(optarg); break;
			case 'f': f0 = atof(optarg); break;
		}
	}

	FloatImage Limg(n, n);

	#pragma omp parallel for
	for (int i = 0; i < n; i++)
		for (int j = 0; j < n; j++)
		{
			float rough = float(i) / (n - 1);
			float cosTheta = float(j) / (n - 1);
			Limg(i, j) = L(cosTheta, rough, f0, s);
		}

	FloatImage Timg(1, n);
	for (int i = 0; i < n; i++) Timg(0, i) = T(i, Limg);

	writeExr(Limg, "L.exr");
	writeExr(Timg, "T.exr");
	return 0;
}


/*****************************************************************************
*
* Copyright (C) 2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <iomanip>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

template<class T>
class func {
public:
  typedef T real_t;
  func(real_t t) : t_(t) {}
  real_t operator()(real_t x, real_t xc) const {
    using std::abs; using std::cos; using std::log;
    if (x < t_ && xc > 0) {
      return cos(2 * x) * log(abs(2 * sin(xc / 2) * sin((t_ + x) / 2)));
    } else if (x > t_ && xc < 0) {
      return cos(2 * x) * log(abs(2 * sin(-xc / 2) * sin((t_ + x) / 2)));
    } else {
      real_t s = abs(cos(x) - cos(t_));
      return s > 0 ? cos(2 * x) * log(s) : real_t(0);
    }
  }
  real_t result() const {
    using std::cos;
    return -boost::math::constants::pi<real_t>() * cos(2 * t_) / 2;
  }
private:
  real_t t_;
};

int main() {
  using std::abs; using std::sqrt;
  typedef double real_t;
  const real_t pi = boost::math::constants::pi<real_t>();

  boost::math::quadrature::tanh_sinh<real_t> integrator;
  real_t termination = sqrt(std::numeric_limits<real_t>::epsilon());

  real_t t = pi / 8;
  func<real_t> f(t);
  real_t error, L1;
  size_t levels;
  real_t q1 = integrator.integrate(f, 0, t, termination, &error, &L1, &levels);
  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
            << "lower part: " << q1 << std::endl
            << "estimated error: " << error << std::endl
            << "L1 * error: " << L1 * error << std::endl
            << "levels: " << levels << std::endl;
  real_t q2 = integrator.integrate(f, t, pi, termination, &error, &L1, &levels);
  std::cout << "upper part: " << q2 << std::endl
            << "estimated error: " << error << std::endl
            << "L1 * error: " << L1 * error << std::endl
            << "levels: " << levels << std::endl;
  std::cout << "result: " << (q1 + q2) << std::endl
            << "real error: " << abs(q1 + q2 - f.result()) << std::endl;
}


//
//  neuralnetwork.h
//  NeuralNetwork
//
//  Created by Nikhil Joshi on 10/15/14.
//  Copyright (c) 2014 Nikhil Joshi. All rights reserved.
//

#ifndef __NeuralNetwork__neuralnetwork__
#define __NeuralNetwork__neuralnetwork__

#define EIGEN_DEFAULT_TO_ROW_MAJOR
#include <Eigen/Dense>

#include <iostream>
#include <ctime>
#include <vector>
#include <initializer_list>

#include "utility/utility.hpp"
#include "utility/functionalitysuite.hpp"

namespace NNetwork {

    class Network {
    public:
        // constructor
        Network(std::vector<unsigned int>&& topology) {
            _topology =  Eigen::toEigen(topology);
            initialize();
        }

        Network(std::initializer_list<unsigned int>& ls)
        : Network(std::vector<unsigned int>(ls))
        {
        }

        // Destructor
        ~Network(){
            for (int i = 0; i < _numLayers; i++)
                delete _functions[i];
        }

        // member functions
        // Setters/Getters
        // network topology
        void setTopology(const std::vector<unsigned int>& topology);
        void setTopology(const std::initializer_list<unsigned int>& topology);
        std::vector<unsigned int> getTopology(void)  const      { return Eigen::toStdVector(_topology); }
        size_t numInputs(void)   const                          { return _numInputs; }
        size_t numOutputs(void)  const                          { return _numOutputs; }
        size_t numLayers(void)   const                          { return _numLayers; }
        // weights
        std::vector<std::vector<double>> getWeightsAfterLayer(const unsigned int layer) const;
        Eigen::MatrixXd getWeightsAfterLayerEigen(const unsigned int layer) const;
        void printWeights(void) const;
        void setRandomWeights(void);
        void setRandomWeightsForLayer(unsigned int layer);
        void setWeightsAfterLayer(const unsigned int layer, const std::vector<std::vector<double>>& weights);
        void setWeightsAfterLayer(const unsigned int layer, const Eigen::Ref<const Eigen::MatrixXd>& weights);

        // activation function
        void setFunctionalityAtLayer(const unsigned int layer, FunctionalitySuite& suit);
        // make it a softmax output
        void setSoftmaxAtOutputLayer(void);
        bool isSoftmaxAtOutputLayer(void)                      { return _isSoftmax; }

        // Train the network on given examples
        // Data is assumed given in standard format :
        // One row = one (training) example n input/feature columns followed by m output/label columns
        double train(const char* dataFileName, const double regularizer,
                     const bool verbose = true);
        double train(const Eigen::MatrixXd& data, const double regularizer,
                     const bool verbose = true);

        // Predict
        unsigned int predict(const std::vector<double>& input);

        // Test methods
        friend bool isValidNetworkImplementation(Network& testNet);

    private:

        Eigen::VectorXu _topology;
        std::vector<Eigen::MatrixXd> _weights;
        std::vector<FunctionalitySuite*> _functions;
        size_t _numInputs, _numOutputs, _numLayers;
        bool _isSoftmax;

        // Member functions
        // initializers
        void initialize(void);
        void scaleWeightVectors(void);
        void setupDefaultNetwork(void);

        // Process input to produce output
        void processInput(const Eigen::Ref<const Eigen::VectorXd>& input,
                          std::vector<Eigen::VectorXd>& inputs,
                          std::vector<Eigen::VectorXd>& activations);
        // Calculate loss for the given input - label pair
        double calculateLoss(const Eigen::Ref<const Eigen::MatrixXd>& data,
                             const double regularizer = 0);
        double calculateLoss(const Eigen::Ref<const Eigen::VectorXd>& input,
                             const Eigen::Ref<const Eigen::VectorXd>& label,
                             const double regularizer = 0);

        // Calculate Error Delta by back Propagation
        void unregularizedLossAndWeightCorrectionsForExample(const Eigen::Ref<const Eigen::VectorXd>& input,
                                                             const Eigen::Ref<const Eigen::VectorXd>& label,
                                                             double& loss,
                                                             std::vector<Eigen::MatrixXd>& weightCorrections);
        void lossAndWeightCorrections(const Eigen::Ref<const Eigen::MatrixXd>& data,
                                      const double regularizer,
                                      double& loss,
                                      std::vector<Eigen::MatrixXd>& weightCorrections);
    };

    // Network implementation checks
    bool isValidNetworkImplementation(Network& testNet);

    // include implementation file
#include "network.ipp"

}

#endif /* defined(__NeuralNetwork__neuralnetwork__) */