#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
#include <chrono>
#include <functional>
#include <iostream>
#include <ncurses.h>
#include <thread>
#include <vector>

#define PI 3.14159265

bool exitRequested = false;

std::thread timer_start(std::function<void(void)> func, unsigned int interval) {
  return std::thread([func, interval]() {
    while (!exitRequested) {
      auto x = std::chrono::steady_clock::now() +
               std::chrono::milliseconds(interval);
      func();
      std::this_thread::sleep_until(x);
    }
  });
}

using Point2D = Eigen::Vector2i;

struct Item {
  char label;
  Point2D center;

  Item(char label, Point2D center) : label(label), center(center){};
};

Point2D rotatePoint(Point2D start, Point2D center, int degrees) {

  float s = sin(degrees * PI / 180.0);
  float c = cos(degrees * PI / 180.0);

  float resultX = start(0) - center(0);
  float resultY = start(1) - center(1);

  // Rotation matrix is calculated here
  float dx = resultX * c - resultY * s;
  float dy = resultX * s + resultY * c;

  return Point2D{int(center(0) + dx), int(center(1) + dy)};
}

void drawLine(Point2D start, Point2D end, WINDOW *window, char symbol) {
  // Using Bresenham's line algorithm from
  // https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
  Point2D current = start;
  int dx = abs(end(0) - start(0));
  int sx = start(0) < end(0) ? 1 : -1;

  int dy = -abs(end(1) - start(1));
  int sy = start(1) < end(1) ? 1 : -1;

  int err = dx + dy;

  while (true) {
    mvwaddch(window, current(1), current(0), symbol);

    if ((current(0) == end(0)) && (current(1) == end(1)))
      break;

    int e2 = 2 * err;

    if (e2 >= dy) {
      err += dy;
      current(0) += sx;
    }

    if (e2 < dx) {
      err += dx;
      current(1) += sy;
    }
  }
}

struct RotatingSquare {
  char label;

  Point2D center;
  int size;
  float rotationSpeed;

  RotatingSquare(char label, Point2D center, int size = 1,
                 float rotationSpeed = 0.0)
      : label(label), center(center), size(size),
        rotationSpeed(rotationSpeed){};
};

template <typename T, class... Args> struct Aged {
  int birthTime;
  T item;
  Aged(T item, int birthTime = 0)
      : birthTime(birthTime), item(std::move(item)){};
};

int loop = 0;
// We'll put game state here
Eigen::Vector2i userCursor;
;
int timeSpeed = 1;

template <class T> class AgedVector {
  std::vector<Aged<T>> agedItems;
  int age;
  std::function<bool(const Aged<T> &, int)> filterFunction = nullptr;

public:
  AgedVector<T>(
      int age = 0,
      std::function<bool(const Aged<T> &, int)> filterFunction = nullptr)
      : age(age), filterFunction(filterFunction){};

  void add(T item) { agedItems.push_back(Aged<T>(item, age)); };

  void tick(int increment = 1) {
    age += increment;
    if (filterFunction != nullptr) {
      auto i = std::begin(agedItems);
      while (i != std::end(agedItems)) {
        if (!filterFunction(*i, age)) {
          i = agedItems.erase(i);
        } else {
          ++i;
        }
      }
    }
  }

  auto begin() const { return agedItems.begin(); }
  auto end() const { return agedItems.end(); }
  int getAge() { return age; }
};

bool under100Ticks(const Aged<Item> &agedItem, int time) {
  if ((time - agedItem.birthTime) > 100) {
    return false;
  }
  return true;
}

AgedVector<Item> timeoutList(0, under100Ticks);
AgedVector<RotatingSquare> squares;

void addDecaying(WINDOW *window, Point2D center) {
  timeoutList.add(Item('*', center));
}

void addSquare(WINDOW *window, Point2D center) {
  squares.add(RotatingSquare('.', center, 2, 10.0));
}

void drawSquare(WINDOW *window, const RotatingSquare &square, int angle) {
  int leftX = square.center(0) - square.size;
  int rightX = square.center(0) + square.size;
  int topY = square.center(1) - square.size;
  int bottomY = square.center(1) + square.size;

  // I don't like that the calculation of the rotation angle here has to use
  // timeSpeed again, when we already passed it in to the squares via tick()

  // Maybe we shouldn't bother with a AgedVector for boxes? Or augment it?

  // Calculate rotated/translated
  Point2D upperLeft = rotatePoint(Point2D{leftX, topY}, square.center, angle);
  Point2D upperRight = rotatePoint(Point2D{rightX, topY}, square.center, angle);
  Point2D lowerLeft =
      rotatePoint(Point2D{leftX, bottomY}, square.center, angle);
  Point2D lowerRight =
      rotatePoint(Point2D{rightX, bottomY}, square.center, angle);

  // Now draw 4 lines
  drawLine(upperLeft, upperRight, window, square.label);
  drawLine(upperLeft, lowerLeft, window, square.label);
  drawLine(upperRight, lowerRight, window, square.label);
  drawLine(lowerLeft, lowerRight, window, square.label);
  mvwaddch(window, upperLeft(1), upperLeft(0), '+');
  mvwaddch(window, upperRight(1), upperRight(0), '+');
  mvwaddch(window, lowerLeft(1), lowerLeft(0), '+');
  mvwaddch(window, lowerRight(1), lowerRight(0), '+');
}

void GameLoop(WINDOW *window) {
  // Get input
  int x = getch();

  // Update the model
  int minX = getbegx(window);
  int minY = getbegy(window);
  int maxX = getmaxx(window);
  int maxY = getmaxy(window);

  switch (x) {
  case 'x':
    exitRequested = true;
    return;
    break;
  case KEY_LEFT:
    userCursor(0) = std::max(userCursor(0) - 1, minX + 1);
    break;
  case KEY_RIGHT:
    userCursor(0) = std::min(userCursor(0) + 1, maxX - 2);
    break;
  case KEY_UP:
    userCursor(1) = std::max(userCursor(1) - 1, minY + 1);
    break;
  case KEY_DOWN:
    userCursor(1) = std::min(userCursor(1) + 1, maxY - 2);
    break;
  case '+':
    timeSpeed++;
    if (timeSpeed > 100) {
      timeSpeed = 100;
    }
    break;
  case '-':
    timeSpeed--;
    if (timeSpeed < 1) {
      timeSpeed = 1;
    }
    break;
  case 's':
    addSquare(window, userCursor);
    break;
  case ' ':
    addDecaying(window, userCursor);
    break;
  }

  // Draw the output
  werase(window);
  wborder(window, 0, 0, 0, 0, 0, 0, 0, 0);
  mvprintw(10, 10, "The iteration is %d", loop++);
  mvprintw(11, 10, "The time multiplier is %d", timeSpeed);

  timeoutList.tick(timeSpeed);
  squares.tick(timeSpeed);
  for (const auto &item : timeoutList) {
    mvwaddch(window, item.item.center(1), item.item.center(0),
             item.item.label |
                 COLOR_PAIR((timeoutList.getAge() - item.birthTime) / 10 + 50));
  }

  for (const auto &agedSquare : squares) {
    const RotatingSquare &square = agedSquare.item;
    int angle =
        (square.rotationSpeed * timeSpeed * (loop - agedSquare.birthTime));
    drawSquare(window, square, angle);
  }

  mvwaddch(window, userCursor(1), userCursor(0), 'X' | A_BOLD);
  wrefresh(window); /* Print it on to the real screen */
}

int main() {
  WINDOW *window = initscr(); /* Start curses mode 		  */
  curs_set(0);
  raw();
  noecho();
  cbreak();
  nodelay(stdscr, TRUE);
  keypad(stdscr, TRUE);
  wresize(window, 300, 300);
  userCursor << 10, 10;
  start_color();
  for (int i = 50; i < 60; i++) {
    init_color(i, (200 - (i - 50) * 20), (1000 - (i - 50) * 100),
               (100 - (i - 50) * 10));
    init_pair(i, i, COLOR_BLACK);
  }
  timer_start(std::bind(GameLoop, window), 50).join();
  endwin(); /* End curses mode		  */
  return 0;
}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
------------------ Author:       Guillermo Ortega               -------------------
------------------ Affiliation:  European Space Agency (ESA)    -------------------
-----------------------------------------------------------------------------------
 Patched by Guillermo on July  11th 2009 to comply with STA statevector defintions and treat special cases
 when eccentricity is bigger than zero or when the true anomly or the argument of the perigee are not well
 defined. This routine needs to be converted into equinoctial elements for v2.0

 Added lines 227-230 by Tiziana Sabatini on July 2009 to avoid bugs with circular orbits
 */

#include<float.h>
#include<math.h>
#include<stdio.h>
#include<iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "statevector.h"

#include <QTextStream>

#define Pi 3.1415926535

using namespace sta;

/* This function converts a state vector given in cartesian coordinates to an state vector given in
   classical orbital elements. For a defintion about the orbital elements, please see "statevector.h"
   /
 Description:
      This function transforms spacecraft cartesian orbital elements in Keplerian
      orbital elements

 Input:
      mu standard gravitational parameter of the planet/moon
      x  x-coordinate in ECI system (km)
      y  y-coordinate in ECI system (km)
      z  z-coordinate in ECI system (km)
      xd vx-coordinate in ECI system (km/s)
      yd vy-coordinate in ECI system (km/s)
      zd vz-coordinate in ECI system (km/s)

 Output:
      a  semi-major axis (km)
      ec eccentricity (-)
      i inclination (rad)
      w0 argument of the perigee (rad)
      o0  right ascention of the ascending node (rad)
      m0 mean anomaly (rad)

 Date: July 2009
 Version: 2.0
*/

/*

void cartesianTOorbital(double mu, double x, double y, double z, double xd, double yd, double zd,
                double& a,double& e,double& i,double& argperi,double& longnode,double& meananom)

{
  Deprecated
}

*/

#ifdef _MSC_VER

// Inverse hyperbolic trig functions are missing from the MSVC math library
double atanh(double x)
{
    return log((1.0 + x) / (1.0 - x)) / 2.0;
}

#endif

/*
   This function converts a state vector given in cartesian coordinates to an state vector given in
   classical orbital elements. For a defintion about the orbital elements, please see "statevector.h"
*/

sta::KeplerianElements cartesianTOorbital(double mu, sta::StateVector cartesianStateVector)
{

      double h, r, v, rdotv, rdot, trueAnomaly;
      double EccentricSINTrueAnomaly, EccentricCOSTrueAnomaly;
      double SINofTheNode, COSofTheNode;
      double ArgumentOfLatitudeSIN, ArgumentOfLatitudeCOS;
      double pseudoTrueAnomaly, pseudoEccentricity;
      double eccentricAnomaly;

      Eigen::Vector3d OrbitalMomentum;

      KeplerianElements foundKeplerianElements;

      // First find the direction of angular momentum vector
      // Compute orbital momentum vector, Eigen style
      OrbitalMomentum = cartesianStateVector.position.cross(cartesianStateVector.velocity);

      // Computing the vector norms
      h = OrbitalMomentum.norm();
      r = cartesianStateVector.position.norm();
      v = cartesianStateVector.velocity.norm();

      rdotv = cartesianStateVector.position.dot(cartesianStateVector.velocity);
      rdot  = rdotv / r;

      // Caculating INCLINATION
      foundKeplerianElements.Inclination = acos(OrbitalMomentum(2) / h );

      // Calculating the Ascending Node
      if (OrbitalMomentum(0) != 0.0 || OrbitalMomentum(1) != 0.0)
      {
          foundKeplerianElements.AscendingNode = atan2( OrbitalMomentum(0), -OrbitalMomentum(1) );

      }
      else
      {
          foundKeplerianElements.AscendingNode = 0.0;
      }

      // Calculating the Semimajor Axis
      foundKeplerianElements.SemimajorAxis = 1.0 / (2.0 / r - ( v * v) / mu);

      // Calculating the Eccentricity
      EccentricSINTrueAnomaly = rdot * h / mu;
      EccentricCOSTrueAnomaly = (( h * h ) / ( mu * r )) - 1.0;

      foundKeplerianElements.Eccentricity = sqrt( EccentricSINTrueAnomaly * EccentricSINTrueAnomaly + EccentricCOSTrueAnomaly * EccentricCOSTrueAnomaly );

      // Calulating the Argument of the Periapsis
      if (EccentricSINTrueAnomaly != 0.0 || EccentricCOSTrueAnomaly != 0.0)
      {
          trueAnomaly = atan2(EccentricSINTrueAnomaly, EccentricCOSTrueAnomaly);
      }
      else
      {
          trueAnomaly = 0.0;
      }

      SINofTheNode = sin(foundKeplerianElements.AscendingNode);
      COSofTheNode = cos(foundKeplerianElements.AscendingNode);

      ArgumentOfLatitudeSIN = (cartesianStateVector.position(1) * COSofTheNode - cartesianStateVector.position(0) * SINofTheNode ) / cos(foundKeplerianElements.Inclination);
      ArgumentOfLatitudeCOS = cartesianStateVector.position(0) * COSofTheNode + cartesianStateVector.position(1) * SINofTheNode;

      if (ArgumentOfLatitudeSIN != 0.0 || ArgumentOfLatitudeCOS != 0.0)
      {
          pseudoTrueAnomaly = atan2(ArgumentOfLatitudeSIN, ArgumentOfLatitudeCOS);
      }
      else
      {
          pseudoTrueAnomaly = 0.0;
      }

      foundKeplerianElements.ArgumentOfPeriapsis = pseudoTrueAnomaly - trueAnomaly;

      if (foundKeplerianElements.ArgumentOfPeriapsis > M_PI)
      {
          foundKeplerianElements.ArgumentOfPeriapsis -= 2.0 * M_PI;
      }

      if (foundKeplerianElements.ArgumentOfPeriapsis < -M_PI)
      {
          foundKeplerianElements.ArgumentOfPeriapsis += 2.0 * M_PI;
      }

      if(foundKeplerianElements.Eccentricity<1e-15)
            {
          foundKeplerianElements.ArgumentOfPeriapsis=0; //patched by Ana to match STK results, and avoid undefinition in circular orbits
      }
      // Calculating now the Mean Anomaly
      pseudoEccentricity = sqrt( fabs(1.0 - foundKeplerianElements.Eccentricity) / (1.0 + foundKeplerianElements.Eccentricity) );

      if (foundKeplerianElements.Eccentricity < 1.0 )  // circular of elliptical case
      {
          eccentricAnomaly = 2.0 * atan (pseudoEccentricity * tan (trueAnomaly / 2.0 ) );
          foundKeplerianElements.MeanAnomaly = eccentricAnomaly - foundKeplerianElements.Eccentricity * sin (eccentricAnomaly);
          if (foundKeplerianElements.MeanAnomaly > M_PI) //Special case in which the mean anomly passed pi radians (elliptic orbit)
          {
              foundKeplerianElements.MeanAnomaly -= 2.0 * M_PI;
          }
          if (foundKeplerianElements.MeanAnomaly < - M_PI) //Special case in which the mean anomly passed -pi radians (elliptic orbit)
          {
              foundKeplerianElements.MeanAnomaly += 2.0 * M_PI;
          }
      }
      else
      {
          eccentricAnomaly = 2.0 * atanh (pseudoEccentricity * tan (trueAnomaly / 2.0 ) );
          foundKeplerianElements.MeanAnomaly = foundKeplerianElements.Eccentricity * sinh (eccentricAnomaly) - eccentricAnomaly;
      }

      foundKeplerianElements.TrueAnomaly = trueAnomaly;
      if (foundKeplerianElements.Eccentricity < 1e-15)
          foundKeplerianElements.TrueAnomaly = foundKeplerianElements.MeanAnomaly;
      foundKeplerianElements.EccentricAnomaly = eccentricAnomaly;

      // Finally putting all together and returning control
      return foundKeplerianElements;

}
sta::DelaunayElements cartesianTOdelaunay(double mu, sta::StateVector cartesianStateVector)
{
    /*
      function that calculates the Delaunay Elements- returns a structure with the 6 elements
      mu- gravitational parameter
      cartesian state vector- state vector for a specified time step
      */
    DelaunayElements CalcDelaunayElements;
    KeplerianElements KeplerianElemList=cartesianTOorbital(mu,cartesianStateVector);
    Eigen::Vector3d OrbitalMomentum;
    OrbitalMomentum = cartesianStateVector.position.cross(cartesianStateVector.velocity);

    CalcDelaunayElements.l=KeplerianElemList.MeanAnomaly;
    CalcDelaunayElements.g=KeplerianElemList.ArgumentOfPeriapsis;
    CalcDelaunayElements.h=KeplerianElemList.AscendingNode;
    CalcDelaunayElements.L=sqrt(mu*(KeplerianElemList.SemimajorAxis));
    CalcDelaunayElements.G=sqrt(mu*(KeplerianElemList.SemimajorAxis)*(1-(KeplerianElemList.Eccentricity*KeplerianElemList.Eccentricity)));
    CalcDelaunayElements.H=sqrt(mu*(KeplerianElemList.SemimajorAxis)*(1-(KeplerianElemList.Eccentricity*KeplerianElemList.Eccentricity)))*cos((KeplerianElemList.Inclination));
//CalcDelaunayElements.H=cartesianStateVector.position.x()*cartesianStateVector.velocity.y()-cartesianStateVector.position.y()*cartesianStateVector.velocity.x();
    return CalcDelaunayElements;
}
sta::EquinoctialElements cartesianTOequinoctial(double mu,sta::StateVector cartesianStateVector)
{
    /*
      function that calculates the Equinoctial Elements- returns a structure with the 6 elements
      mu- gravitational parameter
      cartesian state vector- state vector for a specified time step
      */
    EquinoctialElements CalcEquinoctialElements;
    KeplerianElements KeplerianElemList=cartesianTOorbital(mu,cartesianStateVector);
    Eigen::Vector3d OrbitalMomentum;
    OrbitalMomentum = cartesianStateVector.position.cross(cartesianStateVector.velocity);

    CalcEquinoctialElements.SemimajorAxis=KeplerianElemList.SemimajorAxis;
    CalcEquinoctialElements.ecos=KeplerianElemList.Eccentricity*cos(KeplerianElemList.ArgumentOfPeriapsis+KeplerianElemList.AscendingNode);
    CalcEquinoctialElements.esin=KeplerianElemList.Eccentricity*sin(KeplerianElemList.ArgumentOfPeriapsis+KeplerianElemList.AscendingNode);
    CalcEquinoctialElements.tansin=tan(KeplerianElemList.Inclination/2)*sin(KeplerianElemList.AscendingNode);
    CalcEquinoctialElements.tancos=tan(KeplerianElemList.Inclination/2)*cos(KeplerianElemList.AscendingNode);;
    CalcEquinoctialElements.MeanLon=KeplerianElemList.AscendingNode+KeplerianElemList.ArgumentOfPeriapsis+KeplerianElemList.MeanAnomaly;
return CalcEquinoctialElements;
}


/**
 * \file boost/numeric/ublasx/operation/inv.hpp
 *
 * \brief Matrix inverse.
 *
 * Copyright (c) 2012, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_INV_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_INV_HPP

#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/illcond.hpp>
#include <boost/numeric/ublasx/operation/lu.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <limits>

namespace boost { namespace numeric { namespace ublasx {

using namespace boost::numeric::ublas;

/**
 * \brief Matrix inversion of a square matrix.
 *
 * \return \c true if the given input matrix is invertible; \c false is the
 *  input matrix is (nearly) singular.
 */
template <typename MatrixT>
bool inv_inplace(MatrixT& A)
{
	typedef typename matrix_traits<MatrixT>::value_type value_type;
	typedef typename matrix_traits<MatrixT>::size_type size_type;

	//pre: A is square
	BOOST_UBLAS_CHECK(
		num_rows(A) == num_columns(A),
		bad_size()
	);

	// Compute the inverse X=A^{-1} as the solution of the linear system
	//  AX = I

	MatrixT X(identity_matrix<value_type>(num_rows(A)));

	size_type sing;
	sing = lu_solve_inplace(A, X);

	// Check if matrix is singular
	if (sing)
	{
		BOOST_UBLASX_DEBUG_TRACE("Warning: Matrix is (nearly) singular: cannot compute its inverse.");

		// Fill the matrix with Inf (like MATLAB does)
		A = scalar_matrix<value_type>(
				num_rows(A),
				num_columns(A),
				::std::numeric_limits<value_type>::infinity()
			);

		return false;
	}

	// Check if matrix is ill-conditioned
	if (illcond(A))
	{
		BOOST_UBLASX_DEBUG_TRACE("Warning: Matrix is close to singular or badly scaled.  Results may be inaccurate.");
		::std::clog << "[Warning] Matrix is close to singular or badly scaled.  Results may be inaccurate." << ::std::endl;
	}

	A = X;

	return true;
}

/**
 * \brief Matrix inversion of a square matrix.
 */
template <typename MatrixExprT>
matrix<typename matrix_traits<MatrixExprT>::value_type> inv(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef matrix<value_type> out_matrix_type;

	out_matrix_type X(A);

	inv_inplace(X);

	return X;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_INV_HPP


/*
 * A very basic demo of libeigen3.
 *
 * USAGE:
 *    g++ -o transpose -I /PATH/TO/EIGEN/ transpose.cc
 * or
 *    g++ -o transpose $(pkg-config --cflags eigen3) transpose.cc
 */

#include <cstdlib>
#include <ctime>
#include <iostream>

#include <Eigen/Dense>

using namespace Eigen;

int main()
{
    // Initialize the pseudo-random number generator (Eigen uses rand() internally).
    srand(time(0));

    MatrixXd a = MatrixXd::Random(3, 2);

    std::cout << std::endl << "Here is the matrix a"   << std::endl << a             << std::endl;
    std::cout << std::endl << "Here is the matrix a^T" << std::endl << a.transpose() << std::endl;
    std::cout << std::endl;

    /*
     * !! WARNING !!
     * As for basic arithmetic operators, transpose() and adjoint() simply
     * return a proxy object without doing the actual transposition.
     * If you do b = a.transpose(), then the transpose is evaluated at the same
     * time as the result is written into b. However, there is a complication
     * here.
     * If you do a = a.transpose(), then Eigen starts writing the result into a
     * before the evaluation of the transpose is finished. Therefore, the
     * instruction a = a.transpose() does not replace a with its transpose, as
     * one would expect:
     */

    //a = a.transpose(); // ERROR !!! do NOT do this, even with a square matrix !!!
}


#include <blitz/array.h>

using namespace blitz;

int main()
{
    Array<float,1> x(4), y(4);
    Array<float,2> A(4,4);

    x = 1, 2, 3, 4;
    y = 1, 0, 0, 1;

    firstIndex i;
    secondIndex j;

    A = x(i) * y(j);

    cout << A << endl;

    return 0;
}


#include "MyFrameMain.hpp"
#include <wx/rawbmp.h>
#include <wx/msgdlg.h>
#include <sstream>
#include <wx/dcclient.h> // For wxPaintDC
#include <wx/dcbuffer.h> // For wxBufferedPaintDC
#include <random>
#include <algorithm>
#include <array>
#include <thread>
#include <cmath>

#include <functional> // For std::ref

//#include <boost/multiprecision/cpp_complex.hpp>
#include <complex>

MyFrameMain::MyFrameMain(wxWindow* const parent, const wxWindowID id, const wxString& title) :
	FrameMain(parent, id, title, wxDefaultPosition, wxSize(1222, 512)), m_bitmap(1 /*Width*/, 1 /*Height*/, 24 /*Depth*/)
{
	// To avoid flickering
	SetBackgroundStyle(wxBG_STYLE_PAINT);
	wxNativePixelData data(this->m_bitmap);
	if (!data)
	{
		wxMessageBox("Couldn\'t get access to native pixel data", "Error drawing");
		return;
	}
	else
	{
		const int data_width = data.GetWidth();
		const int data_height = data.GetHeight();

		// Zero out the buffer
		{
			wxNativePixelData::Iterator pixel(data);
			for (int column_index = 0; column_index < data_height; ++column_index, pixel.OffsetY(data, 1))
			{
				const wxNativePixelData::Iterator row_start = pixel;
				for (int row_index = 0; row_index < data_width; ++row_index, ++pixel)
				{
					pixel.Red() = 0;
					pixel.Green() = 0;
					pixel.Blue() = 0;
				}
				pixel = row_start;
			}
		}
	}
}

void MyFrameMain::start_main_loop()
{
	// Can't do this from the constructor in case the created thread needs to call a virtual function of this.
	// That's because accessing virtual functions before the object has finished constructing is undefined
	// behavior.
	this->stop_now = false;
	this->main_loop_thread = std::async(std::launch::async, this->main_loop, std::ref(*this));
}

void MyFrameMain::m_timerUpdateScreenOnTimer(wxTimerEvent& evnt)
{
	if (this->in_on_timer)
		return;
	this->in_on_timer = true;
	this->Refresh();
	//this->Update();
	this->in_on_timer = false;
}

void MyFrameMain::FrameMainOnPaint(wxPaintEvent& evnt)
{
	std::lock_guard<std::mutex> lckr(this->drawing_lock);
	const wxSize new_size = this->GetClientSize();
	if (new_size.x == 0 || new_size.y == 0)
		return;
	if (this->m_bitmap.GetSize() != new_size)
	{
		this->changed_dimensions = true;
	}
	//wxPaintDC dc(this);
	//wxBufferedPaintDC dc(this);
	// The difference between wxBufferedPaintDCand this class is that this class won't
	// double-buffer on platforms which have native double-buffering already,
	// avoiding any unnecessary buffering to avoid flicker.
	wxAutoBufferedPaintDC dc(this);
	dc.DrawBitmap
	(
		this->m_bitmap,
		dc.DeviceToLogicalX(0),
		dc.DeviceToLogicalY(0),
		true /* use mask */
	);
	{
		std::ostringstream text_to_show;
		text_to_show << "Magnification: " << this->m_magnification << "X";
		wxPen pen(*wxWHITE);
		dc.SetPen(pen);
		dc.DrawRectangle(3, 5, 170, 17);
		dc.SetTextForeground(wxColour(0, 0, 0));
		dc.DrawText(text_to_show.str(), wxPoint(5, 5));
	}
	evnt.Skip();
	//wxPen pen(*wxRED, 1); // red pen of width 1
	//dc.SetPen(pen);
	//dc.DrawPoint(wxPoint(100, 100));
	//dc.SetPen(wxNullPen);
}

//static unsigned compute_mandel_pixel_color(const fl& x0, const fl& y0, const fl& len_x, const fl& len_y)
//{
//	fl x2 = 0;
//	fl y2 = 0;
//	fl w = 0;
//	const fl max = len_x * len_y;
//	unsigned iteration = 0;
//	constexpr unsigned max_iteration = 10000;
//	for (; x2 + y2 <= max && iteration < max_iteration; ++iteration)
//	{
//		const fl x = x2 - y2 + x0;
//		const fl y = w - x2 - y2 + y0;
//		x2 = x * x;
//		y2 = y * y;
//		w = (x + y) * (x + y);
//	}
//	return static_cast<unsigned>(static_cast<fl>(iteration) * (static_cast<fl>(0xffffff) / static_cast<fl>(max_iteration)));
//}
constexpr unsigned rgb(const unsigned r, const unsigned g, const unsigned b)
{
	return (r << 16) + (g << 8) + b;
}
constexpr unsigned pallet[] = {
rgb(66, 30, 15),// # brown 3
rgb(25,7,26),// # dark violett
rgb(9,1,47),// # darkest blue
rgb(4,4,73),// # blue 5
rgb(0   ,7,100),// # blue 4
rgb(12 , 44 ,138),// # blue 3
rgb(24 , 82, 177),// # blue 2
rgb(57 ,125 ,209),// # blue 1
rgb(134 ,181, 229),// # blue 0
rgb(211 ,236, 248),// # lightest blue
rgb(241, 233, 191),// # lightest yellow
rgb(248, 201,  95),// # light yellow
rgb(255, 170 , 0),// # dirty yellow
rgb(204, 128  , 0),// # brown 0
rgb(153,  87  , 0),// # brown 1
rgb(106,  52  , 3),// # brown 2
};
//constexpr unsigned pallet[] = {
//0x800000U,//maroon
//0x8B0000U,//dark red
//0xA52A2AU,//brown
//0xB22222U,//firebrick
//0xDC143CU,//crimson
//0xFF0000U,//red
//0xFF6347U,//tomato
//0xFF7F50U,//coral
//0xCD5C5CU,//indian red
//0xF08080U,//light coral
//0xE9967AU,//dark salmon
//0xFA8072U,//salmon
//0xFFA07AU,//light salmon
//0xFF4500U,//orange red
//0xFF8C00U,//dark orange
//0xFFA500U,//orange
//0xFFD700U,//gold
//0xB8860BU,//dark golden rod
//0xDAA520U,//golden rod
//0xEEE8AAU,//pale golden rod
//0xBDB76BU,//dark khaki
//0xF0E68CU,//khaki
//0x808000U,//olive
//0xFFFF00U,//yellow
//0x9ACD32U,//yellow green
//0x556B2FU,//dark olive green
//0x6B8E23U,//olive drab
//0x7CFC00U,//lawn green
//0x7FFF00U,//chart reuse
//0xADFF2FU,//green yellow
//0x006400U,//dark green
//0x008000U,//green
//0x228B22U,//forest green
//0x00FF00U,//lime
//0x32CD32U,//lime green
//0x90EE90U,//light green
//0x98FB98U,//pale green
//0x8FBC8FU,//dark sea green
//0x00FA9AU,//medium spring green
//0x00FF7FU,//spring green
//0x2E8B57U,//sea green
//0x66CDAAU,//medium aqua marine
//0x3CB371U,//medium sea green
//0x20B2AAU,//light sea green
//0x2F4F4FU,//dark slate gray
//0x008080U,//teal
//0x008B8BU,//dark cyan
//0x00FFFFU,//aqua
//0x00FFFFU,//cyan
//0xE0FFFFU,//light cyan
//0x00CED1U,//dark turquoise
//0x40E0D0U,//turquoise
//0x48D1CCU,//medium turquoise
//0xAFEEEEU,//pale turquoise
//0x7FFFD4U,//aqua marine
//0xB0E0E6U,//powder blue
//0x5F9EA0U,//cadet blue
//0x4682B4U,//steel blue
//0x6495EDU,//corn flower blue
//0x00BFFFU,//deep sky blue
//0x1E90FFU,//dodger blue
//0xADD8E6U,//light blue
//0x87CEEBU,//sky blue
//0x87CEFAU,//light sky blue
//0x191970U,//midnight blue
//0x000080U,//navy
//0x00008BU,//dark blue
//0x0000CDU,//medium blue
//0x0000FFU,//blue
//0x4169E1U,//royal blue
//0x8A2BE2U,//blue violet
//0x4B0082U,//indigo
//0x483D8BU,//dark slate blue
//0x6A5ACDU,//slate blue
//0x7B68EEU,//medium slate blue
//0x9370DBU,//medium purple
//0x8B008BU,//dark magenta
//0x9400D3U,//dark violet
//0x9932CCU,//dark orchid
//0xBA55D3U,//medium orchid
//0x800080U,//purple
//0xD8BFD8U,//thistle
//0xDDA0DDU,//plum
//0xEE82EEU,//violet
//0xFF00FFU,//magenta / fuchsia
//0xDA70D6U,//orchid
//0xC71585U,//medium violet red
//0xDB7093U,//pale violet red
//0xFF1493U,//deep pink
//0xFF69B4U,//hot pink
//0xFFB6C1U,//light pink
//0xFFC0CBU,//pink
//0xFAEBD7U,//antique white
//0xF5F5DCU,//beige
//0xFFE4C4U,//bisque
//0xFFEBCDU,//blanched almond
//0xF5DEB3U,//wheat
//0xFFF8DCU,//corn silk
//0xFFFACDU,//lemon chiffon
//0xFAFAD2U,//light golden rod yellow
//0xFFFFE0U,//light yellow
//0x8B4513U,//saddle brown
//0xA0522DU,//sienna
//0xD2691EU,//chocolate
//0xCD853FU,//peru
//0xF4A460U,//sandy brown
//0xDEB887U,//burly wood
//0xD2B48CU,//tan
//0xBC8F8FU,//rosy brown
//0xFFE4B5U,//moccasin
//0xFFDEADU,//navajo white
//0xFFDAB9U,//peach puff
//0xFFE4E1U,//misty rose
//0xFFF0F5U,//lavender blush
//0xFAF0E6U,//linen
//0xFDF5E6U,//old lace
//0xFFEFD5U,//papaya whip
//0xFFF5EEU,//sea shell
//0xF5FFFAU,//mint cream
//0x708090U,//slate gray
//0x778899U,//light slate gray
//0xB0C4DEU,//light steel blue
//0xE6E6FAU,//lavender
//0xFFFAF0U,//floral white
//0xF0F8FFU,//alice blue
//0xF8F8FFU,//ghost white
//0xF0FFF0U,//honeydew
//0xFFFFF0U,//ivory
//0xF0FFFFU,//azure
//0xFFFAFAU,//snow
//0x000000U,//black
//0x696969U,//dim gray / dim grey
//0x808080U,//gray / grey
//0xA9A9A9U,//dark gray / dark grey
//0xC0C0C0U,//silver
//0xD3D3D3U,//light gray / light grey
//0xDCDCDCU,//gainsboro
//0xF5F5F5U,//white smoke
//0xFFFFFFU//white
//};
//static unsigned compute_mandel_pixel_color(const fl& x0, const fl& y0)
//{
//	fl x = 0;
//	fl y = 0;
//	unsigned iteration = 0;
//	constexpr unsigned max_iteration = 2000;
//	while (x * x + y * y <= 4 && iteration < max_iteration)
//	{
//		const fl xtemp = x * x - y * y + x0;
//		y = 2 * x * y + y0;
//		x = xtemp;
//		iteration = iteration + 1;
//	}
//	return pallet[iteration % (sizeof(pallet) / sizeof(pallet[0]))];
//}
static unsigned compute_mandel_pixel_color(const fl& x0, const fl& y0)
{
	// boost::multiprecision::cpp_complex_100
	std::complex<double> c(static_cast<double>(x0), static_cast<double>(y0));
	std::complex<double> z = 0;
	unsigned iteration = 0;
	constexpr unsigned max_iteration = 2000;
	// boost::multiprecision::abs
	for ( ; std::abs(z - c) < 2 && iteration < max_iteration; ++iteration)
	{
		z = z * z + c;
	}
	return pallet[iteration % (sizeof(pallet) / sizeof(pallet[0]))];
}
void MyFrameMain::separate_thread_to_compute_rows(wxNativePixelData& data, wxNativePixelData::Iterator pixel, const unsigned y_start, const unsigned rows_to_compute, MyFrameMain& this_ref, wxSize frame_size)
{
	const int y_index_until_not_including = y_start + rows_to_compute;
	const fl len_x_frame = frame_size.x;
	const fl len_y_frame = frame_size.y;
	if (len_y_frame < y_index_until_not_including)
	{
		throw std::invalid_argument("Trying to draw more rows than exist");
	}
	auto frame_x_to_mandel = [len_x_frame, &this_ref](const fl& x_frame) -> fl
	{
		return this_ref.m_start_x_mandel + this_ref.m_len_x_mandel * (x_frame / len_x_frame);
	};
	auto frame_y_to_mandel = [len_y_frame, &this_ref](const fl& y_frame) -> fl
	{
		return this_ref.m_start_y_mandel + this_ref.m_len_y_mandel * (y_frame / len_y_frame);
	};
	while (!this_ref.changed_dimensions)
	{
		const wxNativePixelData::Iterator pixel_start = pixel;
		for (int y = y_start; y < y_index_until_not_including && !this_ref.changed_dimensions; ++y, pixel.OffsetY(data, 1))
		{
			const fl current_y_mandel = frame_y_to_mandel(frame_size.y - y);
			const wxNativePixelData::Iterator row_start = pixel;
			for (int x = 0; x < frame_size.x && !this_ref.changed_dimensions; ++x, ++pixel)
			{
				const fl current_x_mandel = frame_x_to_mandel(x);
				const unsigned pixel_color = compute_mandel_pixel_color(current_x_mandel, current_y_mandel);
				//pixel_color *= pixel_color;
				//pixel_color /= 2;
				//pixel_color = std::min(pixel_color, 0xffffffU);
				pixel.Red() = (pixel_color & 0xff0000) >> 16;
				pixel.Green() = (pixel_color & 0x00ff00) >> 8;
				pixel.Blue() = (pixel_color & 0x0000ff) >> 0;
			}
			pixel = row_start;
		}
		pixel = pixel_start;
	}
}
void MyFrameMain::main_loop(MyFrameMain& this_ref)
{
	std::random_device truly_random;
	std::default_random_engine rand_algo(truly_random());
	std::uniform_int_distribution<int> range(0, 255);
	while (!this_ref.stop_now)
	{
		wxSize frame_size;
		{
			std::lock_guard<std::mutex> lckr(this_ref.drawing_lock);
			frame_size = this_ref.GetClientSize();
			if (this_ref.m_bitmap.GetSize() != frame_size && frame_size.x > 0 && frame_size.y > 0)
			{
				this_ref.m_bitmap = wxBitmap(frame_size.x, frame_size.y, 24 /* Depth */);
			}
			this_ref.changed_dimensions = false;
		}
		wxNativePixelData data(this_ref.m_bitmap);
		if (!data)
		{
			wxMessageBox("Couldn\'t get access to native pixel data", "Error drawing");
		}
		else
		{
			const unsigned num_cores = std::max<unsigned>(std::thread::hardware_concurrency() / 2U, 1U);
			std::vector<std::future<void>> threads;
			threads.reserve(num_cores);
			wxNativePixelData::Iterator pixel(data);
			const unsigned num_rows_for_each_thread = frame_size.y / num_cores;
			const unsigned num_rows_for_last_thread = frame_size.y - num_rows_for_each_thread * (num_cores - 1);
			for (unsigned core_index = 0; core_index < num_cores; ++core_index)
			{
				const unsigned num_rows_for_current_thread = ((core_index == num_cores - 1) ? num_rows_for_last_thread : num_rows_for_each_thread);
				threads.push_back(std::async(std::launch::async, separate_thread_to_compute_rows, std::ref(data), pixel, core_index * num_rows_for_each_thread, num_rows_for_current_thread, std::ref(this_ref), frame_size));
				pixel.OffsetY(data, num_rows_for_current_thread);
			}
		}
	}
}

void MyFrameMain::FrameMainOnLeftDClick(wxMouseEvent& evnt)
{
	wxPoint click_position;
	wxSize window_size;
	{
		std::lock_guard<std::mutex> lckr(this->drawing_lock);
		click_position = evnt.GetPosition();
		window_size = this->m_bitmap.GetSize();
	}
	const fl ratio_x = static_cast<fl>(click_position.x) / static_cast<fl>(window_size.x);
	const fl ratio_y = static_cast<fl>(window_size.y - click_position.y) / static_cast<fl>(window_size.y);
	const fl new_center_x_mandle = this->m_start_x_mandel + this->m_len_x_mandel * ratio_x;
	const fl new_center_y_mandle = this->m_start_y_mandel + this->m_len_y_mandel * ratio_y;

	this->m_len_x_mandel *= this->m_zoom;
	this->m_len_y_mandel *= this->m_zoom;
	this->m_start_x_mandel = new_center_x_mandle - this->m_len_x_mandel / 2;
	this->m_start_y_mandel = new_center_y_mandle - this->m_len_y_mandel / 2;

	this->m_magnification /= static_cast<boost::multiprecision::cpp_bin_float_100>(this->m_zoom);
}

MyFrameMain::~MyFrameMain()
{
	this->stop_now = true;
	this->changed_dimensions = true;
	// Will happen in any case
	//this->main_loop_thread.get();
}


#include "problemes.h"
#include "chiffres.h"
#include "polygonal.h"
#include "permutation.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef std::vector<vecteur> matrice;

ENREGISTRER_PROBLEME(98, "Anagramic squares") {
    // By replacing each of the letters in the word CARE with 1, 2, 9, and 6 respectively, we form a square number:
    // 1296 = 36². What is remarkable is that, by using the same digital substitutions, the anagram, RACE, also forms a
    // square number: 9216 = 96². We shall call CARE (and RACE) a square anagram word pair and specify further that
    // leading zeroes are not permitted, neither may a different letter have the same digital value as another letter.
    //
    // Using words.txt (right click and 'Save Link/Target As...'), a 16K text file containing nearly two-thousand common
    // English words, find all the square anagram word pairs (a palindromic word is NOT considered to be an anagram of
    // itself).
    //
    // What is the largest square number formed by any member of such a pair?
    //
    // NOTE: All anagrams formed must be contained in the given text file.
    std::map<nombre, std::set<vecteur>> anagrammes;
    vecteur chiffres = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    for (auto permutation: permutation::Permutation<vecteur>(chiffres)) {
        if (permutation.front() != 0) {
            for (unsigned short i = 1; i < 10; ++i) {
                if (polygonal::est_carre(
                        chiffres::conversion_nombre<nombre>(permutation.begin(), std::next(permutation.begin(), i))))
                    anagrammes[i].emplace(permutation.begin(), std::next(permutation.begin(), i));
            }
            if (polygonal::est_carre(chiffres::conversion_nombre<nombre>(permutation.begin(), permutation.end())))
                anagrammes[10].insert(permutation);
        }
    }

    std::vector<std::string> mots;
    std::ifstream ifs("data/p098_words.txt");
    std::string ligne;
    while (ifs >> ligne) {
        std::vector<std::string> v;
        boost::split(v, ligne, boost::is_any_of(",\""));
        mots.insert(mots.end(), v.begin(), v.end());
    }

    nombre resultat = 0;
    for (auto it1 = mots.begin(), en = mots.end(); it1 != en; ++it1) {
        const auto &mot1 = *it1;
        for (auto it2 = std::next(it1); it2 != en; ++it2) {
            const auto &mot2 = *it2;
            if (mot1.size() == mot2.size() && is_permutation(mot1.begin(), mot1.end(), mot2.begin())) {
                for (const auto &anagramme: anagrammes[mot1.size()]) {
                    std::map<char, nombre> decode;
                    for (nombre n = 0; n < mot1.size(); ++n)
                        decode[mot1.at(n)] = anagramme.at(n);

                    vecteur v;
                    for (char c: mot2)
                        v.push_back(decode[c]);

                    if (v.front() != 0) {
                        auto n1 = chiffres::conversion_nombre<nombre>(anagramme.begin(), anagramme.end());
                        auto n2 = chiffres::conversion_nombre<nombre>(v.begin(), v.end());
                        if (polygonal::est_carre(n2)) {
                            // std::cout << "(" << mot1 << ", " << mot2 << ") ==>" << n1 << " " <<  n2 << std::endl;
                            resultat = std::max(n1, resultat);
                            resultat = std::max(n2, resultat);
                        }
                    }
                }
            }
        }
    }

    return std::to_string(resultat);
}


#include "surface_manager.h"

#include <Eigen/Geometry>
#include <iostream>

Surface_Manager::Surface_Manager()
{
	surface_type = SURFACE;
}

Surface_Manager::~Surface_Manager()
{

}

void Surface_Manager::frameAtPoint(Mesh::Point &pt, VFrame& frm)
{
	OpenMesh::Vec3d n;
	assert(surface_type == SURFACE);
	NormAt(pt, n);
	frm.e0 = pt;
	frm.e1 = OpenMesh::cross(n,pt);
	frm.n = n;
}

Mesh::Point Surface_Manager::PointWithCoordinate(Mesh::Point &pt, VFrame &frm, const Eigen::Vector2d &x)
{
	Mesh::Point fin = pt + x(0) * frm.e0 + x(1) * frm.e1;
	Mesh::Point close_p;

	assert(surface_type == SURFACE);
	CompressionProjiect(fin, close_p);

	return close_p;
}

void Surface_Manager::PointProjection(Mesh::Point& pt, Mesh::Point& proj_p)
{
	assert(surface_type == SURFACE);
	CompressionProjiect(pt, proj_p);
}

void Surface_Manager::CompressionProjiect(Mesh::Point& p, Mesh::Point& new_p)
{
	double lambda = 0.76;
	double a = 1.618;

	double pr = p.norm();
	double prxy = sqrt(p[0] * p[0] + p[1] * p[1]);
	double sin_phi = p[2] / pr;
	double cos_phi = prxy / pr;
	double cos_theta = p[0] / prxy;
	double sin_theta = p[1] / prxy;
	if (prxy < 1e-8)
	{
		cos_phi = 0;
		cos_theta = 0;
		sin_theta = 0;
	}

	double r = (1 - lambda)* pow(cos_phi, a) + lambda;
	double new_x = r*cos_theta*cos_phi;
	double new_y = r*sin_theta*cos_phi;
	double new_z = r*sin_phi;

	new_p = Mesh::Point(new_x, new_y, new_z);
}

void Surface_Manager::NormAt(Mesh::Point& p, OpenMesh::Vec3d& n)
{
	double lambda = 0.76;
	double a = 1.618;

	double pr = p.norm();
	double prxy = sqrt(p[0] * p[0] + p[1] * p[1]);
	double sin_phi = p[2] / pr;
	double cos_phi = prxy / pr;
	double cos_theta = p[0] / prxy;
	double sin_theta = p[1] / prxy;
	if (prxy < 1e-8)
	{
		n = p;
		n.normalize();
		p = Mesh::Point(1, 0, 0);
		return;
	}

	double dr = (1 - lambda)*a* pow(cos_phi, a - 1)*sin_phi;
	n = p + dr * OpenMesh::Vec3d(cos_theta * sin_phi, sin_phi * sin_theta, -cos_phi);
	n.normalize();
	p = OpenMesh::Vec3d(-n[1], n[0], 0);
	p.normalize();

	return;
}


/**
 * @addtogroup LinemodTraining
 * @{
 */

#include <cmath>
#include <Eigen/Geometry>
#include <opencv2/core/eigen.hpp>
#include "LinemodTemplateGeneratorIteratorSinCos.hpp"

namespace CDFF
{
namespace Common
{
namespace LinemodTraining
{

LinemodTemplateGeneratorIteratorSinCos::LinemodTemplateGeneratorIteratorSinCos(LinemodTemplateGenerator *px_renderer,
                                                                               float f_lonMin, float f_lonMax, float f_lonStep,
                                                                               float f_latMin, float f_latMax, float f_latStep,
                                                                               float f_angleMin, float f_angleMax, float f_angleStep,
                                                                               float f_radiusMin, float f_radiusMax, float f_radiusStep)
    :
      _px_renderer(px_renderer),

      _f_lonMin(f_lonMin * static_cast<float>(CV_PI/180.0)),
      _f_lonMax(f_lonMax * static_cast<float>(CV_PI/180.0)),
      _f_lonStep(f_lonStep * static_cast<float>(CV_PI/180.0)),
      _f_lon(f_lonMin * static_cast<float>(CV_PI/180.0)),

      _f_latMin(f_latMin * static_cast<float>(CV_PI/180.0)),
      _f_latMax(f_latMax * static_cast<float>(CV_PI/180.0)),
      _f_latStep(f_latStep * static_cast<float>(CV_PI/180.0)),
      _f_lat(f_latMin * static_cast<float>(CV_PI/180.0)),

      _f_angleMin(f_angleMin * static_cast<float>(CV_PI/180.0)),
      _f_angleMax(f_angleMax * static_cast<float>(CV_PI/180.0)),
      _f_angleStep(f_angleStep * static_cast<float>(CV_PI/180.0)),
      _f_angle(f_angleMin * static_cast<float>(CV_PI/180.0)),

      _f_radiusMin(f_radiusMin),
      _f_radiusMax(f_radiusMax),
      _f_radiusStep(f_radiusStep),
      _f_radius(f_radiusMin),
      _mat4_currentPose(),
      _i_size( static_cast<int>((std::floor((f_lonMax-f_lonMin)/f_lonStep)+1)
               * (std::floor((f_latMax-f_latMin)/f_latStep)+1)
               * (std::floor((f_angleMax-f_angleMin)/f_angleStep)+1)
               * (std::floor((f_radiusMax-f_radiusMin)/f_radiusStep)+1))),
      _b_isDone(false)
{
}

LinemodTemplateGeneratorIteratorSinCos & LinemodTemplateGeneratorIteratorSinCos::operator++()
{
    _f_lon += _f_lonStep ;
    if (_f_lon > _f_lonMax){
        _f_lon = _f_lonMin;
        _f_lat += _f_latStep;
        if (_f_lat > _f_latMax){
            _f_lat = _f_latMin;
            _f_angle += _f_angleStep;
            if (_f_angle > _f_angleMax)
            {
                _f_angle = _f_angleMin;
                _f_radius += _f_radiusStep;
                if (_f_radius > _f_radiusMax)
                {
                    _b_isDone = true;
                }
            }
        }
    }
    return *this;
}

void LinemodTemplateGeneratorIteratorSinCos::render(std::vector<cv::Mat> &sources, cv::Mat &mask_out, int num_modalities)
{
    if (isDone())
        return;

    // North east down (NED) Frame
    cv::Mat R_NED = (cv::Mat_<double>(3,3) <<
                     -sin( static_cast<double>(_f_lat))*cos( static_cast<double>(_f_lon)),   -sin( static_cast<double>(_f_lon)),      -cos( static_cast<double>(_f_lat))*cos( static_cast<double>(_f_lon)),
                     -sin( static_cast<double>(_f_lat))*sin( static_cast<double>(_f_lon)),    cos( static_cast<double>(_f_lon)),      -cos( static_cast<double>(_f_lat))*sin( static_cast<double>(_f_lon)),
                     cos( static_cast<double>(_f_lat)),              0,                  -sin( static_cast<double>(_f_lat)));

    // Rotation around Z
    cv::Mat R_z = (cv::Mat_<double>(3,3) <<
                   cos( static_cast<double>(_f_angle)),    -sin( static_cast<double>(_f_angle)),      0,
                   sin( static_cast<double>(_f_angle)),    cos( static_cast<double>(_f_angle)),       0,
                   0,               0,                  1);

    cv::Mat Position = R_NED*R_z*cv::Mat(cv::Vec3d(0,0,- static_cast<double>(_f_radius)));
    _mat4_currentPose = cv::Affine3d(R_NED*R_z, cv::Vec3d(Position));

    _px_renderer->setPose(_mat4_currentPose);
    _px_renderer->render(num_modalities);

    sources.push_back(_px_renderer->_x_colorImage);
    if(num_modalities == 2)
        sources.push_back(_px_renderer->_x_depthImage);
    mask_out = _px_renderer->_x_maskImage;
}

}
}
}
/** @} */


#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <iostream>

#include "../src/spherical_harmonics.hpp"

using namespace fsph;
using namespace boost::math;

int main(int argc, char **argv)
{
    unsigned int lmax(5);
    float phi(0.25), theta(1.5*M_PI);

    PointSPHEvaluator<float> eval(lmax);
    eval.compute(phi, theta);
    PointSPHEvaluator<float>::iterator iter(eval.begin(true));

    double error(0);
    unsigned int N(0);

    for(unsigned int l(0); l <= lmax; ++l)
    {
        for(unsigned int m(0); m <= l; ++m)
        {
            std::complex<float> from_fsph(*iter);
            // std::cout << iter.grad_phi(phi, theta) << ' ' << iter.grad_theta() << std::endl;
            // boost names phi and theta using the opposite convention
            std::complex<float> from_boost(spherical_harmonic(l, m, phi, theta));
            from_boost *= pow(-1, m);
            error += abs(from_fsph - from_boost);
            if(abs(from_fsph - from_boost) > 1e-5)
                std::cout << l << ' ' << m << ' ' << from_fsph << ' ' << from_boost << std::endl;
            ++N;
            ++iter;
        }
        for(unsigned int m(1); m <= l; ++m)
        {
            std::complex<float> from_fsph(*iter);
            // std::cout << iter.grad_phi(phi, theta) << ' ' << iter.grad_theta() << std::endl;
            // boost names phi and theta using the opposite convention
            std::complex<float> from_boost(spherical_harmonic(l, -(int)m, phi, theta));
            error += abs(from_fsph - from_boost);
            if(abs(from_fsph - from_boost) > 1e-5)
                std::cout << l << ' ' << m << ' ' << from_fsph << ' ' << from_boost << std::endl;
            ++N;
            ++iter;
        }
    }

    const unsigned int start_l(2);
    const unsigned int start_m(1);
    PointSPHEvaluator<float>::iterator iter_l(eval.begin_l(start_l, start_m, false));

    for(unsigned int l(start_l); l <= lmax; ++l)
    {
        for(unsigned int m(l == start_l? start_m: 0); m <= l; ++m)
        {
            std::complex<float> from_fsph(*iter_l);
            // std::cout << iter_l.grad_phi(phi, theta) << ' ' << iter_l.grad_theta() << std::endl;
            // boost names phi and theta using the opposite convention
            std::complex<float> from_boost(spherical_harmonic(l, m, phi, theta));
            from_boost *= pow(-1, m);
            error += abs(from_fsph - from_boost);
            if(abs(from_fsph - from_boost) > 1e-5)
                std::cout << l << ' ' << m << ' ' << from_fsph << ' ' << from_boost << std::endl;
            ++N;
            ++iter_l;
        }
    }

    return error/N > 1e-7;
}


/*
 * Filename: inverse_condition_number.cpp
 *
 * Copyright 2020 Tecnalia
 *
 * 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.
 */

#include <manipulability_metrics/metrics/inverse_condition_number.h>

#include <Eigen/SVD>

namespace manipulability_metrics
{
double inverseConditionNumber(const Chain& chain, const KDL::JntArray& joint_positions)
{
  return inverseConditionNumber(chain, KDL::Vector::Zero(), joint_positions);
}

double inverseConditionNumber(const Chain& chain, const KDL::Vector& tcp_offset, const KDL::JntArray& joint_positions)
{
  auto jac = KDL::Jacobian{ static_cast<unsigned int>(chain.n_joints) };
  chain.jacobian(joint_positions, tcp_offset, jac);

  auto jac_svd = Eigen::JacobiSVD<Eigen::Matrix<double, 6, Eigen::Dynamic>>{ jac.data };

  return jac_svd.singularValues()(jac_svd.singularValues().rows() - 1) / jac_svd.singularValues()(0);
}
}  // namespace manipulability_metrics


#ifndef FAST_GICP_SO3_HPP
#define FAST_GICP_SO3_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace fast_gicp {

inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) {
  Eigen::Matrix3f skew = Eigen::Matrix3f::Zero();
  skew(0, 1) = -x[2];
  skew(0, 2) = x[1];
  skew(1, 0) = x[2];
  skew(1, 2) = -x[0];
  skew(2, 0) = -x[1];
  skew(2, 1) = x[0];

  return skew;
}

inline Eigen::Matrix3d skewd(const Eigen::Vector3d& x) {
  Eigen::Matrix3d skew = Eigen::Matrix3d::Zero();
  skew(0, 1) = -x[2];
  skew(0, 2) = x[1];
  skew(1, 0) = x[2];
  skew(1, 2) = -x[0];
  skew(2, 0) = -x[1];
  skew(2, 1) = x[0];

  return skew;
}

/*
 * SO3 expmap code taken from Sophus
 * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585
 *
 * Copyright 2011-2017 Hauke Strasdat
 *           2012-2017 Steven Lovegrove
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */
inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) {
  double theta_sq = omega.dot(omega);

  double theta;
  double imag_factor;
  double real_factor;
  if(theta_sq < 1e-10) {
    theta = 0;
    double theta_quad = theta_sq * theta_sq;
    imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad;
    real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad;
  } else {
    theta = std::sqrt(theta_sq);
    double half_theta = 0.5 * theta;
    imag_factor = std::sin(half_theta) / theta;
    real_factor = std::cos(half_theta);
  }

  return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());
}

}  // namespace fast_gicp

#endif


#include "CRTP/Vectors/Euclidean3Vector.h"

#include <boost/test/unit_test.hpp>

using CRTP::Vectors::Euclidean3Vector;

BOOST_AUTO_TEST_SUITE(CRTP)
BOOST_AUTO_TEST_SUITE(Vectors)

BOOST_AUTO_TEST_SUITE(Euclidean3Vectors_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ConstructsFromInitializerList)
{
  Euclidean3Vector<double> x {30, 40, 50};

  BOOST_TEST(true);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(AccessorsRetrieveData)
{
  Euclidean3Vector<double> x {30, 40, 50};

  BOOST_TEST(x.x() == 30);
  BOOST_TEST(x.y() == 40);
  BOOST_TEST(x.z() == 50);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DimensionRetrievesDimension)
{
  Euclidean3Vector<double> x;

  BOOST_TEST(x.dimension() == 3);
}

BOOST_AUTO_TEST_SUITE_END() // Euclidean3Vectors_tests
BOOST_AUTO_TEST_SUITE_END() // Vectors
BOOST_AUTO_TEST_SUITE_END() // CRTP


#ifndef TRIUMF_BNMR_SLR_SQ_EXP_HPP
#define TRIUMF_BNMR_SLR_SQ_EXP_HPP

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed square exponential integral (from 0 to time_p <= time)
template <typename T = double>
T pulsed_sq_exp_integral(T time, T time_p, T nuclear_lifetime, T slr_rate) {
  // make sure that
  assert(time >= time_p);
  // break up the expression into smaller chunks
  T term_1 = (boost::math::constants::root_pi<T>() / (2.0 * slr_rate)) *
             std::exp(1.0 / (4.0 * slr_rate * slr_rate * nuclear_lifetime *
                             nuclear_lifetime));
  T term_2 =
      std::erf((2.0 * time * slr_rate * slr_rate * nuclear_lifetime + 1.0) /
               (2.0 * slr_rate * nuclear_lifetime));
  T term_3 = std::erf(
      ((2.0 * (time - time_p)) * slr_rate * slr_rate * nuclear_lifetime + 1.0) /
      (2.0 * slr_rate * nuclear_lifetime));
  return term_1 * (term_2 - term_3);
}

/// pulsed square exponential
template <typename T = double>
T pulsed_sq_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                T slr_rate) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_sq_exp_integral(time, time, nuclear_lifetime, slr_rate) /
           normalization(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_sq_exp_integral(time, pulse_length, nuclear_lifetime,
                                   slr_rate) /
            normalization(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed square exponential (ROOT)
template <typename T = double> T pulsed_sq_exp(const T *x, const T *par) {
  return pulsed_sq_exp<T>(*x, par[0], par[1], par[2], par[3]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_SQ_EXP_HPP


#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/find.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>

using namespace boost::mpl;

template <typename Sequence, typename Value>
struct index_of
{
	using it = typename find<Sequence, Value>::type;
	using index = typename it::pos;
	using size = typename size<Sequence>::type;
	using index_smaller_than_size = typename less<index, size>::type;
	using type = typename if_<index_smaller_than_size, index, int_<-1>>::type;
};

int main()
{
	using v = vector_c<int, 5, 2, 3, 1, 4>;

	constexpr int r1 = index_of<v, integral_c<int, 3>>::type::value;
	static_assert(r1 == 2);

	constexpr int r2 = index_of<v, integral_c<int, 6>>::type::value;
	static_assert(r2 == -1);
}


#include "ts.h"
#include <Eigen/Geometry>
#include <cmath>

using namespace std;

vector<double> ts_to_quat(vector<double> ts)
{
    Eigen::Quaterniond rTwist(Eigen::AngleAxisd(ts[1], Eigen::Vector3d::UnitY()));
    Eigen::Vector3d swingVec(ts[0], 0, ts[2]);
    Eigen::Quaterniond rSwing(Eigen::AngleAxisd(swingVec.norm(), swingVec.normalized()));
    Eigen::Quaterniond r = rSwing * rTwist;
    r.normalize();
    if (r.w() < 0) { r.w() = -r.w(); r.x() = -r.x(); r.y() = -r.y(); r.z() = -r.z(); }
    return { r.w(), r.x(), r.y(), r.z() };
}

vector<double> quat_to_ts(vector<double> quat)
{
    Eigen::Quaterniond r(quat[0], quat[1], quat[2], quat[3]);

    r.normalize();
    if (r.w() < 0) { r.w() = -r.w(); r.x() = -r.x(); r.y() = -r.y(); r.z() = -r.z(); }

    Eigen::Quaterniond rTwist(1, 0, 0, 0);
    if (abs(r.y()) > 1e-6) rTwist = Eigen::Quaterniond(r.w(), 0, r.y(), 0);

    rTwist.normalize();

    Eigen::Quaterniond rSwing = r * rTwist.conjugate();

    Eigen::AngleAxisd axTwist(rTwist);
    Eigen::AngleAxisd axSwing(rSwing);

    return {
        axSwing.angle() * axSwing.axis()[0],
        axTwist.angle() * axTwist.axis()[1],
        axSwing.angle() * axSwing.axis()[2]
    };
}


//
// Copyright (c) 2012 Ronaldo Carpio
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appear in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  The authors make no representations
// about the suitability of this software for any purpose.
// It is provided "as is" without express or implied warranty.
//

/*
This is a C++ header-only library for N-dimensional linear interpolation on a rectangular grid. Implements two methods:
* Multilinear: Interpolate using the N-dimensional hypercube containing the point. Interpolation step is O(2^N)
* Simplicial: Interpolate using the N-dimensional simplex containing the point. Interpolation step is O(N log N), but less accurate.
Requires boost/multi_array library.

For a description of the algorithms, see:
* Weiser & Zarantonello (1988), "A Note on Piecewise Linear and Multilinear Table Interpolation in Many Dimensions", _Mathematics of Computation_ 50 (181), p. 189-196
* Davies (1996), "Multidimensional Triangulation and Interpolation for Reinforcement Learning", _Proceedings of Neural Information Processing Systems 1996_
*/

// Orgiginal file from linterp library: github.com/rncarpio/linterp/blob/master/src/linterp.h
// Modified by Sergey Poluyan
// There are no significant changes. Everything except linear interpolation was removed. Some code simplification was done.

// Usage:
//    std::vector<double> f_values = {0.5, 1.0, 0.75};
//    std::vector<std::vector<double>> grids = { {1, 2, 3} };
//    boost::array<int, 1> grid_sizes = { 3 };
//    auto grid_iter_list = linterp::get_begins_ends( grids.begin(), grids.end() );
//    auto obj = linterp::InterpMultilinear<1, double>(grid_iter_list.first.begin(), grid_sizes.begin(), f_values.data(), f_values.data() + f_values.size() );
//    boost::array<double, 1> args = { 1.25 };
//    std::cout << obj.interp( args.begin() ) << std::endl;

#ifndef linterp_hh
#define linterp_hh

#include <vector>
#include <array>

#include <boost/multi_array.hpp>
#include <boost/numeric/ublas/matrix.hpp>

namespace linterp
{

	struct EmptyClass {};

	template <int N, class T, bool CopyData = true, bool Continuous = true, class ArrayRefCountT = EmptyClass, class GridRefCountT = EmptyClass>
	class NDInterpolator
	{
	public:
		typedef T value_type;
		typedef ArrayRefCountT array_ref_count_type;
		typedef GridRefCountT grid_ref_count_type;

		NDInterpolator() {}

		static const int m_N = N;
		static const bool m_bCopyData = CopyData;
		static const bool m_bContinuous = Continuous;

		typedef boost::numeric::ublas::array_adaptor<T> grid_type;
		typedef boost::const_multi_array_ref<T, N> array_type;
		typedef std::unique_ptr<array_type> array_type_ptr;

		array_type_ptr m_pF;
		ArrayRefCountT m_ref_F;					// reference count for m_pF
		std::vector<T> m_F_copy;						// if CopyData == true, this holds the copy of F

		std::vector<grid_type> m_grid_list;
		std::vector<GridRefCountT> m_grid_ref_list;	// reference counts for grids
		std::vector<std::vector<T> > m_grid_copy_list;  	// if CopyData == true, this holds the copies of the grids

		// constructors assume that [f_begin, f_end) is a contiguous array in C-order
		// non ref-counted constructor.
		template <class IterT1, class IterT2, class IterT3>
		NDInterpolator(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end)
		{
			init(grids_begin, grids_len_begin, f_begin, f_end);
		}

		// ref-counted constructor
		template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
		NDInterpolator(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end, ArrayRefCountT &refF, RefCountIterT grid_refs_begin)
		{
			init_refcount(grids_begin, grids_len_begin, f_begin, f_end, refF, grid_refs_begin);
		}

		template <class IterT1, class IterT2, class IterT3>
		void init(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end)
		{
			set_grids(grids_begin, grids_len_begin, m_bCopyData);
			set_f_array(f_begin, f_end, m_bCopyData);
		}
		template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
		void init_refcount(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end, ArrayRefCountT &refF, RefCountIterT grid_refs_begin)
		{
			set_grids(grids_begin, grids_len_begin, m_bCopyData);
			set_grids_refcount(grid_refs_begin, grid_refs_begin + N);
			set_f_array(f_begin, f_end, m_bCopyData);
			set_f_refcount(refF);
		}

		template <class IterT1, class IterT2>
		void set_grids(IterT1 grids_begin, IterT2 grids_len_begin, bool bCopy)
		{
			m_grid_list.clear();
			m_grid_ref_list.clear();
			m_grid_copy_list.clear();
			for(int i=0; i<N; i++)
			{
				int gridLength = grids_len_begin[i];
				if(bCopy == false)
				{
					T const *grid_ptr = &(*grids_begin[i]);
					m_grid_list.push_back(grid_type(gridLength, (T*) grid_ptr));	  				// use the given pointer
				}
				else
				{
					m_grid_copy_list.push_back(std::vector<T>(grids_begin[i], grids_begin[i] + grids_len_begin[i]));	// make our own copy of the grid
					T *begin = &(m_grid_copy_list[i][0]);
					m_grid_list.push_back(grid_type(gridLength, begin));							// use our copy
				}
			}
		}
		template <class IterT1, class RefCountIterT>
		void set_grids_refcount(RefCountIterT refs_begin, RefCountIterT refs_end)
		{
			assert(refs_end - refs_begin == N);
			m_grid_ref_list.assign(refs_begin, refs_begin + N);
		}

		// assumes that [f_begin, f_end) is a contiguous array in C-order
		template <class IterT>
		void set_f_array(IterT f_begin, IterT f_end, bool bCopy)
		{
			unsigned int nGridPoints = 1;
			std::array<int,N> sizes;
			for(unsigned int i=0; i<m_grid_list.size(); i++)
			{
				sizes[i] = m_grid_list[i].size();
				nGridPoints *= sizes[i];
			}

			int f_len = f_end - f_begin;
			if((m_bContinuous && f_len != static_cast<int>(nGridPoints)) || (!m_bContinuous && f_len != 2 * static_cast<int>(nGridPoints)))
			{
				throw std::invalid_argument("f has wrong size");
			}
			for(unsigned int i=0; i<m_grid_list.size(); i++)
			{
				if(!m_bContinuous)
				{
					sizes[i] *= 2;
				}
			}

			m_F_copy.clear();
			if(bCopy == false)
			{
				m_pF.reset(new array_type(f_begin, sizes));
			}
			else
			{
				m_F_copy = std::vector<T>(f_begin, f_end);
				m_pF.reset(new array_type(&m_F_copy[0], sizes));
			}
		}
		void set_f_refcount(ArrayRefCountT &refF)
		{
			m_ref_F = refF;
		}

		// -1 is before the first grid point
		// N-1 (where grid.size() == N) is after the last grid point
		int find_cell(int dim, T x) const
		{
			grid_type const &grid(m_grid_list[dim]);
			if(x < *(grid.begin())) return -1;
			else if(x >= *(grid.end()-1)) return grid.size()-1;
			else
			{
				auto i_upper = std::upper_bound(grid.begin(), grid.end(), x);
				return i_upper - grid.begin() - 1;
			}
		}

		// return the value of f at the given cell and vertex
		T get_f_val(std::array<int,N> const &cell_index, std::array<int,N> const &v_index) const
		{
			std::array<int,N> f_index;

			if(m_bContinuous)
			{
				for(int i=0; i<N; i++)
				{
					if(cell_index[i] < 0)
					{
						f_index[i] = 0;
					}
					else if(cell_index[i] >= static_cast<int>(m_grid_list[i].size()-1))
					{
						f_index[i] = m_grid_list[i].size()-1;
					}
					else
					{
						f_index[i] = cell_index[i] + v_index[i];
					}
				}
			}
			else
			{
				for(int i=0; i<N; i++)
				{
					if(cell_index[i] < 0)
					{
						f_index[i] = 0;
					}
					else if(cell_index[i] >= static_cast<int>(m_grid_list[i].size()-1))
					{
						f_index[i] = (2*m_grid_list[i].size())-1;
					}
					else
					{
						f_index[i] = 1 + (2*cell_index[i]) + v_index[i];
					}
				}
			}
			return (*m_pF)(f_index);
		}

		T get_f_val(std::array<int,N> const &cell_index, int v) const
		{
			std::array<int,N> v_index;
			for(int dim=0; dim<N; dim++)
			{
				v_index[dim] = (v >> (N-dim-1)) & 1;						// test if the i-th bit is set
			}
			return get_f_val(cell_index, v_index);
		}
	};

	template <int N, class T, bool CopyData = true, bool Continuous = true, class ArrayRefCountT = EmptyClass, class GridRefCountT = EmptyClass>
	class InterpMultilinear : public NDInterpolator<N,T,CopyData,Continuous,ArrayRefCountT,GridRefCountT>
	{
	public:
		typedef NDInterpolator<N,T,CopyData,Continuous,ArrayRefCountT,GridRefCountT> super;

		InterpMultilinear() {}

		template <class IterT1, class IterT2, class IterT3>
		InterpMultilinear(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end)
			: super(grids_begin, grids_len_begin, f_begin, f_end)
		{}
		template <class IterT1, class IterT2, class IterT3, class RefCountIterT>
		InterpMultilinear(IterT1 grids_begin, IterT2 grids_len_begin, IterT3 f_begin, IterT3 f_end, ArrayRefCountT &refF, RefCountIterT ref_begins)
			: super(grids_begin, grids_len_begin, f_begin, f_end, refF, ref_begins)
		{}

		template <class IterT1, class IterT2>
		static T linterp_nd_unitcube(IterT1 f_begin, IterT1 f_end, IterT2 xi_begin, IterT2 xi_end)
		{
			int n = xi_end - xi_begin;
			int f_len = f_end - f_begin;
			assert(1 << n == f_len);
			T sub_lower, sub_upper;
			if(n == 1)
			{
				sub_lower = f_begin[0];
				sub_upper = f_begin[1];
			}
			else
			{
				sub_lower = linterp_nd_unitcube(f_begin, f_begin + (f_len/2), xi_begin + 1, xi_end);
				sub_upper = linterp_nd_unitcube(f_begin + (f_len/2), f_end, xi_begin + 1, xi_end);
			}
			T result = sub_lower + (*xi_begin)*(sub_upper - sub_lower);
			return result;
		}

		template <class IterT>
		T interp(IterT x_begin) const
		{
			std::array<T,1> result;
			std::array< std::array<T,1>, N > coord_iter;
			for(int i=0; i<N; i++)
			{
				coord_iter[i][0] = x_begin[i];
			}
			interp_vec(1, coord_iter.begin(), coord_iter.end(), result.begin());
			return result[0];
		}

		template <class IterT1, class IterT2>
		void interp_vec(int n, IterT1 coord_iter_begin, IterT1 coord_iter_end, IterT2 i_result) const
		{
			assert(N == coord_iter_end - coord_iter_begin);
			std::array<int,N> index;
			int c;
			T y;
			std::vector<T> f(1 << N);
			std::array<T,N> x;

			for(int i=0; i<n; i++)  								// loop over each point
			{
				for(int dim=0; dim<N; dim++)  						// loop over each dimension
				{
					auto const &grid(super::m_grid_list[dim]);

					c = this->find_cell(dim, coord_iter_begin[dim][i]);
					if(c == -1)  					// before first grid point
					{
						y = 1.0;
					}
					else if(c == static_cast<int>(grid.size()-1))  	// after last grid point
					{
						y = 0.0;
					}
					else
					{
						y = (coord_iter_begin[dim][i] - grid[c]) / (grid[c + 1] - grid[c]);
						if(y < 0.0) y=0.0;
						else if(y > 1.0) y=1.0;
					}
					index[dim] = c;
					x[dim] = y;
				}
				// copy f values at vertices
				for(int v=0; v < (1 << N); v++)  					// loop over each vertex of hypercube
				{
					f[v] = this->get_f_val(index, v);
				}
				*i_result++ = linterp_nd_unitcube(f.begin(), f.end(), x.begin(), x.end());
			}
		}
	};

	template <class IterT>
	std::pair<std::vector<typename IterT::value_type::const_iterator>, std::vector<typename IterT::value_type::const_iterator> > get_begins_ends(IterT iters_begin, IterT iters_end)
	{
		typedef typename IterT::value_type T;
		typedef std::vector<typename T::const_iterator> VecT;
		int N = iters_end - iters_begin;
		std::pair<VecT, VecT> result;
		result.first.resize(N);
		result.second.resize(N);
		for(int i=0; i<N; i++)
		{
			result.first[i] = iters_begin[i].begin();
			result.second[i] = iters_begin[i].end();
		}
		return result;
	}

}

#endif //_linterp_h


//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Copyright Paul A. Bristow 2013
// Copyright Christopher Kormanyos 2013.
// Copyright John Maddock 2013.

#ifdef _MSC_VER
#  pragma warning (disable : 4512)
#  pragma warning (disable : 4996)
#endif

#define BOOST_TEST_MAIN
#define BOOST_LIB_DIAGNOSTIC "on"// Show library file details.

#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp> // Extra test tool for FP comparison.

#include <iostream>
#include <limits>

//[expression_template_1

#include <boost/multiprecision/cpp_dec_float.hpp>

/*`To define a 50 decimal digit type using `cpp_dec_float`,
you must pass two template parameters to `boost::multiprecision::number`.

It may be more legible to use a two-staged type definition such as this:

``
typedef boost::multiprecision::cpp_dec_float<50> mp_backend;
typedef boost::multiprecision::number<mp_backend, boost::multiprecision::et_off> cpp_dec_float_50_noet;
``

Here, we first define `mp_backend` as `cpp_dec_float` with 50 digits.
The second step passes this backend to `boost::multiprecision::number`
with `boost::multiprecision::et_off`, an enumerated type.

  typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<50>, boost::multiprecision::et_off>
  cpp_dec_float_50_noet;

You can reduce typing with a `using` directive `using namespace boost::multiprecision;`
if desired, as shown below.
*/

using namespace boost::multiprecision;

/*`Now `cpp_dec_float_50_noet` or `cpp_dec_float_50_et`
can be used as a direct replacement for built-in types like `double` etc.
*/

BOOST_AUTO_TEST_CASE(cpp_float_test_check_close_noet)
{ // No expression templates/
  typedef number<cpp_dec_float<50>, et_off> cpp_dec_float_50_noet;

  std::cout.precision(std::numeric_limits<cpp_dec_float_50_noet>::digits10); // All significant digits.
  std::cout << std::showpoint << std::endl; // Show trailing zeros.

  cpp_dec_float_50_noet a ("1.0");
  cpp_dec_float_50_noet b ("1.0");
  b += std::numeric_limits<cpp_dec_float_50_noet>::epsilon(); // Increment least significant decimal digit.

  cpp_dec_float_50_noet eps = std::numeric_limits<cpp_dec_float_50_noet>::epsilon();

  std::cout <<"a = " << a << ",\nb = " << b << ",\neps = " << eps << std::endl;

  BOOST_CHECK_CLOSE(a, b, eps * 100); // Expected to pass (because tolerance is as percent).
  BOOST_CHECK_CLOSE_FRACTION(a, b, eps); // Expected to pass (because tolerance is as fraction).

} // BOOST_AUTO_TEST_CASE(cpp_float_test_check_close)

BOOST_AUTO_TEST_CASE(cpp_float_test_check_close_et)
{ // Using expression templates.
  typedef number<cpp_dec_float<50>, et_on> cpp_dec_float_50_et;

  std::cout.precision(std::numeric_limits<cpp_dec_float_50_et>::digits10); // All significant digits.
  std::cout << std::showpoint << std::endl; // Show trailing zeros.

  cpp_dec_float_50_et a("1.0");
  cpp_dec_float_50_et b("1.0");
  b += std::numeric_limits<cpp_dec_float_50_et>::epsilon(); // Increment least significant decimal digit.

  cpp_dec_float_50_et eps = std::numeric_limits<cpp_dec_float_50_et>::epsilon();

  std::cout << "a = " << a << ",\nb = " << b << ",\neps = " << eps << std::endl;

  BOOST_CHECK_CLOSE(a, b, eps * 100); // Expected to pass (because tolerance is as percent).
  BOOST_CHECK_CLOSE_FRACTION(a, b, eps); // Expected to pass (because tolerance is as fraction).

  /*`Using `cpp_dec_float_50` with the default expression template use switched on,
  the compiler error message for `BOOST_CHECK_CLOSE_FRACTION(a, b, eps); would be:
  */
  // failure floating_point_comparison.hpp(59): error C2440: 'static_cast' :
  // cannot convert from 'int' to 'boost::multiprecision::detail::expression<tag,Arg1,Arg2,Arg3,Arg4>'
//] [/expression_template_1]

} // BOOST_AUTO_TEST_CASE(cpp_float_test_check_close)

/*

Output:

  Description: Autorun "J:\Cpp\big_number\Debug\test_cpp_float_close_fraction.exe"
  Running 1 test case...

  a = 1.0000000000000000000000000000000000000000000000000,
  b = 1.0000000000000000000000000000000000000000000000001,
  eps = 1.0000000000000000000000000000000000000000000000000e-49

  *** No errors detected

*/


//  Copyright 2006 John Maddock
// Copyright Paul A. Bristow 2007.

//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <pch.hpp>

#include <boost/math/concepts/real_concept.hpp>
#include <boost/test/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/ellint_rf.hpp>
#include <boost/math/special_functions/ellint_rc.hpp>
#include <boost/math/special_functions/ellint_rj.hpp>
#include <boost/math/special_functions/ellint_rd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/array.hpp>
#include <boost/tr1/random.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the Carlson Elliptic Integrals.
// There are two sets of tests, spot
// tests which compare our results with the published test values,
// in Numerical Computation of Real or Complex Elliptic Integrals,
// B. C. Carlson: http://arxiv.org/abs/math.CA/9409227
// However, the bulk of the accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these functions.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double";
   }
   else
   {
      largest_type = "long double";
   }
#else
   largest_type = "(long\\s+)?double";
#endif
   //
   // real long doubles:
   //
   if(boost::math::policies::digits<long double, boost::math::policies::policy<> >() > 53)
   {
      add_expected_result(
         ".*",                          // compiler
         ".*",                          // stdlib
         BOOST_PLATFORM,                          // platform
         largest_type,                  // test type(s)
         ".*RJ.*",      // test data group
         ".*", 1000, 50);  // test function
      add_expected_result(
         ".*",                          // compiler
         ".*",                          // stdlib
         BOOST_PLATFORM,                          // platform
         "real_concept",                  // test type(s)
         ".*RJ.*",      // test data group
         ".*", 1000, 50);  // test function
   }
   //
   // Catch all cases come last:
   //
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      ".*RJ.*",      // test data group
      ".*", 180, 50);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                  // test type(s)
      ".*RJ.*",      // test data group
      ".*", 180, 50);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      ".*",      // test data group
      ".*", 15, 8);  // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      "real_concept",                  // test type(s)
      ".*",      // test data group
      ".*", 15, 8);  // test function
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <typename T>
void do_test_ellint_rf(T& data, const char* type_name, const char* test)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   std::cout << "Testing: " << test << std::endl;

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    value_type (*fp)(value_type, value_type, value_type) = boost::math::ellint_rf<value_type, value_type, value_type>;
#else
    value_type (*fp)(value_type, value_type, value_type) = boost::math::ellint_rf;
#endif
    boost::math::tools::test_result<value_type> result;

    result = boost::math::tools::test(
      data,
      bind_func(fp, 0, 1, 2),
      extract_result(3));
   handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "boost::math::ellint_rf", test);

   std::cout << std::endl;

}

template <typename T>
void do_test_ellint_rc(T& data, const char* type_name, const char* test)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   std::cout << "Testing: " << test << std::endl;

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    value_type (*fp)(value_type, value_type) = boost::math::ellint_rc<value_type, value_type>;
#else
    value_type (*fp)(value_type, value_type) = boost::math::ellint_rc;
#endif
    boost::math::tools::test_result<value_type> result;

    result = boost::math::tools::test(
      data,
      bind_func(fp, 0, 1),
      extract_result(2));
   handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "boost::math::ellint_rc", test);

   std::cout << std::endl;

}

template <typename T>
void do_test_ellint_rj(T& data, const char* type_name, const char* test)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   std::cout << "Testing: " << test << std::endl;

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    value_type (*fp)(value_type, value_type, value_type, value_type) = boost::math::ellint_rj<value_type, value_type, value_type, value_type>;
#else
    value_type (*fp)(value_type, value_type, value_type, value_type) = boost::math::ellint_rj;
#endif
    boost::math::tools::test_result<value_type> result;

    result = boost::math::tools::test(
      data,
      bind_func(fp, 0, 1, 2, 3),
      extract_result(4));
   handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "boost::math::ellint_rf", test);

   std::cout << std::endl;

}

template <typename T>
void do_test_ellint_rd(T& data, const char* type_name, const char* test)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   std::cout << "Testing: " << test << std::endl;

#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
    value_type (*fp)(value_type, value_type, value_type) = boost::math::ellint_rd<value_type, value_type, value_type>;
#else
    value_type (*fp)(value_type, value_type, value_type) = boost::math::ellint_rd;
#endif
    boost::math::tools::test_result<value_type> result;

    result = boost::math::tools::test(
      data,
      bind_func(fp, 0, 1, 2),
      extract_result(3));
   handle_test_result(result, data[result.worst()], result.worst(),
      type_name, "boost::math::ellint_rd", test);

   std::cout << std::endl;

}

template <typename T>
void test_spots(T, const char* type_name)
{
   using namespace boost::math;
   using namespace std;
   // Spot values from Numerical Computation of Real or Complex
   // Elliptic Integrals, B. C. Carlson: http://arxiv.org/abs/math.CA/9409227
   // RF:
   T tolerance = (std::max)(T(1e-13f), tools::epsilon<T>() * 5) * 100; // Note 5eps expressed as a persentage!!!
   T eps2 = 2 * tools::epsilon<T>();
   BOOST_CHECK_CLOSE(ellint_rf(T(1), T(2), T(0)), T(1.3110287771461), tolerance);
   BOOST_CHECK_CLOSE(ellint_rf(T(0.5), T(1), T(0)), T(1.8540746773014), tolerance);
   BOOST_CHECK_CLOSE(ellint_rf(T(2), T(3), T(4)), T(0.58408284167715), tolerance);
   // RC:
   BOOST_CHECK_CLOSE_FRACTION(ellint_rc(T(0), T(1)/4), boost::math::constants::pi<T>(), eps2);
   BOOST_CHECK_CLOSE_FRACTION(ellint_rc(T(9)/4, T(2)), log(T(2)), eps2);
   BOOST_CHECK_CLOSE_FRACTION(ellint_rc(T(1)/4, T(-2)), log(T(2))/3, eps2);
   // RJ:
   BOOST_CHECK_CLOSE(ellint_rj(T(0), T(1), T(2), T(3)), T(0.77688623778582), tolerance);
   BOOST_CHECK_CLOSE(ellint_rj(T(2), T(3), T(4), T(5)), T(0.14297579667157), tolerance);
   BOOST_CHECK_CLOSE(ellint_rj(T(2), T(3), T(4), T(-0.5)), T(0.24723819703052), tolerance);
   BOOST_CHECK_CLOSE(ellint_rj(T(2), T(3), T(4), T(-5)), T(-0.12711230042964), tolerance);
   // RD:
   BOOST_CHECK_CLOSE(ellint_rd(T(0), T(2), T(1)), T(1.7972103521034), tolerance);
   BOOST_CHECK_CLOSE(ellint_rd(T(2), T(3), T(4)), T(0.16510527294261), tolerance);

   // Sanity/consistency checks from Numerical Computation of Real or Complex
   // Elliptic Integrals, B. C. Carlson: http://arxiv.org/abs/math.CA/9409227
   std::tr1::mt19937 ran;
   std::tr1::uniform_real<float> ur(0, 1000);
   T eps40 = 40 * tools::epsilon<T>();

   for(unsigned i = 0; i < 1000; ++i)
   {
      T x = ur(ran);
      T y = ur(ran);
      T z = ur(ran);
      T lambda = ur(ran);
      T mu = x * y / lambda;
      // RF, eq 49:
      T s1 = ellint_rf(x+lambda, y+lambda, lambda) +
         ellint_rf(x + mu, y + mu, mu);
      T s2 = ellint_rf(x, y, T(0));
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
      // RC is degenerate case of RF:
      s1 = ellint_rc(x, y);
      s2 = ellint_rf(x, y, y);
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
      // RC, eq 50 (Note have to assume y = x):
      T mu2 = x * x / lambda;
      s1 = ellint_rc(lambda, x+lambda)
         + ellint_rc(mu2, x + mu2);
      s2 = ellint_rc(T(0), x);
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
      /*
      T p = ????; // no closed form for a, b and p???
      s1 = ellint_rj(x+lambda, y+lambda, lambda, p+lambda)
         + ellint_rj(x+mu, y+mu, mu, p+mu);
      s2 = ellint_rj(x, y, T(0), p)
         - 3 * ellint_rc(a, b);
      */
      // RD, eq 53:
      s1 = ellint_rd(lambda, x+lambda, y+lambda)
         + ellint_rd(mu, x+mu, y+mu);
      s2 = ellint_rd(T(0), x, y)
         - 3 / (y * sqrt(x+y+lambda+mu));
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
      // RD is degenerate case of RJ:
      s1 = ellint_rd(x, y, z);
      s2 = ellint_rj(x, y, z, z);
      BOOST_CHECK_CLOSE_FRACTION(s1, s2, eps40);
   }

   //
   // Now random spot values:
   //
#include "ellint_rf_data.ipp"

   do_test_ellint_rf(ellint_rf_data, type_name, "RF: Random data");

#include "ellint_rc_data.ipp"

   do_test_ellint_rc(ellint_rc_data, type_name, "RC: Random data");

#include "ellint_rj_data.ipp"

   do_test_ellint_rj(ellint_rj_data, type_name, "RJ: Random data");

#include "ellint_rd_data.ipp"

   do_test_ellint_rd(ellint_rd_data, type_name, "RD: Random data");
}

int test_main(int, char* [])
{
    expected_results();
    BOOST_MATH_CONTROL_FP;

    boost::math::ellint_rj(1.778e-31, 1.407e+18, 10.05, -4.83e-10);

    test_spots(0.0F, "float");
    test_spots(0.0, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_spots(0.0L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
    test_spots(boost::math::concepts::real_concept(0), "real_concept");
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif

    return 0;
}

/*

test_carlson.cpp
Linking...
Embedding manifest...
Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\test_carlson.exe"
Running 1 test case...
Tests run with Microsoft Visual C++ version 8.0, Dinkumware standard library version 405, Win32
Testing: RF: Random data
boost::math::ellint_rf<float> Max = 0 RMS Mean=0
Testing: RC: Random data
boost::math::ellint_rc<float> Max = 0 RMS Mean=0
Testing: RJ: Random data
boost::math::ellint_rf<float> Max = 0 RMS Mean=0
Testing: RD: Random data
boost::math::ellint_rd<float> Max = 0 RMS Mean=0
Testing: RF: Random data
boost::math::ellint_rf<double> Max = 2.949 RMS Mean=0.7498
    worst case at row: 377
    { 3.418e+025, 2.594e-005, 3.264e-012, 6.169e-012 }
Testing: RC: Random data
boost::math::ellint_rc<double> Max = 2.396 RMS Mean=0.6283
    worst case at row: 10
    { 1.97e-029, 3.224e-025, 2.753e+012 }
Testing: RJ: Random data
boost::math::ellint_rf<double> Max = 152.9 RMS Mean=11.15
    worst case at row: 633
    { 1.876e+016, 0.000278, 3.796e-006, -4.412e-005, -1.656e-005 }
Testing: RD: Random data
boost::math::ellint_rd<double> Max = 2.586 RMS Mean=0.8614
    worst case at row: 45
    { 2.111e-020, 8.757e-026, 1.923e-023, 1.004e+033 }
Testing: RF: Random data
boost::math::ellint_rf<long double> Max = 2.949 RMS Mean=0.7498
    worst case at row: 377
    { 3.418e+025, 2.594e-005, 3.264e-012, 6.169e-012 }
Testing: RC: Random data
boost::math::ellint_rc<long double> Max = 2.396 RMS Mean=0.6283
    worst case at row: 10
    { 1.97e-029, 3.224e-025, 2.753e+012 }
Testing: RJ: Random data
boost::math::ellint_rf<long double> Max = 152.9 RMS Mean=11.15
    worst case at row: 633
    { 1.876e+016, 0.000278, 3.796e-006, -4.412e-005, -1.656e-005 }
Testing: RD: Random data
boost::math::ellint_rd<long double> Max = 2.586 RMS Mean=0.8614
    worst case at row: 45
    { 2.111e-020, 8.757e-026, 1.923e-023, 1.004e+033 }
Testing: RF: Random data
boost::math::ellint_rf<real_concept> Max = 2.949 RMS Mean=0.7498
    worst case at row: 377
    { 3.418e+025, 2.594e-005, 3.264e-012, 6.169e-012 }
Testing: RC: Random data
boost::math::ellint_rc<real_concept> Max = 2.396 RMS Mean=0.6283
    worst case at row: 10
    { 1.97e-029, 3.224e-025, 2.753e+012 }
Testing: RJ: Random data
boost::math::ellint_rf<real_concept> Max = 152.9 RMS Mean=11.15
    worst case at row: 633
    { 1.876e+016, 0.000278, 3.796e-006, -4.412e-005, -1.656e-005 }
Testing: RD: Random data
boost::math::ellint_rd<real_concept> Max = 2.586 RMS Mean=0.8614
    worst case at row: 45
    { 2.111e-020, 8.757e-026, 1.923e-023, 1.004e+033 }
*** No errors detected

*/


#include "EventShapes/EventShapes.h"

#include <Eigen/Dense>

#include <iostream>
#include <vector>

using namespace Eigen;

int test_example_eigen() {
    std::cout << "Running Eigen example" << std::endl;

    // Test eigen
    MatrixXd m(2, 2);
    m(0, 0) = 3;
    m(1, 0) = 2.5;
    m(0, 1) = -1;
    m(1, 1) = m(1, 0) + m(0, 1);
    std::cout << m << std::endl;

    std::cout << std::endl;
    return 0;
}

int test_example_eventshapes(unsigned int ndims = 2) {
    std::cout << "Running example Event Shapes tests" << std::endl;

    // Construct example input vectors
    std::vector<std::vector<float>> vs;

    EventShapes es;

    if (ndims == 3) {
        float epsilon = 0.05;
        vs.push_back({1., epsilon, 0.});
        vs.push_back({0., 1., epsilon});
        vs.push_back({epsilon, 0., 1.});

        es = EventShapes(vs, 3);

    } else {
        vs.push_back({0.5, 1.});
        vs.push_back({1., -2.5});
        vs.push_back({0., 1.5});
        vs.push_back({-1., 2.});

        es = EventShapes(vs, 2);

    }

    // Calculate
    es.calc_all();
    std::cout << "Thrust: " << es.get_thrust() << std::endl;
    std::cout << "Thrust major: " << es.get_thrust_major() << std::endl;
    std::cout << "Thrust minor: " << es.get_thrust_minor() << std::endl;
    std::cout << "Oblateness: " << es.get_oblateness() << std::endl;
    std::cout << "Broadening: " << es.get_broadening() << std::endl;
    std::cout << "S: " << es.get_lin_spher_S() << std::endl;
    std::cout << "A: " << es.get_lin_spher_A() << std::endl;
    std::cout << "C: " << es.get_lin_spher_C() << std::endl;
    std::cout << "D: " << es.get_lin_spher_D() << std::endl;

    std::cout << std::endl;
    return 0;
}

int main() {

    std::cout << "Hello, World!" << std::endl;
    std::cout << "Running package event-shapes tests" << std::endl;

    test_example_eigen();

    test_example_eventshapes(3);
    test_example_eventshapes(2);

    return 0;
}


/**
 * @file
 * @brief Implementation of the GaussLegendre function to compute
 *        quadrature Weights and nodes
 * @author Raffael Casagrande
 * @date   2018-08-11 09:08:26
 * @copyright MIT License
 */

#include "gauss_quadrature.h"
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

namespace lf::quad {
std::tuple<Eigen::VectorXd, Eigen::VectorXd> GaussLegendre(
    unsigned num_points) {
  LF_ASSERT_MSG(num_points > 0, "num_points must be positive.");

  using scalar_t =
      boost::multiprecision::number<boost::multiprecision::cpp_bin_float<
          57, boost::multiprecision::digit_base_2>>;

  static const scalar_t kPi = boost::math::constants::pi<scalar_t>();

  Eigen::VectorXd points(num_points);
  Eigen::VectorXd weights(num_points);

  // the roots are symmetric in the interval, so we only have to find half of
  // them
  unsigned int m = (num_points + 1) / 2;

  // approximation for the roots:
  for (unsigned int i = 0; i < m; ++i) {
    // initial guess for the i-th root:
    scalar_t z = cos(kPi * (i + 0.75) / (num_points + 0.5));
    scalar_t z1;
    scalar_t pp;

    // start of newton
    do {
      // calculate value of legendre polynomial at z using recurrence relation:
      scalar_t p1 = 1.0;
      scalar_t p2 = 0.0;
      scalar_t p3;

      for (unsigned int j = 0; j < num_points; ++j) {
        p3 = p2;
        p2 = p1;
        p1 = ((2.0 * j + 1.) * z * p2 - j * p3) / (j + 1.);
      }

      // p1 is now the value of the legendre polynomial at z. Next we compute
      // its derivative using a standard relation involving also p2, the
      // polynomial of one lower order:
      pp = num_points * (z * p1 - p2) / (z * z - 1.0);

      z1 = z;
      z = z1 - p1 / pp;
    } while (abs(z - z1) > 1e-17);

    points(i) = (0.5 * (1 - z)).convert_to<double>();
    points(num_points - 1 - i) = (0.5 * (1 + z)).convert_to<double>();
    weights(i) = (1. / ((1.0 - z * z) * pp * pp)).convert_to<double>();
    weights(num_points - 1 - i) = weights(i);
  }

  return {std::move(points), std::move(weights)};
}

std::tuple<Eigen::VectorXd, Eigen::VectorXd> GaussJacobi(
    quadDegree_t num_points, double alpha, double beta) {
  LF_ASSERT_MSG(num_points > 0, "num_points must be positive.");
  LF_ASSERT_MSG(alpha > -1, "alpha > -1 required");
  LF_ASSERT_MSG(beta > -1, "beta > -1 required.");

  using boost::math::lgamma;
  using scalar_t =
      boost::multiprecision::number<boost::multiprecision::cpp_bin_float<
          57, boost::multiprecision::digit_base_2>>;

  int MAX_IT = 10;

  scalar_t alfbet;
  scalar_t an;
  scalar_t bn;
  scalar_t r1;
  scalar_t r2;
  scalar_t r3;
  scalar_t a;
  scalar_t b;
  scalar_t c;
  scalar_t p1;
  scalar_t p2;
  scalar_t p3;
  scalar_t pp;
  scalar_t temp;
  scalar_t z;
  scalar_t z1;

  std::vector<scalar_t> points(num_points);
  Eigen::VectorXd weights(num_points);

  // Make an initial guess for the zeros:
  for (quadDegree_t i = 0; i < num_points; ++i) {
    if (i == 0) {
      // initial guess for the largest root
      an = alpha / num_points;
      bn = beta / num_points;
      r1 = (1.0 + alpha) *
           (2.78 / (4.0 + num_points * num_points) + 0.768 * an / num_points);
      r2 = 1.0 + 1.48 * an + 0.96 * bn + 0.452 * an * an + 0.83 * an * bn;
      z = 1.0 - r1 / r2;
    } else if (i == 1) {
      // initial guess for the second largest root
      r1 = (4.1 + alpha) / ((1.0 + alpha) * (1.0 + 0.156 * alpha));
      r2 = 1.0 + 0.06 * (num_points - 8.0) * (1.0 + 0.12 * alpha) / num_points;
      r3 = 1.0 + 0.012 * beta * (1.0 + 0.25 * std::abs(alpha)) / num_points;
      z -= (1.0 - z) * r1 * r2 * r3;
    } else if (i == 2) {
      // initial guess for the third largest root
      r1 = (1.67 + 0.28 * alpha) / (1.0 + 0.37 * alpha);
      r2 = 1.0 + 0.22 * (num_points - 8.0) / num_points;
      r3 = 1.0 + 8.0 * beta / ((6.28 + beta) * num_points * num_points);
      z -= (points[0] - z) * r1 * r2 * r3;
    } else if (i == num_points - 2) {
      // initial guess for the second smallest root
      r1 = (1.0 + 0.235 * beta) / (0.766 + 0.119 * beta);
      r2 = 1.0 / (1.0 + 0.639 * (num_points - 4.0) /
                            (1.0 + 0.71 * (num_points - 4.0)));
      r3 = 1.0 /
           (1.0 + 20.0 * alpha / ((7.5 + alpha) * num_points * num_points));
      z += (z - points[num_points - 4]) * r1 * r2 * r3;
    } else if (i == num_points - 1) {
      // initial guess for the smallest root
      r1 = (1.0 + 0.37 * beta) / (1.67 + 0.28 * beta);
      r2 = 1.0 / (1.0 + 0.22 * (num_points - 8.0) / num_points);
      r3 = 1.0 /
           (1.0 + 8.0 * alpha / ((6.28 + alpha) * num_points * num_points));
      z += (z - points[num_points - 3]) * r1 * r2 * r3;
    } else {
      // initial guess for the other points
      z = 3.0 * points[i - 1] - 3.0 * points[i - 2] + points[i - 3];
    }
    alfbet = alpha + beta;
    quadDegree_t its;
    for (its = 1; its <= MAX_IT; ++its) {
      // refinement by Newton's method
      temp = 2.0 + alfbet;

      // Start the recurrence with P_0 and P1 to avoid a division by zero when
      // alpha * beta = 0 or -1
      p1 = (alpha - beta + temp * z) / 2.0;
      p2 = 1.0;
      for (quadDegree_t j = 2; j <= num_points; ++j) {
        p3 = p2;
        p2 = p1;
        temp = 2 * j + alfbet;
        a = 2 * j * (j + alfbet) * (temp - 2.0);
        b = (temp - 1.0) *
            (alpha * alpha - beta * beta + temp * (temp - 2.0) * z);
        c = 2.0 * (j - 1 + alpha) * (j - 1 + beta) * temp;
        p1 = (b * p2 - c * p3) / a;
      }
      pp = (num_points * (alpha - beta - temp * z) * p1 +
            2.0 * (num_points + alpha) * (num_points + beta) * p2) /
           (temp * (1.0 - z * z));
      // p1 is now the desired jacobia polynomial. We next compute pp, its
      // derivative, by a standard relation involving p2, the polynomial of one
      // lower order
      z1 = z;
      z = z1 - p1 / pp;  // Newtons Formula
      if (abs(z - z1) <= 1e-17) {
        break;
      }
    }
    LF_VERIFY_MSG(its <= MAX_IT, "too many iterations.");

    points[i] = z;
    weights(i) = (exp(lgamma(alpha + num_points) + lgamma(beta + num_points) -
                      lgamma(num_points + 1.) -
                      lgamma(double(num_points) + alfbet + 1.0)) *
                  temp * pow(2.0, alfbet) / (pp * p2))
                     .convert_to<double>();
  }

  Eigen::VectorXd points_result(num_points);
  for (quadDegree_t i = 0; i < num_points; ++i) {
    points_result(i) = points[i].convert_to<double>();
  }

  return {points_result, weights};
}
}  // namespace lf::quad