#include "SimplePly.h"
#include "rply.h"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/SVD>

/**Calculates the distance the point is from the plane
   @param point a single point's position.
   @param planeEq plane Equation.
   @return distance between point and plane.
 */
float distToPlane(Eigen::Vector3f point, Eigen::Vector4f planeEq){
  float distance = planeEq(0)*point(0) + planeEq(1)*point(1) + planeEq(2)*point(2) + planeEq(3);
  distance /= std::sqrt(std::pow(planeEq(0),2) + std::pow(planeEq(1),2) + std::pow(planeEq(2),2));
  distance = std::abs(distance);
  return distance;
}

/**builds a plane out of 3 points
   @param point list of point positions.
   @return the equation of the plane.
 */
Eigen::Vector4f calcPlane(std::vector<Eigen::Vector3f> points){
  Eigen::Vector3f vectAB = points[1] - points[0], vectAC = points[2] - points[0];
  Eigen::Vector3f planeNorm = vectAB.cross(vectAC);
  float planeConst = -points[0].dot(planeNorm);
  Eigen::Vector4f planeEquation(planeNorm(0),planeNorm(1),planeNorm(2),planeConst);
  return planeEquation;
}

/**finds the distance that a point (with the greatest distance from the plane) has
   @param points list of points positions.
   @param planeEq the equation for the plane.
   @return the max distance between any point and the plane.
 */
float maxDistance(std::vector<Eigen::Vector3f> points, Eigen::Vector4f planeEq){
  float maxDist = 0, dist = 0;
  for(int i = 0; i < points.size(); i++){
    if((dist = distToPlane(points[i], planeEq)) > maxDist){
      maxDist = dist;
    }
  }
  return maxDist;
}

/**calculates the centroid of the points given.
   @param points list of points positions.
   @return the centroid of the points positions.
 */
Eigen::Vector3f calcCentroid(std::vector<Eigen::Vector3f> points){
  float aveX = 0, aveY = 0, aveZ = 0;
  Eigen::Vector3f centroid;
  for(int i = 0; i < points.size(); i++){
    aveX += points[i](0);
    aveY += points[i](1);
    aveZ += points[i](2);
  }
  //averaging the values.
  aveX/=points.size();
  aveY/=points.size();
  aveZ/=points.size();
  centroid(0) = aveX;
  centroid(1) = aveY;
  centroid(2) = aveZ;
  return centroid;
}

/** Preforms orthogonal least squares on the points given to find the best plane. Then the
    method projects the points onto the plane.
    @param points list of points location.
    @param plyPoints list of points in ply format.
*/
void leastSquares(std::vector<Eigen::Vector3f> points, std::vector<PlyPoint *> &plyPoints){
  Eigen::MatrixXf pointsMat(points.size(),3), pointsV;
  Eigen::Vector3f centroid = calcCentroid(points), planeNorm, adjustedPoint;
  Eigen::Vector4f planeEq;
  float planeNormNorm = 0, dist = 0;
  //builds matrix to use SVD on.
  for(int i = 0; i < points.size(); i++){
    for(int j = 0; j < 3; j++){
      pointsMat(i,j) = points[i](j) - centroid(j);
    }
  }
  //preforms SVD
  Eigen::JacobiSVD<Eigen::MatrixXf> svd(pointsMat, Eigen::ComputeFullV);
  pointsV = svd.matrixV();
  //builds plane based off of the SVD eigen vector and the centroid.
  for(int i = 0; i < 3; i++){
    planeEq(i) = pointsV(i,0);
    planeEq(3) -= planeEq(i)*centroid(i);
  }
  //finds normal of the plane normal.
  for(int i = 0; i < 3; i++){
    planeNormNorm += std::pow(planeEq(i),2);
  }
  planeNormNorm = std::sqrt(planeNormNorm);
  //builds a normalised plane normal vector.
  for(int i = 0; i < 3; i++){
    planeNorm(i) = planeEq(i)/planeNormNorm;
  }
  //projects points positions onto the calcuated Plane.
  for(int i = 0; i < plyPoints.size(); i++){
    dist = distToPlane(points[i],planeEq);

    adjustedPoint(0)= points[i](0) - (dist*planeNorm(0));
    adjustedPoint(1)= points[i](1) - (dist*planeNorm(1));
    adjustedPoint(2)= points[i](2) - (dist*planeNorm(2));
    if(dist < distToPlane(adjustedPoint,planeEq)){
      adjustedPoint(0)= points[i](0) + (2*dist*planeNorm(0));
      adjustedPoint(1)= points[i](1) + (2*dist*planeNorm(1));
      adjustedPoint(2)= points[i](2) + (2*dist*planeNorm(2));
    }

    (*plyPoints[i]).location(0) = adjustedPoint(0);
    (*plyPoints[i]).location(1) = adjustedPoint(1);
    (*plyPoints[i]).location(2) = adjustedPoint(2);
  }
}

/** Based on a percentage of points in the biggest plane, it calculates the threshold for the plane.
 * @param points the list of points positions.
 * @param planeEq the equation for the plane.
 * @param probThreshold the percentage of points required to determine threshold.
 * @return the calculated threshold.
 */
float calcThreshold(std::vector<Eigen::Vector3f> points, Eigen::Vector4f planeEq, float probThreshold){
  std::vector<float> buckets;
  float maxDist = maxDistance(points, planeEq);
  int numJumps = 10000, pointCount = 0, placeHold = 0, reqPoints = std::ceil(points.size()*probThreshold);
  float jumpSize = maxDist/(numJumps-1), dist, threshold = 0;

  for(int i = 0; i < numJumps; i++){
    buckets.push_back(0);
  }
  //Counts the number of Points in each region bucket.
  for(int i = 0; i < points.size(); i++){
    dist = distToPlane(points[i],planeEq);
    buckets[std::floor(dist/jumpSize)]++;
  }
  //sums point count in buckets until it goes over the users estimated plane count.
  while(pointCount < reqPoints){
    pointCount += buckets[placeHold];
    placeHold++;
  }
  //sets that point as threshold.
  threshold = (placeHold+1)*jumpSize;

  return threshold;

}

/**Determines the number of trials needed to have the accuracy that is requested.
 * @param inlierProb the probability of a point being an inlier.
 * @param acceptedFailProb the accepted rate of failure.
 *@return the number of trials required.
 */
float numChecks(float inlierProb, float acceptedFailProb){
  float numChecks = 0;
  if(inlierProb < 1){
    numChecks = ((std::log(acceptedFailProb))/(std::log(1 - (std::pow(inlierProb,3)))));
  }

  return numChecks;
}

/**Determines if the plane is a null plane.
   @param plane the plane equation.x
   @return a statement of if the plane is a null plane.
 */
bool planeNull(Eigen::Vector4f plane){
  return (abs(plane(0)) == 0 && plane(1) == 0 && plane(2) == 0 && plane(3) == 0);
}

/**Proforms analysis on the inputed point cloud, to find planes in the point cloud, colouring
   them accordingly. The program requires the user to input some information about the data,
   such as a points limit to when to stop looking for planes, an estimate of the percentage
   of points in the largest plane, and the probability of success.
*/
int main (int argc, char *argv[]) {
  int pointIn, count, bestCount, point1In, point2In, point3In, reqPlaneCalcs = 0;
  float posThresh = 0, threshProb = 0.4, threshold = 0, inlierProb = 0.001;
  Eigen::Vector4f bestPlane(0,0,0,0), curPlane;
  Eigen::Vector3f holdPoint;
  Eigen::Vector3i baseColour;
  std::vector<Eigen::Vector3f> points, pointsList, planePoints;
  int trialCount = 0,planeCount = 0;
  bool stillPlanes = true, threshFound = false;
  // Check the commandline arguments.
  if (argc != 6) {
    std::cout << "Usage: planeFinder <input file> <output file> <min num of points in plane> <percentage of points in largest plane> <accepted failure rate>" << std::endl;
    return -1;
  }
  int planeSizeThreshold = atoi(argv[3]);
  float pointPercentBiggestPlane = atof(argv[4]);
  float acceptedFailProb = atof(argv[5]);
  std::vector<PlyPoint *> pointsList2, planePoints2;

  reqPlaneCalcs = numChecks(pointPercentBiggestPlane, acceptedFailProb);
  threshold = std::numeric_limits<float>::max();
  std::cout << "Searching for planes until the next best plane size is less than " << planeSizeThreshold << std::endl;
  std::cout << "The estimated percentage of points in the biggest plane is " << pointPercentBiggestPlane << std::endl;
  std::cout << "Applying RANSAC with an acceptedFailRate of " << acceptedFailProb << std::endl;

  // Storage for the point cloud.
  SimplePly ply;
  // Read in the data from a PLY file
  std::cout << "Reading PLY data from " << argv[1] << std::endl;
  if (!ply.read(argv[1])) {
    std::cout << "Could not read PLY data from file " << argv[1] << std::endl;
    return -1;
  }

  // Stores copy of points read in.
  for(int i = 0; i < ply.size(); ++i){
    pointsList2.push_back(&ply[i]);
  }

  std::cout << "Read " << ply.size() << " points" << std::endl;

  // Recolour points - here we are just doing colour based on index
  std::cout << "Recolouring points" << std::endl;

  //Sets colours
  std::vector<Eigen::Vector3i> colours;
  colours.push_back(Eigen::Vector3i(255,0,0));
  colours.push_back(Eigen::Vector3i(0,255,0));
  colours.push_back(Eigen::Vector3i(0,0,255));
  colours.push_back(Eigen::Vector3i(255,255,0));
  colours.push_back(Eigen::Vector3i(0,255,255));
  colours.push_back(Eigen::Vector3i(255,0,255));
  colours.push_back(Eigen::Vector3i(255,255,255));
  colours.push_back(Eigen::Vector3i(0,0,0));

  holdPoint = Eigen::Vector3f(ply[0].location(0),ply[0].location(1),ply[0].location(2));

  //builds a vector containing vectors of the points positions.
  for(int i = 0; i < ply.size(); i++){
    holdPoint = Eigen::Vector3f(ply[i].location(0),ply[i].location(1),ply[i].location(2));
    pointsList.push_back(holdPoint);
  }

  //starts looking for planes.
  while(stillPlanes && pointsList.size() > 2){
    //zeros bestPlane.
    for(int j = 0; j < 4; j++){
      bestPlane(j) = 0;
    }

    //increase plane Count.

    planeCount++;
    //Resets variables.
    bestCount = 0;
    inlierProb = 0.001;
    trialCount = 0;

    //finds the threshold based off of users estimate.
    if(!threshFound){
      for(int z = 0; z < 110; z++){
	point1In = std::rand()%pointsList.size();
	point2In = std::rand()%pointsList.size();
	point3In = std::rand()%pointsList.size();
	points.clear();
	points.push_back(pointsList[point1In]);
	points.push_back(pointsList[point2In]);
	points.push_back(pointsList[point3In]);
	curPlane = calcPlane(points);
	if(!planeNull(curPlane)){
	  posThresh = calcThreshold(pointsList, curPlane, pointPercentBiggestPlane);
	  if(posThresh < threshold){
	    threshold = posThresh;
	    bestPlane = curPlane;
	  }
	}
      }
      //makes sure only one threshold is found.
      threshFound = true;
    }

    //starts looking for planes with the calculated threshold.
    while(trialCount <  numChecks(inlierProb, acceptedFailProb)){
      trialCount++;
      std::cout << numChecks(inlierProb, acceptedFailProb) << std::endl;
      //finds three random points and builds a plane.
      point1In = std::rand()%pointsList.size();
      point2In = std::rand()%pointsList.size();
      point3In = std::rand()%pointsList.size();
      points.clear();
      points.push_back(pointsList[point1In]);
      points.push_back(pointsList[point2In]);
      points.push_back(pointsList[point3In]);
      curPlane = calcPlane(points);
      count = 0;
      //makes sure it isn't a null plane.
      if(!planeNull(curPlane)){
	for(int k = 0; k < pointsList.size(); k++){
	  //counts number of points within threshold.
	  if(distToPlane(pointsList[k], curPlane) <= threshold){
	    count++;
	  }
	}
	//checks if this is best plane then updates if it is.
	if(count > bestCount){
	  bestCount = count;
	  bestPlane = curPlane;
	  inlierProb = ((float)bestCount)/pointsList.size();

	}
      }
    }
    //Cuts out if the plane is too small.
    if(bestCount < planeSizeThreshold){
      stillPlanes = false;
    }
    //Makes sure the unlikely chance of a null best plane occuring doesn't get through.
    else if(!planeNull(bestPlane)){
      //recolours and removes points.
      for(int j = pointsList2.size() - 1; j > -1; j--){
	holdPoint(0) = (*pointsList2[j]).location(0);
	holdPoint(1) =  (*pointsList2[j]).location(1);
	holdPoint(2) =  (*pointsList2[j]).location(2);

	if(distToPlane(holdPoint, bestPlane) <= threshold){

	  (*pointsList2[j]).colour = colours[planeCount%colours.size()];
	  //planePoints.push_back(pointsList[j]);
	  pointsList[j] = pointsList[pointsList.size() - 1];
	  pointsList.pop_back();
	  //planePoints2.push_back(pointsList2[j]);
	  pointsList2[j] = pointsList2[pointsList2.size() - 1];
	  pointsList2.pop_back();

	}
      }
      //would do least squares if it worked properly. No time to work out the error.
      //leastSquares(planePoints, planePoints2);
      //planePoints.clear();
      //planePoints2.clear();
    }
  }

  // Write the resulting (re-coloured) point cloud to a PLY file.
  std::cout << "Writing PLY data to " << argv[2] << std::endl;
  if (!ply.write(argv[2])) {
    std::cout << "Could not write PLY data to file " << argv[2] << std::endl;
    return -2;
  }
  return 0;
}


/* Boost example/transc.cpp
 * how to use an external library (GMP/MPFR in this case) in order to
 * get correct transcendental functions on intervals
 *
 * Copyright 2003 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval.hpp>
//extern "C" {
#include <gmp.h>
#include <mpfr.h>
//}
#include <iostream>

struct full_rounding:
  boost::numeric::interval_lib::rounded_arith_opp<double>
{
private:
  typedef int mpfr_func(mpfr_t, const __mpfr_struct*, mp_rnd_t);
  double invoke_mpfr(double x, mpfr_func f, mp_rnd_t r) {
    mpfr_t xx;
    mpfr_init_set_d(xx, x, GMP_RNDN);
    f(xx, xx, r);
    double res = mpfr_get_d(xx, r);
    mpfr_clear(xx);
    return res;
  }
public:
# define GENR_FUNC(name) \
  double name##_down(double x) { return invoke_mpfr(x, mpfr_##name, GMP_RNDD); } \
  double name##_up  (double x) { return invoke_mpfr(x, mpfr_##name, GMP_RNDU); }
  GENR_FUNC(exp)
  GENR_FUNC(log)
  GENR_FUNC(sin)
  GENR_FUNC(cos)
  GENR_FUNC(tan)
  GENR_FUNC(asin)
  GENR_FUNC(acos)
  GENR_FUNC(atan)
  GENR_FUNC(sinh)
  GENR_FUNC(cosh)
  GENR_FUNC(tanh)
  GENR_FUNC(asinh)
  GENR_FUNC(acosh)
  GENR_FUNC(atanh)
};

namespace dummy {
  using namespace boost;
  using namespace numeric;
  using namespace interval_lib;
  typedef save_state<full_rounding> R;
  typedef checking_strict<double> P;
  typedef interval<double, policies<R, P> > I;
};

typedef dummy::I I;

template<class os_t>
os_t& operator<<(os_t &os, const I &a) {
  os << '[' << a.lower() << ',' << a.upper() << ']';
  return os;
}

int main() {
  I x(0.5, 2.5);
  std::cout << "x = " << x << std::endl;
  std::cout.precision(16);
# define GENR_TEST(name) \
  std::cout << #name "(x) = " << name(x) << std::endl
  GENR_TEST(exp);
  GENR_TEST(log);
  GENR_TEST(sin);
  GENR_TEST(cos);
  GENR_TEST(tan);
  GENR_TEST(asin);
  GENR_TEST(acos);
  GENR_TEST(atan);
  GENR_TEST(sinh);
  GENR_TEST(cosh);
  GENR_TEST(tanh);
  GENR_TEST(asinh);
  GENR_TEST(acosh);
  GENR_TEST(atanh);
  return 0;
}


// find_mean_and_sd_normal.cpp

// Copyright Paul A. Bristow 2007, 2010.

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

// Example of finding mean or sd for normal distribution.

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[normal_std
/*`
First we need some includes to access the normal distribution,
the algorithms to find location and scale
(and some std output of course).
*/

#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.
#include <boost/math/distributions/cauchy.hpp> // for cauchy_distribution
  using boost::math::cauchy; // typedef provides default type is double.
#include <boost/math/distributions/find_location.hpp>
  using boost::math::find_location;
#include <boost/math/distributions/find_scale.hpp>
  using boost::math::find_scale;
  using boost::math::complement;
  using boost::math::policies::policy;

#include <iostream>
  using std::cout; using std::endl; using std::left; using std::showpoint; using std::noshowpoint;
#include <iomanip>
  using std::setw; using std::setprecision;
#include <limits>
  using std::numeric_limits;
#include <stdexcept>
  using std::exception;
//] [/normal_std Quickbook]

int main()
{
  cout << "Find_location (mean) and find_scale (standard deviation) examples." << endl;
  try
  {

//[normal_find_location_and_scale_eg

/*`
[h4 Using find_location and find_scale to meet dispensing and measurement specifications]

Consider an example from K Krishnamoorthy,
Handbook of Statistical Distributions with Applications,
ISBN 1-58488-635-8, (2006) p 126, example 10.3.7.

"A machine is set to pack 3 kg of ground beef per pack.
Over a long period of time it is found that the average packed was 3 kg
with a standard deviation of 0.1 kg.
Assume the packing is normally distributed."

We start by constructing a normal distribution with the given parameters:
*/

double mean = 3.; // kg
double standard_deviation = 0.1; // kg
normal packs(mean, standard_deviation);
/*`We can then find the fraction (or %) of packages that weigh more than 3.1 kg.
*/

double max_weight = 3.1; // kg
cout << "Percentage of packs > " << max_weight << " is "
<< cdf(complement(packs, max_weight)) * 100. << endl; // P(X > 3.1)

/*`We might want to ensure that 95% of packs are over a minimum weight specification,
then we want the value of the mean such that P(X < 2.9) = 0.05.

Using the mean of 3 kg, we can estimate
the fraction of packs that fail to meet the specification of 2.9 kg.
*/

double minimum_weight = 2.9;
cout <<"Fraction of packs <= " << minimum_weight << " with a mean of " << mean
  << " is " << cdf(complement(packs, minimum_weight)) << endl;
// fraction of packs <= 2.9 with a mean of 3 is 0.841345

/*`This is 0.84 - more than the target fraction of 0.95.
If we want 95% to be over the minimum weight,
what should we set the mean weight to be?

Using the KK StatCalc program supplied with the book and the method given on page 126 gives 3.06449.

We can confirm this by constructing a new distribution which we call 'xpacks'
with a safety margin mean of 3.06449 thus:
*/
double over_mean = 3.06449;
normal xpacks(over_mean, standard_deviation);
cout << "Fraction of packs >= " << minimum_weight
<< " with a mean of " << xpacks.mean()
  << " is " << cdf(complement(xpacks, minimum_weight)) << endl;
// fraction of packs >= 2.9 with a mean of 3.06449 is 0.950005

/*`Using this Math Toolkit, we can calculate the required mean directly thus:
*/
double under_fraction = 0.05;  // so 95% are above the minimum weight mean - sd = 2.9
double low_limit = standard_deviation;
double offset = mean - low_limit - quantile(packs, under_fraction);
double nominal_mean = mean + offset;
// mean + (mean - low_limit - quantile(packs, under_fraction));

normal nominal_packs(nominal_mean, standard_deviation);
cout << "Setting the packer to " << nominal_mean << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(nominal_packs, minimum_weight)) << endl;
// Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95

/*`
This calculation is generalized as the free function called
[link math_toolkit.dist.dist_ref.dist_algorithms find_location].

To use this we will need to
*/

#include <boost/math/distributions/find_location.hpp>
  using boost::math::find_location;
/*`and then use find_location function to find safe_mean,
& construct a new normal distribution called 'goodpacks'.
*/
double safe_mean = find_location<normal>(minimum_weight, under_fraction, standard_deviation);
normal good_packs(safe_mean, standard_deviation);
/*`with the same confirmation as before:
*/
cout << "Setting the packer to " << nominal_mean << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(good_packs, minimum_weight)) << endl;
// Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95

/*`
[h4 Using Cauchy-Lorentz instead of normal distribution]

After examining the weight distribution of a large number of packs, we might decide that,
after all, the assumption of a normal distribution is not really justified.
We might find that the fit is better to a __cauchy_distrib.
This distribution has wider 'wings', so that whereas most of the values
are closer to the mean than the normal, there are also more values than 'normal'
that lie further from the mean than the normal.

This might happen because a larger than normal lump of meat is either included or excluded.

We first create a __cauchy_distrib with the original mean and standard deviation,
and estimate the fraction that lie below our minimum weight specification.
*/

cauchy cpacks(mean, standard_deviation);
cout << "Cauchy Setting the packer to " << mean << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(cpacks, minimum_weight)) << endl;
// Cauchy Setting the packer to 3 will mean that fraction of packs >= 2.9 is 0.75

/*`Note that far fewer of the packs meet the specification, only 75% instead of 95%.
Now we can repeat the find_location, using the cauchy distribution as template parameter,
in place of the normal used above.
*/

double lc = find_location<cauchy>(minimum_weight, under_fraction, standard_deviation);
cout << "find_location<cauchy>(minimum_weight, over fraction, standard_deviation); " << lc << endl;
// find_location<cauchy>(minimum_weight, over fraction, packs.standard_deviation()); 3.53138
/*`Note that the safe_mean setting needs to be much higher, 3.53138 instead of 3.06449,
so we will make rather less profit.

And again confirm that the fraction meeting specification is as expected.
*/
cauchy goodcpacks(lc, standard_deviation);
cout << "Cauchy Setting the packer to " << lc << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(goodcpacks, minimum_weight)) << endl;
// Cauchy Setting the packer to 3.53138 will mean that fraction of packs >= 2.9 is 0.95

/*`Finally we could estimate the effect of a much tighter specification,
that 99% of packs met the specification.
*/

cout << "Cauchy Setting the packer to "
  << find_location<cauchy>(minimum_weight, 0.99, standard_deviation)
  << " will mean that "
  << "fraction of packs >= " << minimum_weight
  << " is " << cdf(complement(goodcpacks, minimum_weight)) << endl;

/*`Setting the packer to 3.13263 will mean that fraction of packs >= 2.9 is 0.99,
but will more than double the mean loss from 0.0644 to 0.133 kg per pack.

Of course, this calculation is not limited to packs of meat, it applies to dispensing anything,
and it also applies to a 'virtual' material like any measurement.

The only caveat is that the calculation assumes that the standard deviation (scale) is known with
a reasonably low uncertainty, something that is not so easy to ensure in practice.
And that the distribution is well defined, __normal_distrib or __cauchy_distrib, or some other.

If one is simply dispensing a very large number of packs,
then it may be feasible to measure the weight of hundreds or thousands of packs.
With a healthy 'degrees of freedom', the confidence intervals for the standard deviation
are not too wide, typically about + and - 10% for hundreds of observations.

For other applications, where it is more difficult or expensive to make many observations,
the confidence intervals are depressingly wide.

See [link math_toolkit.dist.stat_tut.weg.cs_eg.chi_sq_intervals Confidence Intervals on the standard deviation]
for a worked example
[@../../../example/chi_square_std_dev_test.cpp chi_square_std_dev_test.cpp]
of estimating these intervals.

[h4 Changing the scale or standard deviation]

Alternatively, we could invest in a better (more precise) packer
(or measuring device) with a lower standard deviation, or scale.

This might cost more, but would reduce the amount we have to 'give away'
in order to meet the specification.

To estimate how much better (how much smaller standard deviation) it would have to be,
we need to get the 5% quantile to be located at the under_weight limit, 2.9
*/
double p = 0.05; // wanted p th quantile.
cout << "Quantile of " << p << " = " << quantile(packs, p)
  << ", mean = " << packs.mean() << ", sd = " << packs.standard_deviation() << endl;
/*`
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1

With the current packer (mean = 3, sd = 0.1), the 5% quantile is at 2.8551 kg,
a little below our target of 2.9 kg.
So we know that the standard deviation is going to have to be smaller.

Let's start by guessing that it (now 0.1) needs to be halved, to a standard deviation of 0.05 kg.
*/
normal pack05(mean, 0.05);
cout << "Quantile of " << p << " = " << quantile(pack05, p)
  << ", mean = " << pack05.mean() << ", sd = " << pack05.standard_deviation() << endl;
// Quantile of 0.05 = 2.91776, mean = 3, sd = 0.05

cout <<"Fraction of packs >= " << minimum_weight << " with a mean of " << mean
  << " and standard deviation of " << pack05.standard_deviation()
  << " is " << cdf(complement(pack05, minimum_weight)) << endl;
// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.97725
/*`
So 0.05 was quite a good guess, but we are a little over the 2.9 target,
so the standard deviation could be a tiny bit more. So we could do some
more guessing to get closer, say by increasing standard deviation to 0.06 kg,
constructing another new distribution called pack06.
*/
normal pack06(mean, 0.06);
cout << "Quantile of " << p << " = " << quantile(pack06, p)
  << ", mean = " << pack06.mean() << ", sd = " << pack06.standard_deviation() << endl;
// Quantile of 0.05 = 2.90131, mean = 3, sd = 0.06

cout <<"Fraction of packs >= " << minimum_weight << " with a mean of " << mean
  << " and standard deviation of " << pack06.standard_deviation()
  << " is " << cdf(complement(pack06, minimum_weight)) << endl;
// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.95221
/*`
Now we are getting really close, but to do the job properly,
we might need to use root finding method, for example the tools provided,
and used elsewhere, in the Math Toolkit, see
[link math_toolkit.toolkit.internals1.roots2  Root Finding Without Derivatives].

But in this (normal) distribution case, we can and should be even smarter
and make a direct calculation.
*/

/*`Our required limit is minimum_weight = 2.9 kg, often called the random variate z.
For a standard normal distribution, then probability p = N((minimum_weight - mean) / sd).

We want to find the standard deviation that would be required to meet this limit,
so that the p th quantile is located at z (minimum_weight).
In this case, the 0.05 (5%) quantile is at 2.9 kg pack weight, when the mean is 3 kg,
ensuring that 0.95 (95%) of packs are above the minimum weight.

Rearranging, we can directly calculate the required standard deviation:
*/
normal N01; // standard normal distribution with meamn zero and unit standard deviation.
p = 0.05;
double qp = quantile(N01, p);
double sd95 = (minimum_weight - mean) / qp;

cout << "For the "<< p << "th quantile to be located at "
  << minimum_weight << ", would need a standard deviation of " << sd95 << endl;
// For the 0.05th quantile to be located at 2.9, would need a standard deviation of 0.0607957

/*`We can now construct a new (normal) distribution pack95 for the 'better' packer,
and check that our distribution will meet the specification.
*/

normal pack95(mean, sd95);
cout <<"Fraction of packs >= " << minimum_weight << " with a mean of " << mean
  << " and standard deviation of " << pack95.standard_deviation()
  << " is " << cdf(complement(pack95, minimum_weight)) << endl;
// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0607957 is 0.95

/*`This calculation is generalized in the free function find_scale,
as shown below, giving the same standard deviation.
*/
double ss = find_scale<normal>(minimum_weight, under_fraction, packs.mean());
cout << "find_scale<normal>(minimum_weight, under_fraction, packs.mean()); " << ss << endl;
// find_scale<normal>(minimum_weight, under_fraction, packs.mean()); 0.0607957

/*`If we had defined an over_fraction, or percentage that must pass specification
*/
double over_fraction = 0.95;
/*`And (wrongly) written

 double sso = find_scale<normal>(minimum_weight, over_fraction, packs.mean());

With the default policy, we would get a message like

[pre
Message from thrown exception was:
   Error in function boost::math::find_scale<Dist, Policy>(double, double, double, Policy):
   Computed scale (-0.060795683191176959) is <= 0! Was the complement intended?
]

But this would return a *negative* standard deviation - obviously impossible.
The probability should be 1 - over_fraction, not over_fraction, thus:
*/

double ss1o = find_scale<normal>(minimum_weight, 1 - over_fraction, packs.mean());
cout << "find_scale<normal>(minimum_weight, under_fraction, packs.mean()); " << ss1o << endl;
// find_scale<normal>(minimum_weight, under_fraction, packs.mean()); 0.0607957

/*`But notice that using '1 - over_fraction' - will lead to a
[link why_complements loss of accuracy, especially if over_fraction was close to unity.]
In this (very common) case, we should instead use the __complements,
giving the most accurate result.
*/

double ssc = find_scale<normal>(complement(minimum_weight, over_fraction, packs.mean()));
cout << "find_scale<normal>(complement(minimum_weight, over_fraction, packs.mean())); " << ssc << endl;
// find_scale<normal>(complement(minimum_weight, over_fraction, packs.mean())); 0.0607957

/*`Note that our guess of 0.06 was close to the accurate value of 0.060795683191176959.

We can again confirm our prediction thus:
*/

normal pack95c(mean, ssc);
cout <<"Fraction of packs >= " << minimum_weight << " with a mean of " << mean
  << " and standard deviation of " << pack95c.standard_deviation()
  << " is " << cdf(complement(pack95c, minimum_weight)) << endl;
// Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0607957 is 0.95

/*`Notice that these two deceptively simple questions:

* Do we over-fill to make sure we meet a minimum specification (or under-fill to avoid an overdose)?

and/or

* Do we measure better?

are actually extremely common.

The weight of beef might be replaced by a measurement of more or less anything,
from drug tablet content, Apollo landing rocket firing, X-ray treatment doses...

The scale can be variation in dispensing or uncertainty in measurement.
*/
//] [/normal_find_location_and_scale_eg Quickbook end]

  }
  catch(const exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << endl;
  }
  return 0;
}  // int main()

/*

Output is:

//[normal_find_location_and_scale_output

Find_location (mean) and find_scale (standard deviation) examples.
Percentage of packs > 3.1 is 15.8655
Fraction of packs <= 2.9 with a mean of 3 is 0.841345
Fraction of packs >= 2.9 with a mean of 3.06449 is 0.950005
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95
Setting the packer to 3.06449 will mean that fraction of packs >= 2.9 is 0.95
Cauchy Setting the packer to 3 will mean that fraction of packs >= 2.9 is 0.75
find_location<cauchy>(minimum_weight, over fraction, standard_deviation); 3.53138
Cauchy Setting the packer to 3.53138 will mean that fraction of packs >= 2.9 is 0.95
Cauchy Setting the packer to -0.282052 will mean that fraction of packs >= 2.9 is 0.95
Quantile of 0.05 = 2.83551, mean = 3, sd = 0.1
Quantile of 0.05 = 2.91776, mean = 3, sd = 0.05
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.05 is 0.97725
Quantile of 0.05 = 2.90131, mean = 3, sd = 0.06
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.06 is 0.95221
For the 0.05th quantile to be located at 2.9, would need a standard deviation of 0.0607957
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0607957 is 0.95
find_scale<normal>(minimum_weight, under_fraction, packs.mean()); 0.0607957
find_scale<normal>(minimum_weight, under_fraction, packs.mean()); 0.0607957
find_scale<normal>(complement(minimum_weight, over_fraction, packs.mean())); 0.0607957
Fraction of packs >= 2.9 with a mean of 3 and standard deviation of 0.0607957 is 0.95

//] [/normal_find_location_and_scale_eg_output]

*/


#ifndef RSVD_ERROR_ESTIMATORS_HPP_
#define RSVD_ERROR_ESTIMATORS_HPP_

#include <Eigen/Dense>

namespace Rsvd {

/// \brief Compute the relative Frobenius error of a matrix approximation.
///
/// \long The formula for the relative error is
/// \f[ \varepsilon = \frac{ \| \tilde{A} - A \|_{\mathrm{F}} }{ \| A \|_{\mathrm{F}} }. \f]
///
/// Both matrices can be real or complex.
///
/// \tparam MatrixType Eigen matrix type.
///
/// \param reference Reference matrix \f$ A \f$. It is assumed that this matrix has positive
/// Frobenius norm.
///
/// \param approximation Approximation \f$ \tilde{A} \f$ of the reference matrix.
///
/// \return Relative Frobenius error \f$ \varepsilon \f$ of the approximation (non-negative real).
template <typename MatrixType>
typename Eigen::NumTraits<typename MatrixType::Scalar>::Real
relativeFrobeniusNormError(const MatrixType &reference, const MatrixType &approximation) {
  const auto differenceNorm = (approximation - reference).norm();
  const auto referenceNorm = reference.norm();

  assert(referenceNorm > 0);

  return differenceNorm / referenceNorm;
}

} // namespace Rsvd

#endif


#include <iostream>
#include <sstream>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>

using big_int = boost::multiprecision::cpp_int;

big_int ipow(big_int base, big_int exp) {
  big_int result(1);
  while (exp) {
    if (exp & 1) {
      result *= base;
    }
    exp >>= 1;
    base *= base;
  }
  return result;
}

big_int ackermann(unsigned m, unsigned n) {
  static big_int (*ack)(unsigned, big_int) =
      [](unsigned m, big_int n)->big_int {
    switch (m) {
    case 0:
      return n + 1;
    case 1:
      return n + 2;
    case 2:
      return 3 + 2 * n;
    case 3:
      return 5 + 8 * (ipow(big_int(2), n) - 1);
    default:
      return n == 0 ? ack(m - 1, big_int(1)) : ack(m - 1, ack(m, n - 1));
    }
  };
  return ack(m, big_int(n));
}

int main() {
  for (unsigned m = 0; m < 4; ++m) {
    for (unsigned n = 0; n < 10; ++n) {
      std::cout << "A(" << m << ", " << n << ") = " << ackermann(m, n) << "\n";
    }
  }

  std::cout << "A(4, 1) = " << ackermann(4, 1) << "\n";

  std::stringstream ss;
  ss << ackermann(4, 2);
  auto text = ss.str();
  std::cout << "A(4, 2) = (" << text.length() << " digits)\n"
            << text.substr(0, 80) << "\n...\n"
            << text.substr(text.length() - 80) << "\n";
}


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "mkl.h"
#include "mkl_lapacke.h"

namespace votca { namespace tools {

using namespace std;

void linalg_cholesky_decompose( ub::matrix<double> &A){
    // Cholesky decomposition using MKL
    // input matrix A will be changed

    // LAPACK variables
    MKL_INT info;
    MKL_INT n = A.size1();
    char uplo = 'L';

    // pointer for LAPACK
    double * pA = const_cast<double*>(&A.data().begin()[0]);
    info = LAPACKE_dpotrf( LAPACK_ROW_MAJOR , uplo , n, pA, n );
    if ( info != 0 )
        throw std::runtime_error("Matrix not symmetric positive definite");
}

void linalg_cholesky_decompose( ub::matrix<float> &A){
    // Cholesky decomposition using MKL
    // input matrix A will be changed

    // LAPACK variables
    MKL_INT info;
    MKL_INT n = A.size1();
    char uplo = 'L';

    // pointer for LAPACK
    float * pA = const_cast<float*>(&A.data().begin()[0]);
    info = LAPACKE_spotrf( LAPACK_ROW_MAJOR , uplo , n, pA, n );
    if ( info != 0 )
        throw std::runtime_error("Matrix not symmetric positive definite");
}

void linalg_cholesky_solve( ub::vector<double> &x, ub::matrix<double> &A, ub::vector<double> &b ){
    /* calling program should catch the error error code
     * thrown by LAPACKE_dpotrf and take
     * necessary steps
     */

    // LAPACK variables
    MKL_INT info;
    MKL_INT n = A.size1();
    char uplo = 'L';

    // pointer for LAPACK LU factorization of input matrix
    double * pA = const_cast<double*>(&A.data().begin()[0]); // input array

    // get LU factorization
    info = LAPACKE_dpotrf( LAPACK_ROW_MAJOR , uplo , n, pA, n );

    if ( info != 0 )
        throw std::runtime_error("Matrix not symmetric positive definite");

    MKL_INT nrhs = 1;

    // pointer of LAPACK LU solver
    double * pb = const_cast<double*>(&b.data()[0]);
    info = LAPACKE_dpotrs(LAPACK_ROW_MAJOR, uplo, n, nrhs, pA, n, pb, n );

    // on output, b contains solution
    x = b;
}

}}


#include "pch.h"
#include <Eigen/Dense>
#include "ReverseDistortionEstimator.h"
#include "photogrammetry.h"

ReverseDistortionEstimator::ReverseDistortionEstimator(const Camera& cam)
{
	//solves for the inverse distortion (like opencv)
	//see: https://docs.opencv.org/3.4.6/d9/d0c/group__calib3d.html

	int x_spacing = cam.W / 10;
	int y_spacing = cam.H / 10;
	vector<std::array<double, 2> > grid_points;
	vector<std::array<double, 2> > grid_points_undistorted;
	vector<std::array<double, 2> > dist_corrections;
	grid_points.reserve(200);

	//calculating the estimation grid:
	for (int x = 0; x <= cam.W; x += x_spacing)
	{
		for (int y = 0; y <= cam.H; y += y_spacing)
		{
			grid_points.push_back({
				static_cast<double>(x),
				static_cast<double>(y) });
		}
	}
	grid_points.shrink_to_fit();

	//transformation from column/row coordinates to image-centered coordinates
	std::transform(grid_points.begin(), grid_points.end(), grid_points.begin(),
		[&](std::array<double,2>& p ){
		return std::array<double, 2>{
			p[0] = p[0] - cam.W / 2.0 + 0.5,
			p[1] = cam.H / 2.0 - p[1] - 0.5}; });

	//transformation from image-centered coordinates to fiducial coordiantes
	std::transform(grid_points.begin(), grid_points.end(), grid_points.begin(),
		[&](std::array<double, 2>& p) {
		return std::array<double, 2>{
			p[0] = p[0] - cam.InternalOrientation[1],
			p[1] = p[1] - cam.InternalOrientation[2]}; });

	grid_points_undistorted.resize(grid_points.size());
	dist_corrections.resize(grid_points.size());

	std::transform(grid_points.begin(), grid_points.end(), dist_corrections.begin(),
		[&](std::array<double, 2>& p) {
		return std::array<double, 2>{
			fT_xDistCorrection(cam.RadialDistortion, cam.TangentialDistortion, p[0], p[1]),
			fT_yDistCorrection(cam.RadialDistortion, cam.TangentialDistortion, p[0], p[1])}; });

	for (size_t i = 0, S = grid_points.size(); i < S; i++)
	{
		grid_points_undistorted.at(i)[0] = grid_points.at(i)[0] - dist_corrections.at(i)[0];
		grid_points_undistorted.at(i)[1] = grid_points.at(i)[1] - dist_corrections.at(i)[1];
	}

	//solving the reversed distortion using Eieng linear algerbra tools:
	//each coordinate generates one equation:
	Eigen::MatrixXd A(2 * grid_points_undistorted.size(), 5);
	Eigen::VectorXd L(2 * grid_points_undistorted.size());
	Eigen::VectorXd X(5);
	Eigen::VectorXd V(2 * grid_points_undistorted.size());

	for (size_t i = 0, S = grid_points_undistorted.size(); i < S; i++)
	{
		//we are estimating in the normalized space so w have to use normalized values
		double x = grid_points_undistorted.at(i)[0] /cam.InternalOrientation[0];
		double y = -grid_points_undistorted.at(i)[1] /cam.InternalOrientation[0];
		double dx = dist_corrections.at(i)[0] / cam.InternalOrientation[0];
		double dy = -dist_corrections.at(i)[1] / cam.InternalOrientation[0];
		double rr = x * x + y * y;

		A(2 * i, 0) = x * rr;
		A(2 * i, 1) = x * rr*rr;
		A(2 * i, 2) = x * rr*rr*rr;
		A(2 * i, 3) = 2 * x * y;
		A(2 * i, 4) = rr + 2 * x*x;
		L(2 * i) = dx;

		A(2 * i + 1, 0) = y * rr;
		A(2 * i + 1, 1) = y * rr*rr;
		A(2 * i + 1, 2) = y * rr*rr*rr;
		A(2 * i + 1, 3) = rr + 2 * y * y;
		A(2 * i + 1, 4) = 2 * x * y;
		L(2 * i + 1) = dy;
	}

	X = A.colPivHouseholderQr().solve(L);
	V = L - A * X;
	RMSE = std::sqrt((1.0 /(L.rows()-1)) * V.transpose()*V);

	for (int i = 0; i < 5; i++) DistortionYDown.at(i) = X(i);

}

ReverseDistortionEstimator::~ReverseDistortionEstimator()
{
}


#include "mnist_loader.h"
#include "node.h"

#include <iostream>
#include <memory>
#include <Eigen/Core>

#define EIGEN_MPL2_ONLY

#define INPUT_SIZE  784
#define HIDDEN_SIZE 50
#define OUTPUT_SIZE 10

const double VAR_DIFF  = 1.0e-4;
const double MOD_RATIO = 1.0e-2;

namespace {
	Eigen::VectorXf ReLu(Eigen::VectorXf &val)
	{
		Eigen::VectorXf ret(val.rows());

		for(int i = 0 ; i < val.rows() ; i++)
		{
			ret[i] = fmax(0, val[i]);
		}

		return ret;
	}

	Eigen::VectorXf Softmax(Eigen::VectorXf &val)
	{
		Eigen::VectorXf ret(val.rows());

		double sum = 0;
		double *exp_array = new double[val.rows()];

		for(int i = 0 ; i < val.rows() ; i++)
		{
			exp_array[i] = exp(val[i]);
			sum += exp_array[i];
		}

		// printf("softmax sum = %f\n", sum);
		for(int i = 0 ; i < val.rows() ; i++)
		{
			ret[i] = exp_array[i] / sum;
		}

		delete[] exp_array;

		return ret;
	}

	Eigen::VectorXf MakeOnehotVector(int size, int index)
	{
		Eigen::VectorXf ret = Eigen::VectorXf::Zero(size);
		ret[index] = 1;
		return ret;
	}

	float SquareError(Eigen::VectorXf vec1, Eigen::VectorXf vec2)
	{
		assert(vec1.rows() == vec2.rows());
		assert(vec1.cols() == vec2.cols());
		assert(vec1.cols() == 1);

		float error = 0;
		for(int i = 0 ; i < vec1.rows() ; i++)
		{
			float v = vec1[i] - vec2[i];
			error += v * v;
		}

		return error;
	}

	int MaxIndex(Eigen::VectorXf vec)
	{
		int max_index = 0;
		for(int i = 0 ; i < vec.rows() ; i++)
		{
			if(vec[max_index] < vec[i])
			{
				max_index = i;
			}
		}
		return max_index;
	}
};

Eigen::VectorXf Array2VectorXf(uint8_t *array, int size)
{
	Eigen::VectorXf ret(size);

	// this conversion needs due to different data format
	for(int i = 0 ; i < size ; i++)
	{
		ret[i] = array[i];
	}

	return ret;
}

Eigen::VectorXf Predict(Node &node1, Node &node2, Eigen::VectorXf input)
{
	Eigen::VectorXf temp;

	temp = node1.Calc(input);
	temp = ReLu(temp);
	temp = node2.Calc(temp);
	temp = Softmax(temp);

	return temp;
}

double CalcError(Node &node1, Node &node2, Eigen::VectorXf input, Eigen::VectorXf ans)
{
	Eigen::VectorXf output     = Predict(node1, node2, input);
	double          error_rate = SquareError(output, ans);

	return error_rate;
}

int main()
{
	std::vector<std::shared_ptr<Image> > train_image_list;
	std::vector<uint8_t>                 train_label_list;
	std::vector<std::shared_ptr<Image> > test_image_list;
	std::vector<uint8_t>                 test_label_list;

	Eigen::initParallel();
	Eigen::setNbThreads(4);

	printf("[INFO] load image...\n");
	std::shared_ptr<MnistLoader> loader = std::make_shared<MnistLoader>();
	if(loader->LoadImage("../train-images-idx3-ubyte", train_image_list) == false)
	{
		printf("[ERROR] load data error.\n");
	}

	if(loader->LoadLabel("../train-labels-idx1-ubyte", train_label_list) == false)
	{
		printf("[ERROR] load data error.\n");
	}

	if(loader->LoadImage("../t10k-images-idx3-ubyte", test_image_list) == false)
	{
		printf("[ERROR] load data error.\n");
	}

	if(loader->LoadLabel("../t10k-labels-idx1-ubyte", test_label_list) == false)
	{
		printf("[ERROR] load data error.\n");
	}

	Node node1(INPUT_SIZE, HIDDEN_SIZE);
	if(node1.Load("weight_node1") == false)
	{
		printf("[INFO] node1 weight value initialize with random value.\n");
	}
	Node node2(HIDDEN_SIZE, OUTPUT_SIZE);
	if(node2.Load("weight_node2") == false)
	{
		printf("[INFO] node2 weight value initialize with random value.\n");
	}

	printf("[INFO] training...\n");
	sranddev();
	// for(int image_index = 0 ; image_index < train_image_list.size() ; image_index++)
	// for(int image_index = 0 ; image_index < 100 ; image_index++)
	for(int train_count = 0 ; train_count < 100 ; train_count++)
	{
		int image_index = rand() % train_image_list.size();

//		if(image_index % 100 == 0)
//		{
//			printf("[INFO] exec %d/%d\n", image_index, (int)train_label_list.size());
//		}

		std::shared_ptr<Image> image = train_image_list[image_index];
		Eigen::VectorXf        input = Array2VectorXf(image->image, image->image_size);
		int                    ans   = train_label_list[image_index];
		Eigen::VectorXf        ans_vec = MakeOnehotVector(OUTPUT_SIZE, ans);
//		printf("image_index = %d, ans = %d\n", image_index, ans);

		input.normalize();

		double *node1_mod = new double[node1.GetMaxWeightIndex()];
		double *node2_mod = new double[node2.GetMaxWeightIndex()];

		for(int i = 0 ; i < node1.GetMaxWeightIndex() ; i++)
		{
			double dyp = 0;
			double dyn = 0;

			node1.PushWeightDiff(i, VAR_DIFF);
			dyp = CalcError(node1, node2, input, ans_vec);
			node1.PopWeightDiff();

			node1.PushWeightDiff(i, -VAR_DIFF);
			dyn = CalcError(node1, node2, input, ans_vec);
			node1.PopWeightDiff();

			node1_mod[i] = (dyp - dyn) / (2.0 * VAR_DIFF);

//			printf("dyp=%f, dyn=%f, mod=%f\n", dyp, dyn, node1_mod[i]);
		}

		for(int i = 0 ; i < node2.GetMaxWeightIndex() ; i++)
		{
			double dyp = 0;
			double dyn = 0;

			node2.PushWeightDiff(i, VAR_DIFF);
			dyp = CalcError(node1, node2, input, ans_vec);
			node2.PopWeightDiff();

			node2.PushWeightDiff(i, -VAR_DIFF);
			dyn = CalcError(node1, node2, input, ans_vec);
			node2.PopWeightDiff();

			node2_mod[i] = (dyp - dyn) / (2.0 * VAR_DIFF);

//			printf("dyp=%lf, dyn=%lf, mod=%lf\n", dyp, dyn, node2_mod[i]);
		}

		for(int i = 0 ; i < node1.GetMaxWeightIndex() ; i++)
		{
			node1.AddWeight(i, node1_mod[i] * -MOD_RATIO);
		}

		for(int i = 0 ; i < node2.GetMaxWeightIndex() ; i++)
		{
			node2.AddWeight(i, node2_mod[i] * -MOD_RATIO);
		}

		delete[] node1_mod;
		delete[] node2_mod;

		//return 1;

//		if(image_index == 10)
//		{
//			// optimization test
//			return 1;
//		}
	}

	node1.Save("weight_node1");
	node2.Save("weight_node2");

	printf("[INFO] test...\n");
	std::vector<uint8_t> result_list;
	for(int image_index = 0 ; image_index < test_image_list.size() ; image_index++)
	{
		std::shared_ptr<Image> image = test_image_list[image_index];
		Eigen::VectorXf        input = Array2VectorXf(image->image, image->image_size);

		input.normalize();
		Eigen::VectorXf output = Predict(node1, node2, input);

		int result = MaxIndex(output);
		result_list.push_back(result);

	}

	assert(result_list.size() == test_label_list.size());
	int success_count = 0;
	int fail_count    = 0;
	for(int i = 0 ; i < result_list.size() ; i++)
	{
//		printf("result = %d, test = %d,", result_list[i], test_label_list[i]);
		if(result_list[i] == test_label_list[i])
		{
			success_count++;
		}else
		{
			fail_count++;
		}
	}
	float accuracy = success_count / (float)(success_count + fail_count);
	std::cout << "accuracy = " << accuracy << std::endl;

	return 0;
}


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

int main(int argc, char **argv)
{
  // 100x100 Random Matrix
  Eigen::MatrixXd Rand = Eigen::MatrixXd::Random(100, 100);
  Eigen::MatrixXd A = Rand * Rand.transpose();
  Eigen::VectorXd b_ = Eigen::VectorXd::Random(100);
  Eigen::VectorXd x_ = Eigen::VectorXd::Zero(100);

  clock_t t_start = clock();

  // LU Decomposition
  x_ = A.partialPivLu().solve(b_);  // A must be Invertible
  std::cout << "time of partialPivLu() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  x_ = A.fullPivLu().solve(b_);
  std::cout << "time of fullPivLu() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  // QR Decomposition
  x_ = A.householderQr().solve(b_);
  std::cout << "time of householderQr() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  x_ = A.colPivHouseholderQr().solve(b_);
  std::cout << "time of colPivHouseholderQr() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  x_ = A.fullPivHouseholderQr().solve(b_);
  std::cout << "time of fullPivHouseholderQr() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  // Cholesky Decomposition
  x_ = A.llt().solve(b_); // A must be Positive Definite
  std::cout << "time of llt() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  x_ = A.ldlt().solve(b_);  // A must be Positive or Negative Semidefinite
  std::cout << "time of ldlt() is " << 1000 * (clock() - t_start) / (double) CLOCKS_PER_SEC << "ms" << std::endl;
  // std::cout << "x = " << x_.transpose() << std::endl;
  t_start = clock();

  return EXIT_SUCCESS;
}


//
// Dynamics of a single pendulum.
// State space is 2-d [\theta, \dot\theta]: angle, angular velocity
// If \theta=\pi is straight up, \theta=0 is straight down
//

# pragma once

#include <Eigen/Dense>

#include <experiments/simulators/simulator_utils.hh>

namespace simulators
{
namespace pendulum
{

template<int dim>
using Vector = simulators::Vector<dim>;

enum State
{
    THETA = 0,
    THETADOT,
    STATE_DIM
};

enum Control
{
    TORQUE = 0,
    CONTROL_DIM
};

// Useful for holding the parameters of the pendulum, including integration timestep.
class Pendulum
{
public:
    Pendulum(const double dt, const double damping_coeff, const double length);
    // Discrete time dynamics.
    Vector<STATE_DIM> operator()(const Vector<STATE_DIM>& x, const Vector<CONTROL_DIM>& u);
private:
    double dt_;
    double damping_coeff_;
    double length_;

};

// Returns a function that can return x_{t+1} = f(x_t, u_t) for a chosen dt.
// Underlying uses RK4 integration.
ilqr::DynamicsFunc make_discrete_dynamics_func(const double dt, const double length, const double damping_coeff);

// Defines pendulum dynamics as xdot = f(x,u).
//Eigen::VectorXd continuous_dynamics(const Eigen::VectorXd& state, const Eigen::VectorXd& control,
//                                    const double length, const double damping_coeff);

Vector<STATE_DIM> continuous_dynamics(const Vector<STATE_DIM>& x, const Vector<CONTROL_DIM>& u,
                                    const double length, const double damping_coeff);
} // namespace pendulum
} // namespace simulators


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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.

#ifndef SMOOTH__SO3_HPP_
#define SMOOTH__SO3_HPP_

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

#include "internal/lie_group_base.hpp"
#include "internal/macro.hpp"
#include "internal/so3.hpp"
#include "lie_group.hpp"
#include "map.hpp"

namespace smooth {

// \cond
template<typename Scalar>
class SO2;
// \endcond

/**
 * @brief Base class for SO3 Lie group types
 *
 * Internally represented as a member of \f$ \mathbb{S}^3 \f$ (unit quaternions).
 *
 * Memory layout
 * -------------
 *
 * - Group:    \f$ \mathbf{x} = [q_x, q_y, q_z, q_w] \f$ (same as Eigen quaternion).
 * - Tangent:  \f$ \mathbf{a} = [\omega_x, \omega_y, \omega_z] \f$
 *
 * Constraints
 * -----------
 *
 * - Group:   \f$ q_x^2 + q_y^2 + q_z^2 + q_w^2 = 1, q_w >= 0 \f$
 * - Tangent: \f$ -\pi < \omega_x, \omega_y, \omega_z \leq \pi \f$
 *
 * Lie group matrix form
 * ---------------------
 *
 * 3x3 rotation matrix
 *
 * Lie algebra matrix form
 * -----------------------
 *
 * \f[
 * \mathbf{a}^\wedge =
 * \begin{bmatrix}
 *   0 & -\omega_z & \omega_y \\
 *  \omega_z &   0 & -\omega_x \\
 *  -\omega_y &   \omega_x & 0 \\
 * \end{bmatrix} \in \mathbb{R}^{3 \times 3}
 * \f]
 */
template<typename _Derived>
class SO3Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;

protected:
  SO3Base() = default;

public:
  SMOOTH_INHERIT_TYPEDEFS;

  /**
   * @brief Access quaterion.
   */
  Eigen::Map<Eigen::Quaternion<Scalar>> quat() requires is_mutable
  {
    return Eigen::Map<Eigen::Quaternion<Scalar>>(static_cast<_Derived &>(*this).data());
  }

  /**
   * @brief Access const quaterion.
   */
  Eigen::Map<const Eigen::Quaternion<Scalar>> quat() const
  {
    return Eigen::Map<const Eigen::Quaternion<Scalar>>(static_cast<const _Derived &>(*this).data());
  }

  /**
   * @brief Return euler angles.
   *
   * @param i1, i2, i3 euler angle axis convention (\f$0=x, 1=y, 2=z\f$).
   *        Default values correspond to ZYX rotation.
   *
   * Returned angles a1, a2, a3 are s.t. rotation is described by
   * \f$ Rot_{i_1}(a_1) * Rot_{i_2}(a_2) * Rot_{i_3}(a_3). \f$
   * where \f$ Rot_i(a) \f$ rotates an angle \f$a\f$ around the \f$i\f$:th axis.
   */
  Eigen::Vector3<Scalar>
  eulerAngles(Eigen::Index i1 = 2, Eigen::Index i2 = 1, Eigen::Index i3 = 0) const
  {
    return quat().toRotationMatrix().eulerAngles(i1, i2, i3);
  }

  /**
   * @brief Rotation action on 3D vector.
   */
  template<typename EigenDerived>
  Eigen::Vector3<Scalar> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return quat() * v;
  }

  /**
   * @brief Project to SO2.
   *
   * This keeps the yaw/z axis component of the rotation.
   *
   * @note SO2 header must be included.
   */
  SO2<Scalar> project_so2() const
  {
    using std::atan2;

    const auto q = quat();
    Scalar yaw   = atan2(
      Scalar(2) * (q.w() * q.z() + q.x() * q.y()),
      Scalar(1) - Scalar(2) * (q.y() * q.y() + q.z() * q.z()));
    return SO2<Scalar>(yaw);
  }
};

// STORAGE TYPE TRAITS

// \cond
template<typename _Scalar>
class SO3;
// \endcond

// \cond
template<typename _Scalar>
struct liebase_info<SO3<_Scalar>>
{
  static constexpr bool is_mutable = true;

  using Impl   = SO3Impl<_Scalar>;
  using Scalar = _Scalar;

  template<typename NewScalar>
  using PlainObject = SO3<NewScalar>;
};
// \endcond

/**
 * @brief Storage implementation of SO3 Lie group.
 *
 * @see SO3Base for group API.
 */
template<typename _Scalar>
class SO3 : public SO3Base<SO3<_Scalar>>
{
  using Base = SO3Base<SO3<_Scalar>>;

  SMOOTH_GROUP_API(SO3);

public:
  /**
   * @brief Construct from quaternion.
   *
   * @param quat Eigen quaternion.
   *
   * @note Input is normalized inside constructor.
   */
  template<typename Derived>
  SO3(const Eigen::QuaternionBase<Derived> & quat) : coeffs_(quat.normalized().coeffs())
  {
    if (coeffs_(3) < 0) { coeffs_ *= Scalar(-1); }
  }

  /**
   * @brief SO3 representing rotation around the x axis
   * @param angle angle of rotation [rad]
   */
  static SO3 rot_x(const Scalar & angle)
  {
    using std::cos, std::sin;

    SO3 ret;
    ret.coeffs() << sin(angle / 2), Scalar(0), Scalar(0), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }

  /**
   * @brief SO3 representing rotation around the y axis
   * @param angle angle of rotation [rad]
   */
  static SO3 rot_y(const Scalar & angle)
  {
    using std::cos, std::sin;

    SO3 ret;
    ret.coeffs() << Scalar(0), sin(angle / 2), Scalar(0), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }

  /**
   * @brief SO3 representing rotation around the z axis
   * @param angle angle of rotation [rad]
   */
  static SO3 rot_z(const Scalar & angle)
  {
    using std::cos, std::sin;

    SO3 ret;
    ret.coeffs() << Scalar(0), Scalar(0), sin(angle / 2), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }
};

// \cond
template<typename _Scalar>
struct liebase_info<Map<SO3<_Scalar>>> : public liebase_info<SO3<_Scalar>>
{};
// \endcond

/**
 * @brief Memory mapping of SO3 Lie group.
 *
 * @see SO3Base for group API.
 */
template<typename _Scalar>
class Map<SO3<_Scalar>> : public SO3Base<Map<SO3<_Scalar>>>
{
  using Base = SO3Base<Map<SO3<_Scalar>>>;

  SMOOTH_MAP_API();
};

// \cond
template<typename _Scalar>
struct liebase_info<Map<const SO3<_Scalar>>> : public liebase_info<SO3<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond

/**
 * @brief Const memory mapping of SO3 Lie group.
 *
 * @see SO3Base for group API.
 */
template<typename _Scalar>
class Map<const SO3<_Scalar>> : public SO3Base<Map<const SO3<_Scalar>>>
{
  using Base = SO3Base<Map<const SO3<_Scalar>>>;

  SMOOTH_CONST_MAP_API();
};

using SO3f = SO3<float>;   ///< SO3 with float scalar representation
using SO3d = SO3<double>;  ///< SO3 with double scalar representation

}  // namespace smooth

#endif  // SMOOTH__SO3_HPP_


#pragma once
#include <Eigen/Dense>

namespace mdk {
    using VRef = Eigen::Vector3d const&;

    inline double angle(VRef v1, VRef v2, VRef v3) {
        auto u1 = v2 - v1, u2 = v2 - v3;
        return acos(u1.dot(u2) / (u1.norm() * u2.norm()));
    }

    inline double dihedral(VRef v1, VRef v2, VRef v3, VRef v4) {
        auto u1 = v2 - v1, u2 = v3 - v2, u3 = v4 - v3;
        auto d1 = u1.cross(u2), d2 = u2.cross(u3);

        if (d1.isZero() || d2.isZero()) return 0.0;
        double phi = acos(d1.dot(d2) / (d1.norm() * d2.norm()));
        if (d1.dot(u3) < 0.) phi = -phi;
        return phi;
    }

    double asphericity(Vectors const& v) {
        Vector mean = v.vectorwise().mean();

        Eigen::Matrix3d mit;
        double crg;
        for (size_t i = 0; i < v.size(); ++i) {
            Vector diff = v.col(i) - mean;
            crg += diff.squaredNorm();

            Vector sq_diff = diff.array() * diff.array();
            mit(0,0) += sq_diff(1) + sq_diff(2);
            mit(1,1) += sq_diff(2) + sq_diff(0);
            mit(2,2) += sq_diff(0) + sq_diff(1);
            mit(0,1) -= diff(0) * diff(1);
            mit(1,2) -= diff(1) * diff(2);
            mit(0,2) -= diff(0) * diff(2);
        }
        mit(1,0) = mit(0,1);
        mit(2,1) = mit(1,2);
        mit(2,0) = mit(0,2);
        crg = sqrt(crg / v.size());

        auto eigenvaluesC = mit.eigenvalues();
        double eigenvalues[3] = {
            sqrt(eigenvaluesC(0).real() / v.size()),
            sqrt(eigenvaluesC(1).real() / v.size()),
            sqrt(eigenvaluesC(2).real() / v.size())
        };

        std::sort(eigenvalues, eigenvalues + 3);
        return eigenvalues[1] - 0.5 * (eigenvalues[0] + eigenvalues[2]);
    }
}


/*
Copyright 2012-2018 Tamas Bolner

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 <cinttypes>
#include <Eigen/Dense>
#include "SimplexSolver.h"
#include "exception.h"

using namespace Eigen;

/**
 * Constructor
 *
 * @param int mode This can be one of these: SIMPLEX_MINIMIZE, SIMPLEX_MAXIMIZE
 * @param const VectorXd &objectiveFunction The coefficients of the objective function.
 * @param const MatrixXd &constraints Full matrix for the constraints. Contains also the righthand-side values.
 * @returns SimplexSolver
 */
SimplexSolver::SimplexSolver(int mode, const VectorXd &objectiveFunction, const MatrixXd &constraints) {
    int64_t constantColumn, temp;
    this->foundSolution = false;
    this->optimum = 0;
    this->numberOfVariables = objectiveFunction.rows();

    /*
        Validate input parameters
    */
    if (mode != SIMPLEX_MINIMIZE && mode != SIMPLEX_MAXIMIZE) {
        throw FException("SimplexSolver: invalid value for the 'mode' parameter.");
    }

    if (objectiveFunction.rows() < 1) {
        throw FException("SimplexSolver: The coefficient vector of the objective function must contain at least one row.");
    }

    if (constraints.rows() < 1) {
        throw FException("SimplexSolver: The constraint matrix must contain at least one row.");
    }

    if (constraints.cols() != objectiveFunction.rows() + 1) {
        throw FException("SimplexSolver: The constraint matrix has %d columns, but should have %d, because the coefficient vector of the objective function has %d rows.",
            constraints.cols(), objectiveFunction.rows() + 1, objectiveFunction.rows());
    }

    for (int i = 0; i < this->numberOfVariables; i++) {
        if (objectiveFunction(i) == 0) {
            throw FException("SimplexSolver: One of the coefficients of the objective function is zero.");
        }
    }

    temp = constraints.cols() - 1;
    for (int i = 0; i < constraints.rows(); i++) {
        if (constraints(i, temp) < 0) {
            throw FException("SimplexSolver: All righthand-side coefficients of the constraint matrix must be non-negative.");
        }
    }

    /*
        Build tableau
    */
    if (mode == SIMPLEX_MAXIMIZE) {
        // Maximize
        this->tableau.resize(constraints.rows() + 1, this->numberOfVariables + constraints.rows() + 1);

        this->tableau <<    -objectiveFunction.transpose(),                   MatrixXd::Zero(1, constraints.rows() + 1),
                            constraints.leftCols(this->numberOfVariables),    MatrixXd::Identity(constraints.rows(), constraints.rows()), constraints.rightCols(1);
    } else {
        // Minimize: construct the Dual problem
        this->tableau.resize(this->numberOfVariables + 1, this->numberOfVariables + constraints.rows() + 1);

        this->tableau <<    -constraints.rightCols(1).transpose(),                        MatrixXd::Zero(1, this->numberOfVariables + 1),
                            constraints.leftCols(this->numberOfVariables).transpose(),    MatrixXd::Identity(this->numberOfVariables, this->numberOfVariables), objectiveFunction;
    }

    /*
        Simplex algorithm
    */
    if (mode == SIMPLEX_MAXIMIZE) {
        // Maximize original problem
        if (!this->simplexAlgorithm(this->numberOfVariables)) {
            return;    // No solution
        }
    } else {
        // Maximize the dual of the minimization problem
        if (!this->simplexAlgorithm(constraints.rows())) {
            return;    // No solution
        }
    }

    /*
        Fetch solution
    */
    constantColumn = this->tableau.cols() - 1;
    this->solution.resize(this->numberOfVariables);

    if (mode == SIMPLEX_MAXIMIZE) {
        // Maximize
        for (int i = 0; i < this->numberOfVariables; i++) {
            temp = this->getPivotRow(i);

            if (temp > 0) {
                // Basic variable
                this->solution(i) = this->tableau(temp, constantColumn);
            } else {
                // Non-basic variable
                this->solution(i) = 0;
            }
        }
        this->foundSolution = true;
        this->optimum = this->tableau(0, constantColumn);
    } else {
        // Minimize
        for (int i = 0; i < this->numberOfVariables; i++) {
            this->solution(i) = this->tableau(0, constraints.rows() + i);
        }
        this->foundSolution = true;
        this->optimum = this->tableau(0, constantColumn);
    }
}

/**
 * Returns true if a solution has been found.
 * Return false otherwise.
 *
 * @returns boolean
 */
bool SimplexSolver::hasSolution() {
    return this->foundSolution;
}

/**
 * Returns the maximum/minimum value of
 * the objective function.
 *
 * @returns double
 */
double SimplexSolver::getOptimum() {
    return this->optimum;
}

/**
 * Returns a vector with the variable
 * values for the solution.
 *
 * return VectorXd
 */
VectorXd SimplexSolver::getSolution() {
    return this->solution;
}

/**
 * Searches for the pivot row in the given column, by calculating the ratios.
 * Tries to find smallest non-negative ratio.
 * Returns -1 if all possible pivots are 0 or if the ratios are negative.
 * Deals with cases like this:  0/negative < 0/positive
 *
 * @param int64_t column
 * @returns int64_t Returns the number of the pivot row, or -1 if found none.
 */
int64_t SimplexSolver::findPivot_min(int64_t column) {
    int64_t minIndex = -1;
    int64_t constantColumn = this->tableau.cols() - 1;
    double minRatio = 0;
    double minConstant = 0;    // For the "0/negative < 0/positive" difference the constants have to be tracked also.
    double ratio;
    int64_t rowNum = this->tableau.rows();

    for (int i = 1; i < rowNum; i++) {
        if (this->tableau(i, column) == 0) {
            continue;
        }

        ratio = this->tableau(i, constantColumn) / this->tableau(i, column);
        if (ratio < 0) {
            //The ratio must be non-negative
            continue;
        }

        if (minIndex == -1) {
            // First pivot candidate
            minIndex = i;
            minRatio = ratio;
            minConstant = this->tableau(i, constantColumn);
        } else {
            if (ratio == 0 && ratio == minRatio) {
                // 0/negative < 0/positive
                if (this->tableau(i, constantColumn) < minConstant) {
                    minIndex = i;
                    minRatio = ratio;
                    minConstant = this->tableau(i, constantColumn);
                }
            } else if (ratio < minRatio) {
                minIndex = i;
                minRatio = ratio;
                minConstant = this->tableau(i, constantColumn);
            }
        }
    }

    return minIndex;
}

/**
 * Iterates through the this->tableau matrix to solve the problem.
 *
 * @param int64_t variableNum The number of variables (dimensions). (different for the minimization problem)
 * @returns bool Returns true if a solution has been found. Returns false otherwise.
 */
bool SimplexSolver::simplexAlgorithm(int64_t variableNum) {
    MatrixXd::Index pivotColumn;
    int64_t pivotRow;

    while (true) {
        /*
            Find pivot column, check for halt condition
        */
        this->tableau.row(0).leftCols(variableNum).minCoeff(&pivotColumn);
        if (this->tableau(0, pivotColumn) >= 0) {
            //Found no negative coefficient
            break;
        }

        /*
            Find pivot row
        */
        pivotRow = this->findPivot_min(pivotColumn);
        if (pivotRow == -1) {
            //no solution
            return false;
        }

        /*
            Do pivot operation
        */
        this->tableau.row(pivotRow) /= this->tableau(pivotRow, pivotColumn);
        this->tableau(pivotRow, pivotColumn) = 1;    // For possible precision issues
        for (int i = 0; i < this->tableau.rows(); i++) {
            if (i == pivotRow) continue;

            this->tableau.row(i) -= this->tableau.row(pivotRow) * this->tableau(i, pivotColumn);
            this->tableau(i, pivotColumn) = 0;    // For possible precision issues
        }
    }

    return true;
}

/**
 * If the given column has only one coefficient with value 1 (except in topmost row), and all other
 * coefficients are zero, then returns the row of the non-zero value.
 * Otherwise return -1.
 * This method is used in the final step of maximization, when we read
 * the solution from the tableau.
 *
 * @param int64_t column
 * @returns int64_t
 */
int64_t SimplexSolver::getPivotRow(int64_t column) {
    int64_t one_row = -1;

    for (int i = 1; i < this->tableau.rows(); i++) {
        if (this->tableau(i, column) == 1) {
            if (one_row >= 0) {
                return -1;
            } else {
                one_row = i;
                continue;
            }
        } else if (this->tableau(i, column) != 0) {
            return -1;
        }
    }

    return one_row;
}


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

// #include <Python.h>
// #include "matplotlibcpp.h"

// namespace plt = matplotlibcpp;
using namespace std;
using namespace Eigen;

void ucitajTacke(vector<pair<float, float>> &tacke, int duzina) {
	pair<float, float> tacka;
	for (int i = 0; i < duzina; i++) {
		cin >> tacka.first >> tacka.second;
		tacke.push_back(tacka);
	}
}

MatrixXf kreirajMatricuIx3(vector<pair<float, float>> &tacke) {
	MatrixXf Mat(tacke.size(), 3);

	int i = 0;
	while (i < tacke.size()) {
		Mat(i, 0) = tacke[i].first;
		Mat(i, 1) = tacke[i].second;
		Mat(i, 2) = 1;
		i++;
	}
	/*
		| t[0][p1] t[0][p2] 1 |
		| t[1][p1] t[1][p2] 1 |
		| t[2][p1] t[3][p2] 1 |
		| t[4][p1] t[4][p2] 1 |
		| ........			  |
	*/
	return Mat;
}

// Svaka tacka i njena slika iz vektora pravi ovu matricu 2 x 9
// Za broj tacaka n, pravi matricu 2n x 9
MatrixXf kreirajMatricu2nx9(vector<pair<float, float>> &pocetne, vector<pair<float, float>> &projektovane) {

	MatrixXf matrix2nx9(2 * pocetne.size(), 9);

	auto i = pocetne.begin();
	auto j = projektovane.begin();
	int k = 0;

	while (i < pocetne.end()) {
		matrix2nx9(k, 0) = 0;
		matrix2nx9(k, 1) = 0;
		matrix2nx9(k, 2) = 0;
		matrix2nx9(k, 3) = -i->first;
		matrix2nx9(k, 4) = -i->second;
		matrix2nx9(k, 5) = -1;
		matrix2nx9(k, 6) = j->second * i->first;
		matrix2nx9(k, 7) = j->second * i->second;
		matrix2nx9(k, 8) = j->second;

		matrix2nx9(k + 1, 0) = i->first;
		matrix2nx9(k + 1, 1) = i->second;
		matrix2nx9(k + 1, 2) = 1;
		matrix2nx9(k + 1, 3) = 0;
		matrix2nx9(k + 1, 4) = 0;
		matrix2nx9(k + 1, 5) = 0;
		matrix2nx9(k + 1, 6) = -(j->first * i->first);
		matrix2nx9(k + 1, 7) = -(j->first * i->second);
		matrix2nx9(k + 1, 8) = -(j->first);

		i++;
		j++;
		k += 2;
		/// |  0	 0    0  -b[0] -b[1] -1   (p[1]*b[0])   (p[1]*b[1])   p[1]	|		b - par pocetnih koordinata tacaka
		///	| p[0]  p[1]  1   0     0     0  -(p[0]*b[0])  -(p[0]*b[1])  -p[0]	|		p - par projektovanih koordinata tacaka
		///	|	.	.	.	.	.	.	.	.	.	.	.	.	.	.		.	|
		/// |	.	.	.	.	.	.	.	.	.	.	.	.	.	.		.	|
		/// |	.	.	.	.	.	.	.	.	.	.	.	.	.	.		.	|
	}

	return matrix2nx9;
}

MatrixXf resiP(MatrixXf Mat, Vector3f vec, int k) {
	Vector3f stepen = ((Mat.transpose()).inverse()) * vec;

	int m = stepen.size();
	int n = k;
	MatrixXf P(n, m);
	for (int i = 0; i < n; i++) {
		for (int j = 0; j < m; j++) {
			P(i, j) = stepen(i) * Mat(i, j);
		}
	}
	return P.transpose();
}

MatrixXf naivniAlgoritam(vector<pair<float, float>> pocetne, vector<pair<float, float>> projektovane) {

	float x, y;

	x = pocetne.back().first;
	y = pocetne.back().second;
	Vector3f v(x, y, 1);

	x = projektovane.back().first;
	y = projektovane.back().second;
	Vector3f vp(x, y, 1);

	pocetne.pop_back();
	projektovane.pop_back();

	MatrixXf M = kreirajMatricuIx3(pocetne);
	MatrixXf Mp = kreirajMatricuIx3(projektovane);

	MatrixXf P1 = resiP(M, v, pocetne.size());
	MatrixXf P2 = resiP(Mp, vp, projektovane.size());

	return P2 * P1.inverse();
}

MatrixXf DLTAlgoritam(vector<pair<float, float>> pocetne, vector<pair<float, float>> projektovane) {

	int br_tacaka = pocetne.size();

	MatrixXf matrix2nx9 = kreirajMatricu2nx9(pocetne, projektovane);
	// SVD razbija matricu na proizvode 3 matrice
	JacobiSVD<MatrixXf> Svd(matrix2nx9, ComputeFullV);

	MatrixXf Mat(2 * br_tacaka, 9);
	MatrixXf matrica_poslednje_kolone(3, 3);
	Mat = Svd.matrixV();	// Uzimamo 3. matricu V iz A = USV*

	int br_kolona = Mat.cols();
	// Uzimamo poslednju kolonu iz dekompozicije
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			matrica_poslednje_kolone(i, j) = Mat(i * 3 + j, br_kolona - 1);
		}
	}

	return matrica_poslednje_kolone;
}

float izrSrednjeRastojanje(vector<pair<float, float>> tacke, pair<float, float> tezite) {

	float prosecno_rastojanje = 0;
	int n = tacke.size();
	for (auto i = tacke.begin(); i < tacke.end(); i++) {
		float medju_rastojanje = sqrt(pow(i->first - tezite.first, 2) + pow(i->second - tezite.second, 2));	// sqrt((x2-x1)^2 + (y2-y1)^2)
		prosecno_rastojanje += medju_rastojanje;
	}

	return prosecno_rastojanje / n;
}

Vector3f izrCentroidu(vector<pair<float, float>> tacke) {

	float tx = 0;
	float ty = 0;
	int n = tacke.size();

	// Sabiramo tezine koordinata
	for (auto i = tacke.begin(); i < tacke.end(); i++) {
		tx += i->first;
		ty += i->second;
	}

	// centroida = (tx/n, ty/n, 1)
	// homogene
	Vector3f centroida;
	centroida << tx / n, ty / n, 1;

	/*
	// Test: teziste
	cout << "Teziste je : " << centroida;
	cout << endl;
	*/

	return centroida;
}

MatrixXf izrMatricuNormalizacije(vector<pair<float, float>> tacke) {
	/*
	1. Izracunati teziste sistema tacaka
	2. Translirati teziste u koordinatni pocetak (matrica T)
	3. Skalirati tacke tako da prosecna udaljenost tacke od koordinatnog pocetka bude sqrt(2) (matrica homotetije S)
	4. Matrica normalizacije je jednaka S * T
	*/
	Vector3f vteziste = izrCentroidu(tacke);
	pair<float, float> t(vteziste(0), vteziste(1));

	// Matrica T: Translira teziste ovog skupa tacaka u O(0,0)
	MatrixXf T(3, 3);
	T << 1, 0, -vteziste(0),
		 0, 1, -vteziste(1),
		 0, 0,	    1;
	// cout << T << endl;
	float srednje_rastojanje = izrSrednjeRastojanje(tacke, t);
	float k_skaliranja = sqrt(2) / srednje_rastojanje;
	// Matrica S: Skalira, tako da prosecno rastojanje tacaka od koordinatnog pocetka bude jednak sqrt(2)
	MatrixXf S(3, 3);
	S << k_skaliranja,		0,			0,
			0,			k_skaliranja,	0,
			0,				0,			1;
	// cout << S << endl;
	// Prvo transliramo pa skaliramo
	return S * T;
}

MatrixXf nDLTAlgoritam(vector<pair<float, float>> pocetne, vector<pair<float, float>> projektovane) {
	/*
	1. Normalizuj tacke i slike:	nx[i] = T * x[i], npx[i] = Tp * px[i]
	2. Uradi DLT na:				nx[i] <-> npx[i]
	3. Denormalizuj resenje:		H = Tp_inv * nH * T
	*/

	MatrixXf T = izrMatricuNormalizacije(pocetne);
	MatrixXf s_tacka = (T * kreirajMatricuIx3(pocetne).transpose()).transpose();	// 3x3 * nx3.transp
	//	cout << T << endl << endl << s_tacka << endl << endl;

	MatrixXf Tp = izrMatricuNormalizacije(projektovane);
	MatrixXf p_tacka = (Tp * kreirajMatricuIx3(projektovane).transpose()).transpose();	// 3x3 * nx3.transp
	//	cout << Tp << endl << endl << p_tacka << endl << endl;

	int n = pocetne.size();
	vector<pair<float, float>> s;
	for (int i = 0; i < n; i++) {
		s.push_back(pair<float, float>(s_tacka(i, 0), s_tacka(i, 1)));
		//	cout << s_tacka(i, 0) << " " << s_tacka(i, 1) << endl;
	}

	vector<pair<float, float>> p;
	for (int i = 0; i < n; i++) {
		p.push_back(pair<float, float>(p_tacka(i, 0), p_tacka(i, 1)));
		//	cout << p_tacka(i, 0) << " " << p_tacka(i, 1) << endl;
	}

	/*
	////	---- Plot ----
	for (int i = 0; i < n; i++) {
		plt::plot(s_tacka(i,0), s_tacka(i, 1), 'go');
		plt::plot(p_tacka(i, 0), p_tacka(i, 1), 'g+');
	}
	*/

	MatrixXf H = DLTAlgoritam(s, p);
	return Tp.inverse() * H * T;
}

int main() {
	int opcija;
	cout << "Izaberite opciju: " << endl << "1. Rucni unos koordinata" << endl << "2. Izabir koordinata pomocu misa" << endl << ":::  ";
	cin >> opcija;
	if (opcija == 1) {				// Rucni unos koordinata
		// Unos tacaka
		int br_tacaka;
		cout << "Unesite broj korespodencija: ";
		cin >> br_tacaka;
		if (br_tacaka < 0) {
			cout << "greska: Neispravan unos! (ocekivan broj veci od 0)" << endl;
			return -1;

		}

		vector<pair<float, float>> pocetne_tacke;
		vector<pair<float, float>> projektovane_tacke;

		cout << "Ucitajte ulazne tacke kao parove x, y: " << endl;
		ucitajTacke(pocetne_tacke, br_tacaka);
		cout << "Ucitajte izlazne tacke kao parove x, y: " << endl;
		ucitajTacke(projektovane_tacke, br_tacaka);
		cout << endl;
		if (br_tacaka == 4) {
			cout << "--- Naivni algoritam: ---" << endl;
			cout << naivniAlgoritam(pocetne_tacke, projektovane_tacke) << endl;

			/*
			////	---- Plot ----
			for (int i = 0; i < br_tacaka; i++) {
				plt::plot(pocetne_tacke[i].first, pocetne_tacke[i].second, 'ro');
				plt::plot(projektovane_tacke[i].first, projektovane_tacke[i].second, 'r+');
			}
			*/

		}

		cout << "--- DLT algoritam: ---" << endl;
		cout << DLTAlgoritam(pocetne_tacke, projektovane_tacke) << endl;

		cout << "---Normalizovani DLT algoritam: ---" << endl;
		cout << nDLTAlgoritam(pocetne_tacke, projektovane_tacke);

		//	ERROR: Error	C1083	Cannot open include file : 'Python.h' : No such file or directory
		//	VS ne cita Python header iako je ubacen u Additional Include Libraries, kao i postaveljen kao header u src dir

		/*
		////	---- Plot ----
		plt::figure_size(1200, 780);
		plt::show();
		*/

	}
	else if (opcija == 2) {			// Izabir koordinata pomocu misa

		cout << "Nije zavrseno!";
		return 0;

	}
	else {							// Greska
		cout << "greska: Izabrana je nepostojeca opcija!";
		return -1;
	}

	return 0;
}

02. Calculating projections and Removing distortion/removing_distortion.cpp


//  Copyright John Maddock 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)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_eg_10

/*`

To understand how the rounding policies for
the discrete distributions can be used, we'll
use the 50-sample binomial distribution with a
success fraction of 0.5 once again, and calculate
all the possible quantiles at 0.05 and 0.95.

Begin by including the needed headers:

*/

#include <iostream>
#include <boost/math/distributions/binomial.hpp>

/*`

Next we'll bring the needed declarations into scope, and
define distribution types for all the available rounding policies:

*/

using namespace boost::math::policies;
using namespace boost::math;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_outwards> > >
        binom_round_outwards;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_inwards> > >
        binom_round_inwards;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_down> > >
        binom_round_down;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_up> > >
        binom_round_up;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<integer_round_nearest> > >
        binom_round_nearest;

typedef binomial_distribution<
            double,
            policy<discrete_quantile<real> > >
        binom_real_quantile;

/*`

Now let's set to work calling those quantiles:

*/

int main()
{
   std::cout <<
      "Testing rounding policies for a 50 sample binomial distribution,\n"
      "with a success fraction of 0.5.\n\n"
      "Lower quantiles are calculated at p = 0.05\n\n"
      "Upper quantiles at p = 0.95.\n\n";

   std::cout << std::setw(25) << std::right
      << "Policy"<< std::setw(18) << std::right
      << "Lower Quantile" << std::setw(18) << std::right
      << "Upper Quantile" << std::endl;

   // Test integer_round_outwards:
   std::cout << std::setw(25) << std::right
      << "integer_round_outwards"
      << std::setw(18) << std::right
      << quantile(binom_round_outwards(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_outwards(50, 0.5), 0.95)
      << std::endl;

   // Test integer_round_inwards:
   std::cout << std::setw(25) << std::right
      << "integer_round_inwards"
      << std::setw(18) << std::right
      << quantile(binom_round_inwards(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_inwards(50, 0.5), 0.95)
      << std::endl;

   // Test integer_round_down:
   std::cout << std::setw(25) << std::right
      << "integer_round_down"
      << std::setw(18) << std::right
      << quantile(binom_round_down(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_down(50, 0.5), 0.95)
      << std::endl;

   // Test integer_round_up:
   std::cout << std::setw(25) << std::right
      << "integer_round_up"
      << std::setw(18) << std::right
      << quantile(binom_round_up(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_up(50, 0.5), 0.95)
      << std::endl;

   // Test integer_round_nearest:
   std::cout << std::setw(25) << std::right
      << "integer_round_nearest"
      << std::setw(18) << std::right
      << quantile(binom_round_nearest(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_round_nearest(50, 0.5), 0.95)
      << std::endl;

   // Test real:
   std::cout << std::setw(25) << std::right
      << "real"
      << std::setw(18) << std::right
      << quantile(binom_real_quantile(50, 0.5), 0.05)
      << std::setw(18) << std::right
      << quantile(binom_real_quantile(50, 0.5), 0.95)
      << std::endl;
}

/*`

Which produces the program output:

[pre
Testing rounding policies for a 50 sample binomial distribution,
with a success fraction of 0.5.

Lower quantiles are calculated at p = 0.05

Upper quantiles at p = 0.95.

Testing rounding policies for a 50 sample binomial distribution,
with a success fraction of 0.5.

Lower quantiles are calculated at p = 0.05

Upper quantiles at p = 0.95.

                   Policy    Lower Quantile    Upper Quantile
   integer_round_outwards                18                31
    integer_round_inwards                19                30
       integer_round_down                18                30
         integer_round_up                19                31
    integer_round_nearest                19                30
                     real            18.701            30.299
]

*/

//] ends quickbook import


#include <iostream>
#include <Eigen\Dense>

using namespace Eigen;
using namespace std;

void main() {
	{
		MatrixXf M1(3, 3); // column-major storage
		M1 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
		cout << "M1: \n" << M1 << endl;
		Map<RowVectorXf> v1(M1.data(), M1.size());
		cout << "v1: \n" << v1 << endl;

		Matrix<float, Dynamic, Dynamic, RowMajor> M2(M1);
		cout << "M2: \n" << M2 << endl;
		Map<RowVectorXf> v2(M2.data(), M2.size());
		cout << "v2: \n" << v2 << endl;
	}
	{
		MatrixXf M1(2, 6);
		M1 << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;
		cout << "M1: \n" << M1 << endl;
		Map<VectorXf> v1(M1.data(), M1.size());
		cout << "v1: \n" << v1 << endl;
		Map<MatrixXf> M2(M1.data(), 6, 2);
		Map<VectorXf> v2(M2.data(), M2.size());
		cout << "M2: \n" << M2 << endl;
		cout << "v2: \n" << v2 << endl;
	}
	{
		// Slicing
		RowVectorXf v = RowVectorXf::LinSpaced(20, 0, 19);
		cout << "Input: \n" << v << endl;
		Map<RowVectorXf, 0, InnerStride<2>> v2(v.data(), v.size() / 2);
		cout << "Even: \n" << v2 << endl;
	}
	{
		MatrixXf M1 = MatrixXf::Random(3, 8);
		cout << "Column major input: \n" << M1 << endl;
		Map<MatrixXf, 0, OuterStride<>> M2(M1.data(), M1.rows(),
										  (M1.cols() + 2) / 3,
										   OuterStride<>(M1.outerStride() * 3));
		cout << "1 column over 3: \n" << M2 << endl;

		typedef Matrix<float, Dynamic, Dynamic, RowMajor> RowMajorMatrixXf;
		RowMajorMatrixXf M3(M1);
		cout << "Row major input:" << endl << M3 << "\n";
		Map<RowMajorMatrixXf, 0, Stride<Dynamic, 3> > M4(M3.data(), M3.rows(), (M3.cols() + 2) / 3,
			Stride<Dynamic, 3>(M3.outerStride(), 3));
		cout << "1 column over 3:" << endl << M4 << "\n";
	}
	system("pause");
}


//
//  Polynomials.cpp
//  kcalg
//
//  Created by knightc on 2019/7/15.
//  Copyright © 2019 knightc. All rights reserved.
//

#include <stdio.h>
#include <fstream>

#include "opentsb/math.h"

#include <NTL/ZZXFactoring.h> //单 x 的整数系数的多项式因式分解
#include <NTL/ZZX.h>  //单 x 的 整数系数函数/多项式
#include <NTL/ZZ_pX.h> //单 x 的 模 p 的整数系数函数/多项式
#include <NTL/ZZ_pXFactoring.h> //单 x 的 模 p 的整数系数多项式因式分解
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2EX.h>
#include <NTL/GF2EXFactoring.h>

using namespace NTL;
using namespace std;

ZZ y_polynomials(const ZZX &f,const ZZ xPoint) {

    ZZ m;
    m= f.rep[0];

    for (long i=1 ; i < f.rep.length(); i++) {
        if (!IsZero(f.rep[i])){
        m+=power(xPoint, i) * f.rep[i];
        }
    }

    return m;
}

void test_polynomials_main() {

    ZZX f;

    //f= a0+a1*x +a2*x^2 + a3 * x^3 +... + an * x^n
    ifstream fin("/Users/kc/git/alg-test/kcalg/kcalg/poly.txt");
    fin >> f;
   // cin >> f;//xcode的输入会自动补齐，但是键盘输入需要手动输入"【 "与 " 】"

    Vec< Pair<ZZX,long> > factors;//pair--系数、指数的对；vec_pair_ZZX_long
    ZZ c;

    factor(c, factors, f);//多项式的因式分解

 //   cout << c << "\n" << factors << "\n";

    //特定多项式输出:分圆多项式
    vec_ZZX phi(INIT_SIZE, 5);
    for (long i = 1; i <= 5; i++) {
        ZZX t;
        t = 1;

        for (long j = 1; j <= i-1; j++)
            if (i % j == 0)
                t *= phi(j);

        phi(i) = (ZZX(INIT_MONO, i) - 1)/t;

     //   cout << phi(i) << "\n";
    }

    //多项式赋值示例
    ZZX t1;
    ZZ t11;
    SetCoeff(t1, 4, 1);    //SetCoeff的优点：自动判断长度是否变化，即保证领项系数非0
    //SetCoeff(t1, 2560, 128); //尝试一下，大数
    t1.rep[2] = 2; //ZZX.rep的缺点：改变系数的同时不判断长度是否变化
    SetCoeff(t1, 0, -1);
    SetCoeff(t1, 5, 0); //领项系数设为 0 时，自动去掉
  //  t1[4]=2; //也可以直接定义系数值

    cout << t1 << "\n";
    ZZ t12;
    t12=12345678901234567890;//long 最大只能 20 位，不能直接转换。但是 ZZ 类型可以无穷大
    t11=y_polynomials(t1, t12);
    cout << t11 << endl;//t12=2 时，正确值为 23

    ZZX t2;
    t2.rep.SetLength(6); //如果没有SetCoeff，使用ZZX.rep之前必须先用该句初始化长度
    t2.rep[4] = 1;
    t2.rep[0] = -1;
//    cout << t2 << "\n"; //注：此时领项系数是0，即第 6 项的系数为 0

    t2.normalize(); //作用：重新调整长度，以保证领项系数非0
 //   cout << t2 << "\n";

    //下面两种从多项式中取系数的值的方法等价
    if (t1.rep[4] == 1 and coeff(t1, 4) == 1 ){
      //  cout << "多项式取值正确" << "\n";
    }
  //  cout << GCD(t1, t2) << endl;

    //mod p polynomials
    ZZ p(65537);
    ZZ_p::init(p);

    ZZ_pX f_px;
    //cin >> f_px;
    SetCoeff(f_px, 4, 2);
    f_px.rep[3]=35;
    SetCoeff(f_px,5,1);//领项系数必须为 1：leadcoeff=1

    vec_pair_ZZ_pX_long factors_px;
    CanZass(factors_px, f_px);  // calls "Cantor/Zassenhaus" algorithm
  //  cout << factors_px << "\n" << f_px << "\n";

    // mod gf2e polynomials
    GF2X P;
    SetCoeff(P, 8, 1);
    SetCoeff(P, 4, 1);
    SetCoeff(P, 3, 1);
    SetCoeff(P, 1, 1);
    SetCoeff(P, 0, 1);
    GF2E::init(P);

    GF2EX f_gf2ex;
    SetCoeff(f_gf2ex, 0, 65537);
   // f_gf2ex.rep[2]= 23;

    vec_pair_GF2EX_long factors_gf2ex;
  //  CanZass(factors_gf2ex, f_gf2ex);
   // cout << factors_gf2ex << "\n" << f_gf2ex << "\n";

}


#include <Eigen/CholmodSupport>
#include <fstream>
#include <iomanip>
#include "../../include/Optimization/LineSearch.h"
#include "../../include/Optimization/NewtonDescent.h"
#include "../../include/timer.h"
// #include "SuiteSparse_config.h"

void OptSolver::newtonSolver(std::function<double(const Eigen::VectorXd&, Eigen::VectorXd*, Eigen::SparseMatrix<double>*, bool)> objFunc, std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> findMaxStep, Eigen::VectorXd& x0, int numIter, double gradTol, double xTol, double fTol, bool disPlayInfo, std::function<void(const Eigen::VectorXd&, double&, double&)> getNormFunc, std::string* savingFolder, std::function<void(Eigen::VectorXd&)> postProcess)
{
	const int DIM = x0.rows();
    Eigen::VectorXd randomVec = x0;
    randomVec.setRandom();
    x0 += 1e-6 * randomVec;
	Eigen::VectorXd grad = Eigen::VectorXd::Zero(DIM);
	Eigen::SparseMatrix<double> hessian;

	Eigen::VectorXd neggrad, delta_x;
	double maxStepSize = 1.0;
	double reg = 1e-8;

	bool isProj = true;
    Timer<std::chrono::high_resolution_clock> totalTimer;
    double totalAssemblingTime = 0;
    double totalSolvingTime = 0;
    double totalLineSearchTime = 0;

    totalTimer.start();
	std::ofstream optInfo;
	if (savingFolder)
	{
		optInfo = std::ofstream((*savingFolder) + "_optInfo.txt");
		optInfo << "Newton solver with termination criterion: " << std::endl;
		std::cout << "gradient tol: " << gradTol << ", function update tol: " << fTol << ", variable update tol: " << xTol << ", maximum iteration: " << numIter << std::endl << std::endl;
	}
	int i = 0;
	for (; i < numIter; i++)
	{
		if(disPlayInfo)
			std::cout << "\niter: " << i << std::endl;
		if(savingFolder)
			optInfo << "\niter: " << i << std::endl;
        Timer<std::chrono::high_resolution_clock> localTimer;
        localTimer.start();
		double f = objFunc(x0, &grad, &hessian, isProj);
        localTimer.stop();
        double localAssTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
        totalAssemblingTime += localAssTime;

        localTimer.start();
		Eigen::SparseMatrix<double> H = hessian;
		Eigen::SparseMatrix<double> I(DIM, DIM);
		I.setIdentity();
		std::cout << "num of nonzeros: " << H.nonZeros() << ", rows: " << H.rows() << ", cols: " << H.cols() << std::endl;
		Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver(H);

		// Eigen::CholmodSimplicialLLT<Eigen::SparseMatrix<double> > solver(H);

		while (solver.info() != Eigen::Success)
		{
			if (disPlayInfo)
			{
				if (isProj)
					std::cout << "some small perturb is needed to remove round-off error, current reg = " << reg << std::endl;
				else
					std::cout << "Matrix is not positive definite, current reg = " << reg << std::endl;
			}

			H = hessian + reg * I;
			solver.compute(H);
			reg = std::max(2 * reg, 1e-16);

            if(reg > 1e4)
            {
                std::cout << "reg is too large, use SPD hessian instead." << std::endl;
                reg = 1e-6;
                isProj = true;
                f = objFunc(x0, &grad, &hessian, isProj);
            }
		}

		neggrad = -grad;
		delta_x = solver.solve(neggrad);

        localTimer.stop();
        double localSolvingTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
        totalSolvingTime += localSolvingTime;

		maxStepSize = findMaxStep(x0, delta_x);

        localTimer.start();
		double rate = LineSearch::backtrackingArmijo(x0, grad, delta_x, objFunc, maxStepSize);
        localTimer.stop();
        double localLinesearchTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
        totalLineSearchTime += localLinesearchTime;

		if (!isProj)
		{
			reg *= 0.5;
			reg = std::max(reg, 1e-16);
		}

		x0 = x0 + rate * delta_x;

		double fnew = objFunc(x0, &grad, NULL, isProj);
		if (disPlayInfo)
		{
			std::cout << "line search rate : " << rate << ", actual hessian : " << !isProj << ", reg = " << reg << std::endl;
			std::cout << "f_old: " << f << ", f_new: " << fnew << ", grad norm: " << grad.norm() << ", delta x: " << rate * delta_x.norm() << ", delta_f: " << f - fnew << std::endl;
			if (getNormFunc)
			{
				double gradz, gradw;
				getNormFunc(grad, gradz, gradw);

				double updatez, updatew;
				getNormFunc(rate * delta_x, updatez, updatew);
				std::cout << "z grad: " << gradz << ", w grad: " << gradw << ", z change: " << updatez << ", w change: " << updatew  << std::endl;
			}
            std::cout << "timing info (in total seconds): " << std::endl;
            std::cout << "assembling took: " << totalAssemblingTime << ", LLT solver took: "  << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;
		}

        if(postProcess)
        {
            postProcess(x0);
            if (disPlayInfo)
            {
                double tmpfnew = objFunc(x0, &grad, NULL, isProj);
                std::cout << "after post process, f_new: " << tmpfnew << ", grad norm: " << grad.norm() << std::endl;
            }

        }

		if (savingFolder)
		{
			optInfo << "line search rate : " << rate << ", actual hessian : " << !isProj << ", reg = " << reg << std::endl;
			optInfo << "f_old: " << f << ", f_new: " << fnew << ", grad norm: " << grad.norm() << ", delta x: " << rate * delta_x.norm() << ", delta_f: " << f - fnew << std::endl;
			if (getNormFunc)
			{
				double gradz, gradw;
				getNormFunc(grad, gradz, gradw);

				double updatez, updatew;
				getNormFunc(rate * delta_x, updatez, updatew);
				optInfo << "z grad: " << gradz << ", w grad: " << gradw << ", z change: " << updatez << ", w change: " << updatew << std::endl;
			}
			optInfo << "timing info (in total seconds): " << std::endl;
			optInfo << "assembling took: " << totalAssemblingTime << ", LLT solver took: " << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;

			if (i % 100 == 0)
			{
				std::string fileName = (*savingFolder) + "intermediate.txt";
				std::ofstream ofs(fileName);
				if (ofs)
				{
					ofs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << x0 << std::endl;
				}
			}
		}

		if ((f - fnew) / f < 1e-5 || rate * delta_x.norm() < 1e-5 || grad.norm() < 1e-4 || std::abs(f) < 1e-4)
		{
			isProj = false;
			// double f = objFunc(x0, &grad, &hessian, isProj);
			// H = hessian;
			// solver.compute(hessian);
			// double tmpReg = 1e-8;
			// while (solver.info() != Eigen::Success)
			// {
			// 	hessian = H + tmpReg * I;
			// 	solver.compute(hessian);
			// 	tmpReg = std::max(2 * tmpReg, 1e-16);
			// }
			// if(tmpReg > 1e-5)
			// {
			// 	std::cout << "hessian is far away from PD, stick with PD hessian." << std::endl;
			// 	isProj = true;
			// }

		}

		if (rate < 1e-8)
		{
			std::cout << "terminate with small line search rate (<1e-8): L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (grad.norm() < gradTol)
		{
			std::cout << "terminate with gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (rate * delta_x.norm() < xTol)
		{
			std::cout << "terminate with small variable change, gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (f - fnew < fTol)
		{
			std::cout << "terminate with small energy change, gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}
	}
	if (i >= numIter)
		std::cout << "terminate with reaching the maximum iteration, with gradient L2-norm = " << grad.norm() << std::endl;

    double f = objFunc(x0, &grad, &hessian, false);
    Eigen::SparseMatrix<double> I(DIM, DIM), H = hessian;
    I.setIdentity();
    Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver(H);

    reg = 1e-10;
    while (solver.info() != Eigen::Success)
    {
        if (disPlayInfo)
        {
            if (isProj)
                std::cout << "some small perturb is needed to remove round-off error, current reg = " << reg << std::endl;
            else
                std::cout << "Matrix is not positive definite, current reg = " << reg << std::endl;
        }

        H = hessian + reg * I;
        solver.compute(H);
        reg = std::max(2 * reg, 1e-16);
    }
    std::cout << "end up with energy: " << f << ", gradient: " << grad.norm() << std::endl;
    std::cout << "mininal evalues of hessian is between: " << reg / 2 << ", " << reg << std::endl;

    totalTimer.stop();
    if(disPlayInfo)
    {
        std::cout << "total time costed (s): " << totalTimer.elapsed<std::chrono::milliseconds>() * 1e-3 << ", within that, assembling took: " << totalAssemblingTime << ", LLT solver took: "  << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;
    }

	if (savingFolder)
	{
		optInfo << "total time costed (s): " << totalTimer.elapsed<std::chrono::milliseconds>() * 1e-3 << ", within that, assembling took: " << totalAssemblingTime << ", LLT solver took: " << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;

		std::string fileName = (*savingFolder) + "final_res.txt";
		std::ofstream ofs(fileName);
		if (ofs)
		{
			ofs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << x0 << std::endl;
		}

	}

}