// proj-marker-transform.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <vector>
#include <iostream>
#include <time.h>

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Cholesky>

#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

#include <math/func.h>
#include <math/matrix.h>
#include <import.h>

#include <math/SparseBlockSquareMatrix.h>

Eigen::VectorXf pcg( const SparseBlockSquareMatrix &A, const Eigen::VectorXf &b, int maxIters, float threshold = 1e-4 ) {
	Eigen::VectorXf M = A.diagonal();
	for ( int i = 0; i < M.rows(); i++ ) {
		if ( fabs( M[i] ) > threshold ) {
			M[i] = 1.0f / M[i];
		} else {
			M[i] = 1.0f;
		}
	}

	Eigen::VectorXf x = Eigen::VectorXf::Zero(A.size());
	Eigen::VectorXf p = Eigen::VectorXf::Zero(A.size());
	Eigen::VectorXf r = b; // b - A * x

	int m_numIters = 0;
	clock_t begin = clock();
	for ( int i = 0; i < maxIters; i++ ) {
		m_numIters++;

		Eigen::VectorXf Mr = M.array() * r.array();
		float rMr = r.dot( Mr );
		if ( rMr < threshold * A.size() ) {
		//	break;
		}
		if ( threshold <= 0 && rMr < 1e-6 ) {
			rMr = 0.0f;
		} else {
			rMr = 1.0f / rMr;
		}
		p += Mr * rMr;

		Eigen::VectorXf Ap = A * p;
		float pAp = p.dot( Ap );
		if ( threshold <= 0 && pAp < 1e-6 ) {
			pAp = 0.0f;
		} else {
			pAp = 1.0f / pAp;
		}
		x +=  p * pAp;
		r -= Ap * pAp;

		float rme = sqrt( r.dot( r ) / A.size() );
		if ( rme < 1e-6 ) {
			break;
		}
	}
	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
	printf( "pcg cpu (ms) : %.3f : %.3f : %d\n", time_spent * 1000, time_spent * 1000 / m_numIters, m_numIters );

	return x;
}

float icp_point_to_point_svd(
	const std::vector< Eigen::Vector3f > &src_points,
	const std::vector< Eigen::Vector3f > &dst_points,
	Eigen::Matrix3f &out_r, // rotation
	Eigen::Vector3f &out_t  // translation
) {
	if ( src_points.empty() ) {
		out_r.setIdentity();
		out_t.setZero();
		return 0.0f;
	}

	// E(src)
	Eigen::Vector3f src_mean = mean( src_points );

	// E(dst)
	Eigen::Vector3f dst_mean = mean( dst_points );

	// A = Sum( ( src - E(src) ) * ( dst - E(dst) )' )
	Eigen::Matrix3f A = Eigen::Matrix3f::Identity() * 1e-6f;
	for ( int i = 0; i < src_points.size(); i++ ) {
		A += ( src_points[i] - src_mean ) * ( dst_points[i] - dst_mean ).transpose();
	}

	// SVD : A = U * D * V'
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);

	// R = V * U'
	out_r = svd.matrixV() * svd.matrixU().transpose();

	// T = E(dst) - R * E(src)
	out_t = dst_mean - out_r * src_mean;

	// rme
	return rme( src_points, dst_points, out_r, out_t );
}

void init_affine_transforms( int size, Eigen::VectorXf &x, const Eigen::Matrix3f &rot, const std::vector< Eigen::Vector3f > &src, const std::vector< Eigen::Vector3f > &dst ) {
	for ( int i = 0; i < size; i++ ) {
		x.block( 12 * i + 3 * 0, 0, 3, 1 ) = rot.col(0); Eigen::Vector3f( 1, 0, 0 );
		x.block( 12 * i + 3 * 1, 0, 3, 1 ) = rot.col(1); Eigen::Vector3f( 0, 1, 0 );
		x.block( 12 * i + 3 * 2, 0, 3, 1 ) = rot.col(2); Eigen::Vector3f( 0, 0, 1 );
		x.block( 12 * i + 3 * 3, 0, 3, 1 ) = dst[i] - src[i];
	}
}

void EvalRigidTerm( int size, Eigen::MatrixXf &JtJ, Eigen::VectorXf &Jtr, Eigen::VectorXf &x, std::vector<Eigen::Vector3f> &src, std::vector<Eigen::Vector3f> &dst, float &E ) {
	// E-rigid
	float w_rigid = 1.0f;
	{
		for ( int i = 0; i < size; i++ ) {
			Eigen::Vector3f M[4];
			M[0] = x.block( i * 12 + 0 * 3, 0, 3, 1 );
			M[1] = x.block( i * 12 + 1 * 3, 0, 3, 1 );
			M[2] = x.block( i * 12 + 2 * 3, 0, 3, 1 );
			M[3] = x.block( i * 12 + 3 * 3, 0, 3, 1 );

			// ck dot ck - 1
			for ( int k = 0; k < 3; k++ ) {
				float r = M[k].dot( M[k] ) - 1;

				Eigen::VectorXf J = Eigen::VectorXf::Zero( 12 );
				J.block( k * 3, 0, 3, 1 ) = M[k] + M[k];

				E += r * r * w_rigid;
				JtJ.block( i * 12, i * 12, 12, 12 ) += J * J.transpose() * w_rigid; // block (i, i)
				Jtr.block( i * 12, 0, 12, 1 ) += J * r * w_rigid; // block ( i )
			}

			// ck dot cj
			for ( int k = 0; k < 3; k++ ) {
				int j = ( k + 1 ) % 3;

				float r = M[k].dot( M[j] );

				E += r * r * w_rigid;

				Eigen::VectorXf J = Eigen::VectorXf::Zero( 12 );

				J.block( k * 3, 0, 3, 1 ) = M[j];
				J.block( j * 3, 0, 3, 1 ) = M[k];

				JtJ.block( i * 12, i * 12, 12, 12 ) += J * J.transpose() * w_rigid; // block (i, i)
				Jtr.block( i * 12, 0, 12, 1 ) += J * r * w_rigid; // block ( i )
			}
		}
	}
	printf( "E-rigid : %f\n", E );
}

void EvalSmoothTerm( int size, Eigen::MatrixXf &JtJ, Eigen::VectorXf &Jtr, Eigen::VectorXf &x, std::vector<Eigen::Vector3f> &src, std::vector<Eigen::Vector3f> &dst, float &out_E, const std::vector<std::vector<int>> &links ) {
	// E-smooth
	float E = 0;
	float w_smooth = 1.0f;
	{
		for ( int i = 0; i < size; i++ ) {
		for ( int k = 0; k < links[i].size(); k++ ) {
			int j = links[i][k];

			Eigen::Vector3f Mi[4], Mj[4];
			Mi[0] = x.block( i * 12 + 0 * 3, 0, 3, 1 );
			Mi[1] = x.block( i * 12 + 1 * 3, 0, 3, 1 );
			Mi[2] = x.block( i * 12 + 2 * 3, 0, 3, 1 );
			Mi[3] = x.block( i * 12 + 3 * 3, 0, 3, 1 );

			Mj[0] = x.block( j * 12 + 0 * 3, 0, 3, 1 );
			Mj[1] = x.block( j * 12 + 1 * 3, 0, 3, 1 );
			Mj[2] = x.block( j * 12 + 2 * 3, 0, 3, 1 );
			Mj[3] = x.block( j * 12 + 3 * 3, 0, 3, 1 );

			// Ri * ( gj - gi ) - ( gj - gi ) - ( tj - ti )

			Eigen::Vector3f dg = src[j] - src[i];

			Eigen::Vector3f dt = Mj[3] - Mi[3];

			Eigen::Vector3f r = ( dg[0] * Mi[0] + dg[1] * Mi[1] + dg[2] * Mi[2] ) - dg - dt;

			E += r.dot( r ) * w_smooth;

			Eigen::MatrixXf Ji = Eigen::MatrixXf::Zero( 3, 12 );
			Ji( 0, 0 + 3 * 0 ) = dg[0]; Ji( 0, 0 + 3 * 1 ) = dg[1]; Ji( 0, 0 + 3 * 2 ) = dg[2];
			Ji( 1, 1 + 3 * 0 ) = dg[0];	Ji( 1, 1 + 3 * 1 ) = dg[1];	Ji( 1, 1 + 3 * 2 ) = dg[2];
			Ji( 2, 2 + 3 * 0 ) = dg[0];	Ji( 2, 2 + 3 * 1 ) = dg[1];	Ji( 2, 2 + 3 * 2 ) = dg[2];

		//	std::cout << Ji <<std::endl;

			JtJ.block( i * 12, i * 12, 12, 12 ) += Ji.transpose() * Ji * w_smooth;
			Jtr.block( i * 12, 0, 12, 1 ) += Ji.transpose() * r * w_smooth;
		}}
	}
	printf( "E-smooth : %f\n", E );
	out_E += E;
}
bool read_text_file( const char *path, std::vector<std::vector<Eigen::Vector3f>> &poses ) {
	FILE *fp = fopen( path, "r" );
	if ( !fp ) {
		printf( "failed to read text file: %s\n", path );
		return false;
	}

	int frame = 0;
	while( fscanf( fp, "%d", &frame ) == 1 ) {
		frame -= 1;
		if ( frame != poses.size() ) {
			break;
		}
		printf( "read %d\n", frame );

		int subFrame = 0;
		fscanf( fp, "%d", &subFrame );

		poses.push_back( std::vector<Eigen::Vector3f>( 41 ) );
		for ( int i = 0; i < 41; i++ ) {
			float x, y, z;
			fscanf( fp, "%f", &x );
			fscanf( fp, "%f", &y );
			fscanf( fp, "%f", &z );
		//	printf( "%f %f %f\n", x, y, z);
			poses[frame][i][0] = x * 0.001f;
			poses[frame][i][1] = y * 0.001f;
			poses[frame][i][2] = z * 0.001f;
		}
	}

	fclose( fp );
	return true;
}

void test_gen_data( void ) {
	int size = 8;

	std::vector<Eigen::Vector3f> src( size ), dst( size );

	// gen transform
	Eigen::Vector3f euler;
	euler << 20, 50, -45;
	Eigen::Matrix3f rot = euler_to_mat( radians( euler ) );
	Eigen::Vector3f t;
	t << 30, 50, -10;
	std::cout << "Truth" << std::endl;
	std::cout << rot << std::endl;
	std::cout << t << std::endl;

	// gen data
	for ( int i = 0; i < size; i++ ) {
		Eigen::Vector3f euler;
		euler << 20, 50, -45;
		euler[1] += i * 5.0f;
		Eigen::Matrix3f rot = euler_to_mat( radians( euler ) );

		Eigen::Vector3f t;
		t << 30, 50, -10;

		src[i] = Eigen::Vector3f::Random();
		src[i][0] = fmod( src[i][0], 256.0f );
		src[i][1] = fmod( src[i][1], 256.0f );
		src[i][2] = fmod( src[i][2], 256.0f );
		dst[i] = rot * src[i] + t;
		//std::cout << "i << std::endl;
		//std::cout << src[i] << std::endl;
		//std::cout << dst[i] << std::endl;
	}

	// rigid transform
	Eigen::Matrix3f out_rot = Eigen::Matrix3f::Identity();
	Eigen::Vector3f out_t = Eigen::Vector3f::Zero();;
	{
		float rme = icp_point_to_point_svd( src, dst, out_rot, out_t );

		std::cout << "Result" << std::endl;
		std::cout << out_rot << std::endl;
		std::cout << out_t << std::endl;
		std::cout << "rme : " << rme << std::endl;
		std::cout << std::endl;
	}

	Eigen::MatrixXf JtJ = Eigen::MatrixXf::Identity( 12 * size, 12 * size ) * 1e-6f;
	Eigen::VectorXf Jtr = Eigen::VectorXf::Zero( 12 * size );
	Eigen::VectorXf x( 12 * size );
	float E = 0.0f;

	init_affine_transforms( size, x, out_rot, src, dst );

	for ( int i = 0; i < 5; i++ ) {
		JtJ = Eigen::MatrixXf::Identity( 12 * size, 12 * size ) * 1e-6f;
		Jtr = Eigen::VectorXf::Zero( 12 * size );
		E = 0.0f;

	//	EvalLinearSystem( size, JtJ, Jtr, x, src, dst, E );
		//std::cout << Jtr;
		x -= JtJ.fullPivHouseholderQr().solve( Jtr );
	}
}

class Timer {
public :
	clock_t begin, end;

	void Start( void ) {
		begin = clock();
	}

	void Pause( void ) {
		end = clock();
	}

	void Print( const char *id ) {
		double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
		printf( "time : %s : %f\n", id, time_spent );
	}
};

int _tmain(int argc, _TCHAR* argv[])
{
	std::vector<std::vector<Eigen::Vector3f>> poses;

	read_text_file( "New Session 0103.csv", poses );

	// rigid transform

	Timer timer;

	Eigen::Matrix3f out_rot = Eigen::Matrix3f::Identity();
	Eigen::Vector3f out_t = Eigen::Vector3f::Zero();
	{
		timer.Start();
		float rme = icp_point_to_point_svd( poses[10], poses[600], out_rot, out_t );
		timer.Pause();
		timer.Print( "ICP" );

		std::cout << "Result" << std::endl;
		std::cout << out_rot << std::endl;
		std::cout << out_t << std::endl;
		std::cout << "rme : " << rme << std::endl;
		std::cout << std::endl;
	}

	int size = poses[0].size();

	Eigen::MatrixXf JtJ = Eigen::MatrixXf::Identity( 12 * size, 12 * size ) * 1e-6f;
	Eigen::VectorXf Jtr = Eigen::VectorXf::Zero( 12 * size );
	Eigen::VectorXf x( 12 * size );
	float E = 0.0f;

	init_affine_transforms( size, x, out_rot, poses[0], poses[600] );

	std::vector<std::vector<int>> links( poses[0].size() );
	for ( int i = 0; i < poses[0].size(); i++ ) {
		for ( int j = i + 1; j < poses[0].size(); j++ ) {
			if ( ( poses[0][i] - poses[0][j] ).norm() < 0.25f ) {
				links[i].push_back( j );
				links[j].push_back( i );
			}
		}
	}

	for ( int i = 0; i < links.size(); i++ ) {
		printf( "%d : %d\n", i, links[i].size() );
	}

	for ( int i = 0; i < 7; i++ ) {
		JtJ = Eigen::MatrixXf::Identity( 12 * size, 12 * size ) * 1e-3f;
		Jtr = Eigen::VectorXf::Zero( 12 * size );
		E = 0.0f;

		timer.Start();
		EvalRigidTerm( size, JtJ, Jtr, x, poses[0], poses[300], E );
		EvalSmoothTerm( size, JtJ, Jtr, x, poses[0], poses[300], E, links );
		timer.Pause();
		timer.Print( "Jacobian" );

		//std::cout << Jtr;
		timer.Start();
		SparseBlockSquareMatrix B;
		B.createFromDenseMatrix( JtJ, 12 );

		x -= pcg( B, Jtr, 100, 1e-6 );

		//x -= JtJ.fullPivHouseholderQr().solve( Jtr );
		timer.Pause();
		timer.Print( "Solve" );
	}

	return 0;
}


#include <Eigen/Dense>
#include <memory>
#include <iostream>
#include <limits>
#include "bpmfutils.h"
#include "sparsetensor.h"

using namespace Eigen;

std::pair<double,double> eval_rmse_tensor(
		SparseMode & sparseMode,
		const int Nepoch,
		Eigen::VectorXd & predictions,
		Eigen::VectorXd & predictions_var,
		std::vector< std::unique_ptr<Eigen::MatrixXd> > & samples,
		double mean_value)
{
  auto& U = samples[0];

  const int nmodes = samples.size();
  const int num_latents = U->rows();

  const unsigned N = sparseMode.values.size();
  double se = 0.0, se_avg = 0.0;

  if (N == 0) {
    // No test data, returning NaN's
    return std::make_pair(std::numeric_limits<double>::quiet_NaN(),
                          std::numeric_limits<double>::quiet_NaN());
  }

  if (N != predictions.size()) {
    throw std::runtime_error("Ytest.size() and predictions.size() must be equal.");
  }
	if (sparseMode.row_ptr.size() - 1 != U->cols()) {
    throw std::runtime_error("U.cols() and sparseMode size must be equal.");
	}

#pragma omp parallel for schedule(dynamic, 2) reduction(+:se, se_avg)
  for (int n = 0; n < U->cols(); n++) {
    Eigen::VectorXd u = U->col(n);
    for (int j = sparseMode.row_ptr(n);
             j < sparseMode.row_ptr(n + 1);
             j++)
    {
      VectorXi idx = sparseMode.indices.row(j);
      double pred = mean_value;
      for (int d = 0; d < num_latents; d++) {
        double tmp = u(d);

        for (int m = 1; m < nmodes; m++) {
          tmp *= (*samples[m])(d, idx(m - 1));
        }
        pred += tmp;
      }

      double pred_avg;
      if (Nepoch == 0) {
        pred_avg = pred;
      } else {
        double delta = pred - predictions(j);
        pred_avg = (predictions(j) + delta / (Nepoch + 1));
        predictions_var(j) += delta * (pred - pred_avg);
      }
      se     += square(sparseMode.values(j) - pred);
      se_avg += square(sparseMode.values(j) - pred_avg);
      predictions(j) = pred_avg;
    }
  }
  const double rmse = sqrt(se / N);
  const double rmse_avg = sqrt(se_avg / N);
  return std::make_pair(rmse, rmse_avg);
}


// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.

#include "../precomp.hpp"
#include "../usac.hpp"
#ifdef HAVE_EIGEN
#include <Eigen/Eigen>
#endif

namespace cv { namespace usac {
class HomographyMinimalSolver4ptsGEMImpl : public HomographyMinimalSolver4ptsGEM {
private:
    const Mat * points_mat;
    const float * const points;
public:
    explicit HomographyMinimalSolver4ptsGEMImpl (const Mat &points_) :
        points_mat(&points_), points ((float*) points_.data) {}

    int estimate (const std::vector<int>& sample, std::vector<Mat> &models) const override {
        int m = 8, n = 9;
        std::vector<double> A(72, 0);
        int cnt = 0;
        for (int i = 0; i < 4; i++) {
            const int smpl = 4*sample[i];
            const double x1 = points[smpl], y1 = points[smpl+1], x2 = points[smpl+2], y2 = points[smpl+3];

            A[cnt++] = -x1;
            A[cnt++] = -y1;
            A[cnt++] = -1;
            cnt += 3; // skip zeros
            A[cnt++] = x2*x1;
            A[cnt++] = x2*y1;
            A[cnt++] = x2;

            cnt += 3;
            A[cnt++] = -x1;
            A[cnt++] = -y1;
            A[cnt++] = -1;
            A[cnt++] = y2*x1;
            A[cnt++] = y2*y1;
            A[cnt++] = y2;
        }

        if (!Math::eliminateUpperTriangular(A, m, n))
            return 0;

        models = std::vector<Mat>{ Mat_<double>(3,3) };
        auto * h = (double *) models[0].data;
        h[8] = 1.;

        // start from the last row
        for (int i = m-1; i >= 0; i--) {
            double acc = 0;
            for (int j = i+1; j < n; j++)
                acc -= A[i*n+j]*h[j];

            h[i] = acc / A[i*n+i];
            // due to numerical errors return 0 solutions
            if (std::isnan(h[i]))
                return 0;
        }
        return 1;
    }

    int getMaxNumberOfSolutions () const override { return 1; }
    int getSampleSize() const override { return 4; }
    Ptr<MinimalSolver> clone () const override {
        return makePtr<HomographyMinimalSolver4ptsGEMImpl>(*points_mat);
    }
};
Ptr<HomographyMinimalSolver4ptsGEM> HomographyMinimalSolver4ptsGEM::create(const Mat &points_) {
    return makePtr<HomographyMinimalSolver4ptsGEMImpl>(points_);
}

class HomographyNonMinimalSolverImpl : public HomographyNonMinimalSolver {
private:
    const Mat * points_mat;
    const Ptr<NormTransform> normTr;
public:
    explicit HomographyNonMinimalSolverImpl (const Mat &points_) :
        points_mat(&points_), normTr (NormTransform::create(points_)) {}

    /*
     * Find Homography matrix using (weighted) non-minimal estimation.
     * Use Principal Component Analysis. Use normalized points.
     */
    int estimate (const std::vector<int> &sample, int sample_size, std::vector<Mat> &models,
            const std::vector<double> &weights) const override {
        if (sample_size < getMinimumRequiredSampleSize())
            return 0;

        Matx33d T1, T2;
        Mat norm_points_;
        normTr->getNormTransformation(norm_points_, sample, sample_size, T1, T2);

        /*
         * @norm_points is matrix 4 x inlier_size
         * @weights is vector of inliers_size
         * weights[i] is weight of i-th inlier
         */
        const auto * const norm_points = (float *) norm_points_.data;

        double a1[9] = {0, 0, -1, 0, 0, 0, 0, 0, 0},
               a2[9] = {0, 0, 0, 0, 0, -1, 0, 0, 0},
               AtA[81] = {0};

        if (weights.empty()) {
            for (int i = 0; i < sample_size; i++) {
                const int smpl = 4*i;
                const double x1 = norm_points[smpl  ], y1 = norm_points[smpl+1],
                             x2 = norm_points[smpl+2], y2 = norm_points[smpl+3];

                a1[0] = -x1;
                a1[1] = -y1;
                a1[6] = x2*x1;
                a1[7] = x2*y1;
                a1[8] = x2;

                a2[3] = -x1;
                a2[4] = -y1;
                a2[6] = y2*x1;
                a2[7] = y2*y1;
                a2[8] = y2;

                for (int j = 0; j < 9; j++)
                    for (int z = j; z < 9; z++)
                        AtA[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z];
            }
        } else {
            for (int i = 0; i < sample_size; i++) {
                const int smpl = 4*i;
                const double weight = weights[i];
                const double x1 = norm_points[smpl  ], y1 = norm_points[smpl+1],
                             x2 = norm_points[smpl+2], y2 = norm_points[smpl+3];
                const double minus_weight_times_x1 = -weight * x1,
                             minus_weight_times_y1 = -weight * y1,
                                   weight_times_x2 =  weight * x2,
                                   weight_times_y2 =  weight * y2;

                a1[0] = minus_weight_times_x1;
                a1[1] = minus_weight_times_y1;
                a1[2] = -weight;
                a1[6] = weight_times_x2 * x1;
                a1[7] = weight_times_x2 * y1;
                a1[8] = weight_times_x2;

                a2[3] = minus_weight_times_x1;
                a2[4] = minus_weight_times_y1;
                a2[5] = -weight;
                a2[6] = weight_times_y2 * x1;
                a2[7] = weight_times_y2 * y1;
                a2[8] = weight_times_y2;

                for (int j = 0; j < 9; j++)
                    for (int z = j; z < 9; z++)
                        AtA[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z];
            }
        }

        // copy symmetric part of covariance matrix
        for (int j = 1; j < 9; j++)
            for (int z = 0; z < j; z++)
                AtA[j*9+z] = AtA[z*9+j];

#ifdef HAVE_EIGEN
        Mat H = Mat_<double>(3,3);
        Eigen::HouseholderQR<Eigen::Matrix<double, 9, 9>> qr((Eigen::Matrix<double, 9, 9> (AtA)));
        const Eigen::Matrix<double, 9, 9> &Q = qr.householderQ();
        // extract the last nullspace
        Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)H.data) = Q.col(8);
#else
        Matx<double, 9, 9> Vt;
        Vec<double, 9> D;
        if (! eigen(Matx<double, 9, 9>(AtA), D, Vt)) return 0;
        Mat H = Mat_<double>(3, 3, Vt.val + 72/*=8*9*/);
#endif

        models = std::vector<Mat>{ T2.inv() * H * T1 };
        return 1;
    }

    int getMinimumRequiredSampleSize() const override { return 4; }
    int getMaxNumberOfSolutions () const override { return 1; }
    Ptr<NonMinimalSolver> clone () const override {
        return makePtr<HomographyNonMinimalSolverImpl>(*points_mat);
    }
};
Ptr<HomographyNonMinimalSolver> HomographyNonMinimalSolver::create(const Mat &points_) {
    return makePtr<HomographyNonMinimalSolverImpl>(points_);
}

class AffineMinimalSolverImpl : public AffineMinimalSolver {
private:
    const Mat * points_mat;
    const float * const points;
public:
    explicit AffineMinimalSolverImpl (const Mat &points_) :
            points_mat(&points_), points((float *) points_.data) {}
    /*
        Affine transformation
        x1 y1 1 0  0  0   a   u1
        0  0  0 x1 y1 1   b   v1
        x2 y2 1 0  0  0   c   u2
        0  0  0 x2 y2 1 * d = v2
        x3 y3 1 0  0  0   e   u3
        0  0  0 x3 y3 1   f   v3
    */
    int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const override {
        const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2];
        const auto
                x1 = points[smpl1], y1 = points[smpl1+1], u1 = points[smpl1+2], v1 = points[smpl1+3],
                x2 = points[smpl2], y2 = points[smpl2+1], u2 = points[smpl2+2], v2 = points[smpl2+3],
                x3 = points[smpl3], y3 = points[smpl3+1], u3 = points[smpl3+2], v3 = points[smpl3+3];

        // covers degeneracy test when all 3 points are collinear.
        // In this case denominator will be 0
        double denominator = x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2;
        if (fabs(denominator) < FLT_EPSILON) // check if denominator is zero
            return 0;
        denominator = 1. / denominator;

        double a =  (u1*y2 - u2*y1 - u1*y3 + u3*y1 + u2*y3 - u3*y2) * denominator;
        double b = -(u1*x2 - u2*x1 - u1*x3 + u3*x1 + u2*x3 - u3*x2) * denominator;
        double c = u1 - a * x1 - b * y1; // ax1 + by1 + c = u1
        double d =  (v1*y2 - v2*y1 - v1*y3 + v3*y1 + v2*y3 - v3*y2) * denominator;
        double e = -(v1*x2 - v2*x1 - v1*x3 + v3*x1 + v2*x3 - v3*x2) * denominator;
        double f = v1 - d * x1 - e * y1; // dx1 + ey1 + f = v1

        models[0] = Mat(Matx33d(a, b, c, d, e, f, 0, 0, 1));
        return 1;
    }
    int getSampleSize() const override { return 3; }
    int getMaxNumberOfSolutions () const override { return 1; }
    Ptr<MinimalSolver> clone () const override {
        return makePtr<AffineMinimalSolverImpl>(*points_mat);
    }
};
Ptr<AffineMinimalSolver> AffineMinimalSolver::create(const Mat &points_) {
    return makePtr<AffineMinimalSolverImpl>(points_);
}

class AffineNonMinimalSolverImpl : public AffineNonMinimalSolver {
private:
    const Mat * points_mat;
    const float * const points;
    // const NormTransform<double> norm_transform;
public:
    explicit AffineNonMinimalSolverImpl (const Mat &points_) :
            points_mat(&points_), points((float*) points_.data)
    /*, norm_transform(points_)*/ {}

    int estimate (const std::vector<int> &sample, int sample_size, std::vector<Mat> &models,
                  const std::vector<double> &weights) const override {
        // surprisingly normalization of points does not improve the output model
        // Mat norm_points_, T1, T2;
        // norm_transform.getNormTransformation(norm_points_, sample, sample_size, T1, T2);
        // const auto * const n_pts = (double *) norm_points_.data;

        if (sample_size < getMinimumRequiredSampleSize())
            return 0;
        // do Least Squares
        // Ax = b   ->  A^T Ax = A^T b
        // x = (A^T A)^-1 A^T b
        double AtA[36] = {0}, Ab[6] = {0};
        double r1[6] = {0, 0, 1, 0, 0, 0}; // row 1 of A
        double r2[6] = {0, 0, 0, 0, 0, 1}; // row 2 of A

        if (weights.empty())
            for (int p = 0; p < sample_size; p++) {
                // if (weights != nullptr) weight = weights[sample[p]];

                const int smpl = 4*sample[p];
                const double x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
                // const double x1=n_pts[smpl], y1=n_pts[smpl+1], x2=n_pts[smpl+2], y2=n_pts[smpl+3];

                r1[0] = x1;
                r1[1] = y1;

                r2[3] = x1;
                r2[4] = y1;

                for (int j = 0; j < 6; j++) {
                    for (int z = j; z < 6; z++)
                        AtA[j * 6 + z] += r1[j] * r1[z] + r2[j] * r2[z];
                    Ab[j] += r1[j]*x2 + r2[j]*y2;
                }
            }
        else
            for (int p = 0; p < sample_size; p++) {
                const int smpl = 4*sample[p];
                const double weight = weights[p];
                const double weight_times_x1 = weight * points[smpl  ],
                             weight_times_y1 = weight * points[smpl+1],
                             weight_times_x2 = weight * points[smpl+2],
                             weight_times_y2 = weight * points[smpl+3];

                r1[0] = weight_times_x1;
                r1[1] = weight_times_y1;
                r1[2] = weight;

                r2[3] = weight_times_x1;
                r2[4] = weight_times_y1;
                r2[5] = weight;

                for (int j = 0; j < 6; j++) {
                    for (int z = j; z < 6; z++)
                        AtA[j * 6 + z] += r1[j] * r1[z] + r2[j] * r2[z];
                    Ab[j] += r1[j]*weight_times_x2 + r2[j]*weight_times_y2;
                }
            }

        // copy symmetric part
        for (int j = 1; j < 6; j++)
            for (int z = 0; z < j; z++)
                AtA[j*6+z] = AtA[z*6+j];

        Vec6d aff;
        if (!solve(Matx66d(AtA), Vec6d(Ab), aff))
            return 0;
        models[0] = Mat(Matx33d(aff(0), aff(1), aff(2),
                                aff(3), aff(4), aff(5),
                                0, 0, 1));

        // models[0] = T2.inv() * models[0] * T1;
        return 1;
    }

    int getMinimumRequiredSampleSize() const override { return 3; }
    int getMaxNumberOfSolutions () const override { return 1; }
    Ptr<NonMinimalSolver> clone () const override {
        return makePtr<AffineNonMinimalSolverImpl>(*points_mat);
    }
};
Ptr<AffineNonMinimalSolver> AffineNonMinimalSolver::create(const Mat &points_) {
    return makePtr<AffineNonMinimalSolverImpl>(points_);
}
}}


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

#ifndef EKF_HPP
#define EKF_HPP

#include <iostream>

#include <Eigen/Dense>

#include "steering_functions/steering_functions.hpp"
#include "steering_functions/utilities/utilities.hpp"

using namespace std;
using namespace steer;

typedef Eigen::Matrix<double, 2, 2> Matrix2d;
typedef Eigen::Matrix<double, 3, 3> Matrix3d;
typedef Eigen::Matrix<double, 3, 2> Matrix32d;
typedef Eigen::Matrix<double, 2, 3> Matrix23d;

class EKF
{
public:
  /** Constructor */
  EKF();

  /** \brief Sets the parameters required by the EKF */
  void set_parameters(const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise,
                      const Controller &_controller);

  /** \brief Converts a covariance given by a double array to an Eigen matrix */
  Matrix3d covariance_to_eigen(const double covariance[16]) const;

  /** \brief Converts a covariance given by an Eigen matrix to a double array */
  void eigen_to_covariance(const Matrix3d &covariance_eigen, double covariance[16]) const;

  /** \brief Computes the Jacobians of the motion equations with respect to the state and control */
  void get_motion_jacobi(const State &state, const Control &control, double integration_step, Matrix3d &F_x,
                         Matrix32d &F_u) const;

  /** \brief Computes the Jacobian of the observation equations with respect to the state */
  Matrix3d get_observation_jacobi() const;

  /** \brief Returns the motion covariance in control space */
  Matrix2d get_motion_covariance(const State &state, const Control &control, double integration_step) const;

  /** \brief Returns the observation covariance */
  Matrix3d get_observation_covariance() const;

  /** \brief Returns the gain of the controller */
  Matrix23d get_controller_gain(const Control &control) const;

  /** \brief Returns the rotation matrix from a global frame to a local frame */
  Matrix3d get_rotation_matrix(double angle) const;

  /** \brief Predicts the covariances based on the paper:
      Rapidly-exploring random belief trees for motion planning under uncertainty, A. Bry and N. Roy, IEEE ICRA 2011 */
  void predict(const State_With_Covariance &state, const Control &control, double integration_step,
               State_With_Covariance &state_pred) const;

  /** \brief Predicts the covariances */
  void update(const State_With_Covariance &state_pred, State_With_Covariance &state_corr) const;

  /** \brief Overload operator new for fixed-size vectorizable Eigen member variable */
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

private:
  /** \brief Motion noise */
  Motion_Noise motion_noise_;

  /** \brief Measurement noise */
  Measurement_Noise measurement_noise_;

  /** \brief Feedback controller */
  Controller controller_;

  /** \brief Identity matrix */
  Matrix3d I_;
};

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

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

#include <Eigen/LU> // required for MatrixBase::determinant
#include <Eigen/SVD> // required for SVD

using namespace Eigen;

//  Constructs a random matrix from the unitary group U(size).
template <typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixUnitary(int size)
{
  typedef T Scalar;
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;

  MatrixType Q;

  int max_tries = 40;
  double is_unitary = false;

  while (!is_unitary && max_tries > 0)
  {
    // initialize random matrix
    Q = MatrixType::Random(size, size);

    // orthogonalize columns using the Gram-Schmidt algorithm
    for (int col = 0; col < size; ++col)
    {
      typename MatrixType::ColXpr colVec = Q.col(col);
      for (int prevCol = 0; prevCol < col; ++prevCol)
      {
        typename MatrixType::ColXpr prevColVec = Q.col(prevCol);
        colVec -= colVec.dot(prevColVec)*prevColVec;
      }
      Q.col(col) = colVec.normalized();
    }

    // this additional orthogonalization is not necessary in theory but should enhance
    // the numerical orthogonality of the matrix
    for (int row = 0; row < size; ++row)
    {
      typename MatrixType::RowXpr rowVec = Q.row(row);
      for (int prevRow = 0; prevRow < row; ++prevRow)
      {
        typename MatrixType::RowXpr prevRowVec = Q.row(prevRow);
        rowVec -= rowVec.dot(prevRowVec)*prevRowVec;
      }
      Q.row(row) = rowVec.normalized();
    }

    // final check
    is_unitary = Q.isUnitary();
    --max_tries;
  }

  if (max_tries == 0)
    eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!");

  return Q;
}

//  Constructs a random matrix from the special unitary group SU(size).
template <typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size)
{
  typedef T Scalar;

  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;

  // initialize unitary matrix
  MatrixType Q = randMatrixUnitary<Scalar>(size);

  // tweak the first column to make the determinant be 1
  Q.col(0) *= numext::conj(Q.determinant());

  return Q;
}

template <typename MatrixType>
void run_test(int dim, int num_elements)
{
  using std::abs;
  typedef typename internal::traits<MatrixType>::Scalar Scalar;
  typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
  typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX;

  // MUST be positive because in any other case det(cR_t) may become negative for
  // odd dimensions!
  const Scalar c = abs(internal::random<Scalar>());

  MatrixX R = randMatrixSpecialUnitary<Scalar>(dim);
  VectorX t = Scalar(50)*VectorX::Random(dim,1);

  MatrixX cR_t = MatrixX::Identity(dim+1,dim+1);
  cR_t.block(0,0,dim,dim) = c*R;
  cR_t.block(0,dim,dim,1) = t;

  MatrixX src = MatrixX::Random(dim+1, num_elements);
  src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));

  MatrixX dst = cR_t*src;

  MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements));

  const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm();
  VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon());
}

template<typename Scalar, int Dimension>
void run_fixed_size_test(int num_elements)
{
  using std::abs;
  typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX;
  typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix;
  typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix;
  typedef Matrix<Scalar, Dimension, 1> FixedVector;

  const int dim = Dimension;

  // MUST be positive because in any other case det(cR_t) may become negative for
  // odd dimensions!
  const Scalar c = abs(internal::random<Scalar>());

  FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim);
  FixedVector t = Scalar(50)*FixedVector::Random(dim,1);

  HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1);
  cR_t.block(0,0,dim,dim) = c*R;
  cR_t.block(0,dim,dim,1) = t;

  MatrixX src = MatrixX::Random(dim+1, num_elements);
  src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));

  MatrixX dst = cR_t*src;

  Block<MatrixX, Dimension, Dynamic> src_block(src,0,0,dim,num_elements);
  Block<MatrixX, Dimension, Dynamic> dst_block(dst,0,0,dim,num_elements);

  HomMatrix cR_t_umeyama = umeyama(src_block, dst_block);

  const Scalar error = ( cR_t_umeyama*src - dst ).array().square().sum();

  VERIFY(error < Scalar(10)*std::numeric_limits<Scalar>::epsilon());
}

void test_umeyama()
{
  for (int i=0; i<g_repeat; ++i)
  {
    const int num_elements = internal::random<int>(40,500);

    // works also for dimensions bigger than 3...
    for (int dim=2; dim<8; ++dim)
    {
      CALL_SUBTEST_1(run_test<MatrixXd>(dim, num_elements));
      CALL_SUBTEST_2(run_test<MatrixXf>(dim, num_elements));
    }

    CALL_SUBTEST_3((run_fixed_size_test<float, 2>(num_elements)));
    CALL_SUBTEST_4((run_fixed_size_test<float, 3>(num_elements)));
    CALL_SUBTEST_5((run_fixed_size_test<float, 4>(num_elements)));

    CALL_SUBTEST_6((run_fixed_size_test<double, 2>(num_elements)));
    CALL_SUBTEST_7((run_fixed_size_test<double, 3>(num_elements)));
    CALL_SUBTEST_8((run_fixed_size_test<double, 4>(num_elements)));
  }

  // Those two calls don't compile and result in meaningful error messages!
  // umeyama(MatrixXcf(),MatrixXcf());
  // umeyama(MatrixXcd(),MatrixXcd());
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

// エラトステネスの篩
template <typename T>
std::vector<T> sieve_of_eratosthenes(T n) {
	std::vector<T> sieve(n, 0);

	for(int i = 2; i < n; i++) {
		sieve[i] = i;
	}

	T i = 2;
	while(i * i < n) {
		if(sieve[i]) {
			for(T j = i * i; j < n; j += i) {
				sieve[j] = 0;
			}
		}
		i++;
	}

	return sieve;
}

// 素数リスト
template <typename T>
std::vector<T> prime_list(T n) {
	std::vector<T> primes = sieve_of_eratosthenes(n);
	primes.erase(std::remove(primes.begin(), primes.end(), 0), primes.end());
	return primes;
}

int main() {
	int q;
	cin >> q;
	VI l(q);
	VI r(q);
	REP(i, q) { cin >> l[i] >> r[i]; }
	VI sieve = sieve_of_eratosthenes(100030);
	VI primes = prime_list(100030);
	VI count(100001, 0);
	for(int i = 1; i <= 100000; i += 2) {
		count[i] += (sieve[i] && sieve[(i + 1) / 2]) ? 1 : 0;
	}
	VI s(100002, 0);
	for(int i = 1; i <= 100001; i++) {
		s[i] = s[i - 1] + count[i - 1];
	}
	REP(i, q) { cout << s[r[i] + 1] - s[l[i]] << endl; }
	return 0;
}


//   Copyright (c) 2018 Shahrzad Shirzad
//
//   Distributed under the Boost Software License, Version 1.0.0. (See accompanying
//   file LICENSE_1_0.0.txt or copy at http://www.boost.org/LICENSE_1_0.0.txt)

#include <phylanx/phylanx.hpp>
#include <hpx/hpx_init.hpp>

#include <iostream>

#include <blaze/Math.h>
#include <boost/program_options.hpp>
#include <cstdint>
#include <string>

///////////////////////////////////////////////////////////////////////////////
char const* const read_x_code = R"(
    //
    // Read input-data from given CSV file
    //
    define(read_x, filepath, row_start, row_stop, col_start, col_stop,
        slice(file_read_csv(filepath), make_list(row_start , row_stop),
              make_list(col_start , col_stop))
    )
    read_x
)";

char const* const als_code = R"(
    //
    // Alternating Least squares algorithm
    //
    define(als, ratings, regularization, num_factors, iterations, alpha, enable_output,
        block(
            define(num_users, shape(ratings, 0)),
            define(num_items, shape(ratings, 1)),
            define(conf, alpha * ratings),

            define(conf_u, constant(0.0, make_list(num_items))),
            define(conf_i, constant(0.0,make_list(num_users))),

            define(c_u, constant(0.0, make_list(num_items, num_items))),
            define(c_i, constant(0.0, make_list(num_users, num_users))),
            define(p_u, constant(0.0, make_list(num_items))),
            define(p_i, constant(0.0, make_list(num_users))),

            set_seed(0),
            define(X, random(make_list(num_users, num_factors))),
            define(Y, random(make_list(num_items, num_factors))),
            define(I_f, identity(num_factors)),
            define(I_i, identity(num_items)),
            define(I_u, identity(num_users)),
            define(k, 0),
            define(i, 0),
            define(u, 0),

            define(XtX, constant(0.0, make_list(num_factors, num_factors))),
            define(YtY, constant(0.0, make_list(num_factors, num_factors))),
            define(A, constant(0.0, make_list(num_factors, num_factors))),
            define(b, constant(0.0, make_list(num_factors))),

            while(k < iterations,
                block(
                    if(enable_output,
                            block(
                                    cout("iteration ",k),
                                    cout("X: ",X),
                                    cout("Y: ",Y)
                            )
                    ),
                    store(YtY, dot(transpose(Y), Y) + regularization * I_f),
                    store(XtX, dot(transpose(X), X) + regularization * I_f),

                    while(u < num_users,
                        block(
                            store(conf_u, slice_row(conf, u)),
                            store(c_u, diag(conf_u)),
                            store(p_u, __ne(conf_u,0.0,true)),
                            store(A, dot(dot(transpose(Y), c_u), Y)+ YtY),
                            store(b, dot(dot(transpose(Y), (c_u + I_i)), transpose(p_u))),
                            store(slice(X, list(u, u + 1, 1),nil), dot(inverse(A), b)),
                            store(u, u + 1)
                        )
                    ),
                    store(u, 0),
                    while(i < num_items,
                        block(
                            store(conf_i, slice_column(conf, i)),
                            store(c_i, diag(conf_i)),
                            store(p_i, __ne(conf_i, 0.0, true)),
                            store(A, dot(dot(transpose(X), c_i),X) + XtX),
                            store(b, dot(dot(transpose(X), (c_i + I_u)), transpose(p_i))),
                            store(slice(Y, list(i, i + 1, 1),nil), dot(inverse(A), b)),
                            store(i, i + 1)
                        )
                    ),
                    store(i, 0),
                    store(k, k + 1)
                )
            ),
            list(X, Y)
        )
    )
    als
)";

int hpx_main(boost::program_options::variables_map& vm)
{
    if (vm.count("data_csv") == 0)
    {
        std::cerr << "Please specify '--data_csv=data-file'";
        return hpx::finalize();
    }

    // evaluate generated execution tree
    auto row_start = static_cast<int64_t>(0);
    auto col_start = static_cast<int64_t>(0);
    auto row_stop = vm["row_stop"].as<std::int64_t>();
    auto col_stop = vm["col_stop"].as<std::int64_t>();

    auto regularization = vm["regularization"].as<double>();
    auto iterations = vm["iterations"].as<int64_t>();
    auto num_factors = vm["factors"].as<int64_t>();
    auto alpha = vm["alpha"].as<double>();
    auto filepath = vm["data_csv"].as<std::string>();

    bool enable_output = vm.count("enable_output") != 0;

    // compile the given code
    phylanx::execution_tree::compiler::function_list snippets;
    auto const& code_read_x =
        phylanx::execution_tree::compile("read_x", read_x_code, snippets);
    auto read_x = code_read_x.run();

    auto ratings = read_x(filepath, row_start, row_stop, col_start, col_stop);

    auto const& code_als = phylanx::execution_tree::compile(als_code, snippets);
    auto als = code_als.run();

    hpx::evaluate_active_counters(true, "start");
    hpx::util::high_resolution_timer t;
    auto result = als(
        ratings, regularization, num_factors, iterations, alpha, enable_output);
    auto time_diff = t.elapsed();
    std::cout << t.elapsed();

    hpx::evaluate_active_counters(true, " finish");

    auto result_r = phylanx::execution_tree::extract_list_value(result);
    auto it = result_r.begin();
    std::cout << "X: \n"
              << phylanx::execution_tree::extract_numeric_value(*it++)
              << "\nY: \n"
              << phylanx::execution_tree::extract_numeric_value(*it)
              << std::endl;

    return hpx::finalize();
}

int main(int argc, char* argv[])
{
    // command line handling
    boost::program_options::options_description desc("usage: als [options]");
    desc.add_options()
            ("enable_output,e", "enable progress output (default: false)")
            ("iterations,i",
             boost::program_options::value<std::int64_t>()->default_value(3),
             "number of iterations (default: 10.0)")
            ("factors,f",
             boost::program_options::value<std::int64_t>()->default_value(10),
             "number of factors (default: 10)")
            ("alpha,a",
             boost::program_options::value<double>()->default_value(40),
             "alpha (default: 40)")
            ("regularization,r",
             boost::program_options::value<double>()->default_value(0.1),
             "regularization (default: 0.1)")
            ("data_csv",
             boost::program_options::value<std::string>(),
             "file name for reading data")
            ("row_stop",
             boost::program_options::value<std::int64_t>()->default_value(10),
             "row_stop (default: 10)")
            ("col_stop",
             boost::program_options::value<std::int64_t>()->default_value(100),
             "col_stop (default: 100)")
            ;
    return hpx::init(desc, argc, argv);
}


//
//  Copyright (c) 2018-2019, Cem Bassoy, cem.bassoy@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <iostream>

int main()
{
	using namespace boost::numeric::ublas;

	using format_t  = column_major;
	using value_t   = float;
	using tensor_t = tensor<value_t,format_t>;
	using matrix_t = matrix<value_t,format_t>;
	using namespace boost::numeric::ublas::index;

	// Tensor-Vector-Multiplications - Including Transposition
	{

		auto n = shape{3,4,2};
		auto A = tensor_t(n,1);
		auto B1 = matrix_t(n[1],n[2],2);
		auto v1 = tensor_t(shape{n[0],1},2);
		auto v2 = tensor_t(shape{n[1],1},2);
//		auto v3 = tensor_t(shape{n[2],1},2);

		// C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);
		// tensor_t C1 = B1 + prod(A,vector_t(n[0],1),1);
//		tensor_t C1 = B1 + A(_i,_,_) * v1(_i,_);

		// C2(i,k) = A(i,j,k)*v2(j) + 4;
		//tensor_t C2 = prod(A,vector_t(n[1],1),2) + 4;
//		tensor_t C2 = A(_,_i,_) * v2(_i,_) + 4;

		// not yet implemented!
		// C3() = A(i,j,k)*T1(i)*T2(j)*T2(k);
		// tensor_t C3 = prod(prod(prod(A,v1,1),v2,1),v3,1);
		// tensor_t C3 = A(_i,_j,_k) * v1(_i,_) * v2(_j,_) * v3(_k,_);

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C2(i,k) = A(i,j,k)*v2(j) + 4;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

	}

	// Tensor-Matrix-Multiplications - Including Transposition
	{
		auto n = shape{3,4,2};
		auto m = 5u;
		auto A = tensor_t(n,2);
		auto B  = tensor_t(shape{n[1],n[2],m},2);
		auto B1 = tensor_t(shape{m,n[0]},1);
		auto B2 = tensor_t(shape{m,n[1]},1);

		// C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);
		// tensor_t C1 = B + prod(A,B1,1);
//		tensor_t C1 = B + A(_i,_,_) * B1(_,_i);

		// C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;
		// tensor_t C2 = prod(A,B2) + 4;
//		tensor_t C2 =  A(_,_j,_) * B2(_,_j) + 4;

		// C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);
		// not yet implemented.

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

//		// formatted output
//		std::cout << "% --------------------------- " << std::endl;
//		std::cout << "% --------------------------- " << std::endl << std::endl;
//		std::cout << "% C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);" << std::endl << std::endl;
//		std::cout << "C3=" << C3 << ";" << std::endl << std::endl;
	}

	// Tensor-Tensor-Multiplications Including Transposition
	{
		auto na = shape{3,4,5};
		auto nb = shape{4,6,3,2};
		auto A = tensor_t(na,2);
		auto B = tensor_t(nb,3);
		auto T1 = tensor_t(shape{na[2],na[2]},2);
		auto T2 = tensor_t(shape{na[2],nb[1],nb[3]},2);

		// C1(j,l) = T1(j,l) + A(i,j,k)*A(i,j,l) + 5;
		// tensor_t C1 = T1 + prod(A,A,perm_t{1,2}) + 5;
//		tensor_t C1 = T1 + A(_i,_j,_m)*A(_i,_j,_l) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(k,l) = T1(k,l) + A(i,j,k)*A(i,j,l) + 5;" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;
		//tensor_t C2 = T2 + prod(A,B,perm_t{1,2},perm_t{3,1}) + 5;
//		tensor_t C2 = T2 + A(_i,_j,_k)*B(_j,_l,_i,_m) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "%  C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

	}
}


/**
 * @author Alessandro Bianco
 */

/**
 * @addtogroup DFNs
 * @{
 */

#include "EssentialMatrixDecomposition.hpp"

#include <Errors/Assert.hpp>
#include <Macros/YamlcppMacros.hpp>

#include <opencv2/calib3d.hpp>
#include <Eigen/Geometry>

#include <stdlib.h>
#include <fstream>

using namespace PoseWrapper;
using namespace MatrixWrapper;
using namespace CorrespondenceMap2DWrapper;
using namespace Converters;
using namespace Helpers;

namespace CDFF
{
namespace DFN
{
namespace CamerasTransformEstimation
{

EssentialMatrixDecomposition::EssentialMatrixDecomposition()
{
	parameters = DEFAULT_PARAMETERS;

	parametersHelper.AddParameter<int>("GeneralParameters", "NumberOfTestPoints", parameters.numberOfTestPoints, DEFAULT_PARAMETERS.numberOfTestPoints);
	parametersHelper.AddParameter<double>("FirstCameraMatrix", "FocalLengthX", parameters.firstCameraMatrix.focalLengthX, DEFAULT_PARAMETERS.firstCameraMatrix.focalLengthX);
	parametersHelper.AddParameter<double>("FirstCameraMatrix", "FocalLengthY", parameters.firstCameraMatrix.focalLengthY, DEFAULT_PARAMETERS.firstCameraMatrix.focalLengthY);
	parametersHelper.AddParameter<double>("FirstCameraMatrix", "PrinciplePointX", parameters.firstCameraMatrix.principlePoint.x, DEFAULT_PARAMETERS.firstCameraMatrix.principlePoint.x);
	parametersHelper.AddParameter<double>("FirstCameraMatrix", "PrinciplePointY", parameters.firstCameraMatrix.principlePoint.y, DEFAULT_PARAMETERS.firstCameraMatrix.principlePoint.y);
	parametersHelper.AddParameter<double>("SecondCameraMatrix", "FocalLengthX", parameters.secondCameraMatrix.focalLengthX, DEFAULT_PARAMETERS.secondCameraMatrix.focalLengthX);
	parametersHelper.AddParameter<double>("SecondCameraMatrix", "FocalLengthY", parameters.secondCameraMatrix.focalLengthY, DEFAULT_PARAMETERS.secondCameraMatrix.focalLengthY);
	parametersHelper.AddParameter<double>("SecondCameraMatrix", "PrinciplePointX", parameters.secondCameraMatrix.principlePoint.x, DEFAULT_PARAMETERS.secondCameraMatrix.principlePoint.x);
	parametersHelper.AddParameter<double>("SecondCameraMatrix", "PrinciplePointY", parameters.secondCameraMatrix.principlePoint.y, DEFAULT_PARAMETERS.secondCameraMatrix.principlePoint.y);

	firstCameraMatrix = ConvertToMat(DEFAULT_PARAMETERS.firstCameraMatrix);
	secondCameraMatrix = ConvertToMat(DEFAULT_PARAMETERS.secondCameraMatrix);

	configurationFilePath = "";
}

EssentialMatrixDecomposition::~EssentialMatrixDecomposition()
{
}

void EssentialMatrixDecomposition::configure()
{
	parametersHelper.ReadFile(configurationFilePath);
	ValidateParameters();

	firstCameraMatrix = ConvertToMat(parameters.firstCameraMatrix);
	secondCameraMatrix = ConvertToMat(parameters.secondCameraMatrix);
}

void EssentialMatrixDecomposition::process()
{
	// Read data from input port
	cv::Mat correspondenceMap = Convert(&inMatches);
	cv::Mat fundamentalMatrix = ConvertToMat(&inFundamentalMatrix);

	// Process data
	ValidateInputs(fundamentalMatrix, correspondenceMap);
	std::vector<cv::Mat> transformsList = ComputeTransformMatrix(fundamentalMatrix);
	int validTransformIndex = FindValidTransform(transformsList, correspondenceMap);

	// Write data to output port
	if (validTransformIndex < 0)
	{
		// outTransform =  new Transform3D();
		outSuccess = false;
	}
	else
	{
		Pose3DConstPtr tmp = matToTransform3DConverter.Convert(transformsList.at(validTransformIndex));
		Copy(*tmp, outTransform);
		delete(tmp);
		outSuccess = true;
	}
}

const EssentialMatrixDecomposition::EssentialMatrixDecompositionOptionsSet EssentialMatrixDecomposition::DEFAULT_PARAMETERS =
{
	/*.numberOfTestPoints =*/ 20,
	//.firstCameraMatrix =
	{
		/*.focalLengthX =*/ 1.0,
		/*.focalLengthY =*/ 1.0,
		/*.principlePoint =*/ cv::Point2d(0, 0)
	},
	//.secondCameraMatrix =
	{
		/*.focalLengthX =*/ 1.0,
		/*.focalLengthY =*/ 1.0,
		/*.principlePoint =*/ cv::Point2d(0, 0)
	}
};

cv::Mat EssentialMatrixDecomposition::ConvertToMat(CameraMatrix cameraMatrix)
{
	cv::Mat conversion(3, 3, CV_64FC1, cv::Scalar(0));
	conversion.at<double>(0,0) = cameraMatrix.focalLengthX;
	conversion.at<double>(1,1) = cameraMatrix.focalLengthY;
	conversion.at<double>(0,2) = cameraMatrix.principlePoint.x;
	conversion.at<double>(1,2) = cameraMatrix.principlePoint.y;
	conversion.at<double>(2,2) = 1.0;
	return conversion;
}

cv::Mat EssentialMatrixDecomposition::ConvertToMat(MatrixWrapper::Matrix3dConstPtr matrix)
{
	cv::Mat cvMatrix(3, 3, CV_64FC1);
	for (unsigned row = 0; row < 3; row++)
	{
		for (unsigned column = 0; column < 3; column++)
		{
			cvMatrix.at<double>(row,column) = GetElement(*matrix, row, column);
		}
	}
	return cvMatrix;
}

cv::Mat EssentialMatrixDecomposition::Convert(CorrespondenceMap2DConstPtr correspondenceMap)
{
	cv::Mat cvCorrespondenceMap(4, GetNumberOfCorrespondences(*correspondenceMap), CV_64FC1);
	for (int correspondenceIndex = 0; correspondenceIndex < GetNumberOfCorrespondences(*correspondenceMap); correspondenceIndex++)
	{
		BaseTypesWrapper::Point2D firstPoint = GetSource(*correspondenceMap, correspondenceIndex);
		BaseTypesWrapper::Point2D secondPoint = GetSink(*correspondenceMap, correspondenceIndex);
		cvCorrespondenceMap.at<double>(0, correspondenceIndex) = firstPoint.x;
		cvCorrespondenceMap.at<double>(1, correspondenceIndex) = firstPoint.y;
		cvCorrespondenceMap.at<double>(2, correspondenceIndex) = secondPoint.x;
		cvCorrespondenceMap.at<double>(3, correspondenceIndex) = secondPoint.y;
	}
	return cvCorrespondenceMap;
}

std::vector<cv::Mat> EssentialMatrixDecomposition::ComputeTransformMatrix(cv::Mat fundamentalMatrix)
{
	cv::Mat essentialMatrix = secondCameraMatrix.t() * fundamentalMatrix * firstCameraMatrix;

	cv::Mat firstRotationMatrix, secondRotationMatrix, translationMatrix;
	cv::decomposeEssentialMat(essentialMatrix, firstRotationMatrix, secondRotationMatrix, translationMatrix);

	std::vector<cv::Mat> projectionMatricesList(4);
	cv::hconcat(firstRotationMatrix, translationMatrix, projectionMatricesList[0]);
	cv::hconcat(firstRotationMatrix, -translationMatrix, projectionMatricesList[1]);
	cv::hconcat(secondRotationMatrix, translationMatrix, projectionMatricesList[2]);
	cv::hconcat(secondRotationMatrix, -translationMatrix, projectionMatricesList[3]);

	return projectionMatricesList;
}

int EssentialMatrixDecomposition::FindValidTransform(std::vector<cv::Mat> projectionsList, cv::Mat correspondenceMap)
{
	static const float EPSILON = 1e-2;

	int validMatrixIndex = -1;
	bool validMatrixFound = false;
	for (unsigned matrixIndex = 0; matrixIndex < projectionsList.size(); matrixIndex++)
	{
		cv::Mat currentProjectionMatrix = projectionsList.at(matrixIndex);
		double rotationDeterminant = cv::determinant( currentProjectionMatrix( cv::Rect(0,0,2,2) ));
		bool orientationPreserved = std::abs(rotationDeterminant - 1) < EPSILON;

		bool matrixIsValid = orientationPreserved && ProjectionMatrixIsValidForTestPoints(currentProjectionMatrix, correspondenceMap);

		if (matrixIsValid)
		{
			if (validMatrixFound)
			{
				// There should only be one valid matrix. If multiple valid matrices are found, the decomposition has failed.
				return -1;
			}
			validMatrixFound = true;
			validMatrixIndex = matrixIndex;
		}
	}

	return validMatrixIndex;
}

//Projection test as described by described in Richard Hartley and Andrew Zisserman, in "Multiple View Geometry in Computer Vision".
bool EssentialMatrixDecomposition::ProjectionMatrixIsValidForTestPoints(cv::Mat projectionMatrix, cv::Mat correspondenceMap)
{
	static const float EPSILON = 1e-2;

	double rotationDeterminant = cv::determinant( projectionMatrix(cv::Rect(0,0,3,3)) );
	int rotationDeterminantSign = rotationDeterminant >= 0 ? 1 : -1;
	double principleRayX = projectionMatrix.at<double>(2, 0);
	double principleRayY = projectionMatrix.at<double>(2, 1);
	double principleRayZ = projectionMatrix.at<double>(2, 2);
	double principleRayNorm = std::sqrt (principleRayX*principleRayX + principleRayY*principleRayY + principleRayZ*principleRayZ);

	cv::Mat identityProjection(3, 4, CV_64FC1, cv::Scalar(0));
	identityProjection.at<double>(0,0) = 1;
	identityProjection.at<double>(1,1) = 1;
	identityProjection.at<double>(2,2) = 1;

	unsigned testPointsNumber = (correspondenceMap.cols > parameters.numberOfTestPoints) ? parameters.numberOfTestPoints : correspondenceMap.cols;
	cv::Mat testPointCloudMatrix;
	cv::triangulatePoints(
		firstCameraMatrix * identityProjection,
		secondCameraMatrix * projectionMatrix,
		correspondenceMap(cv::Rect(0, 0, testPointsNumber, 2 )),
		correspondenceMap(cv::Rect(0, 2, testPointsNumber, 2 )),
		testPointCloudMatrix
	);

	bool matrixIsValid = false;
	bool matrixIsInvalid = false;
	unsigned validPointsCount = 0;
	unsigned invalidPointsCount = 0;
	for (unsigned pointIndex = 0; pointIndex < testPointCloudMatrix.cols && !matrixIsValid && !matrixIsInvalid; pointIndex++)
	{
		cv::Mat testPoint = testPointCloudMatrix(cv::Rect(pointIndex, 0, 1, 4));
		cv::Mat projectedPoint = projectionMatrix * testPoint;
		float t = testPoint.at<double>(3, 0);
		float w = projectedPoint.at<double>(2, 0);
		double depth = (rotationDeterminantSign * w ) / (t * principleRayNorm);

		double scale = 4;
		cv::Mat scaledTestPoint = scale * testPoint;
		cv::Mat scaledProjectedPoint = projectionMatrix * scaledTestPoint;
		double scaledT = scaledTestPoint.at<double>(3, 0);
		double scaledW = scaledProjectedPoint.at<double>(2, 0);
		double scaledDepth = (rotationDeterminantSign * scaledW ) / (scaledT * principleRayNorm);

		bool validPoint = (depth < scaledDepth + EPSILON && depth > scaledDepth - EPSILON && depth > 0);
		if (validPoint)
		{
			validPointsCount++;
		}
		else
		{
			invalidPointsCount++;
		}
		matrixIsValid = validPointsCount >= testPointsNumber / 2;
		matrixIsInvalid = invalidPointsCount >= testPointsNumber / 2;
	}

	return matrixIsValid || !matrixIsInvalid;
}

void EssentialMatrixDecomposition::ValidateParameters()
{
	ASSERT(parameters.numberOfTestPoints > 1, "EssentialMatrixComputation Configuration Error: number of test points has to be positive and greater than 1");
	ASSERT(parameters.firstCameraMatrix.focalLengthX > 0 && parameters.firstCameraMatrix.focalLengthY > 0, "EssentialMatrixComputation Configuration Error: focalLength is not positive");
	ASSERT(parameters.secondCameraMatrix.focalLengthX > 0 && parameters.secondCameraMatrix.focalLengthY > 0, "EssentialMatrixComputation Configuration Error: focalLength is not positive");
}

void EssentialMatrixDecomposition::ValidateInputs(cv::Mat fundamentalMatrix, cv::Mat correspondenceMap)
{
	ASSERT(fundamentalMatrix.cols == 3 && fundamentalMatrix.rows == 3, "EssentialMatrixComputation Error, unexpected fundamental matrix size");
}

}
}
}

/** @} */


/*****************************************************************************/
/*                                                                           */
/* Best-fit                                                                  */
/*                                                                           */
/* Copyright 2014                                                            */
/* Alasdair Craig                                                            */
/* ac@acraig.za.net                                                          */
/* License: Code Project Open License 1.02                                   */
/* http://www.codeproject.com/info/cpol10.aspx                               */
/*                                                                           */
/*****************************************************************************/

#ifdef _MSC_VER
#pragma warning( disable : 4244 ) // suppress possible loss-of-data warning arising from boost
#endif

#include "BestFit.h"
#include "cholesky.h"

#include <limits>
#include <cassert>
#include <stdexcept>
#include <iomanip>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#define CONVERGENCE_CRITERIA 0.000000001
#define MAX_ITERATIONS 50

BestFit::BestFit(int unknowns)
	: m_verbosity(1)
	, m_oStream(m_nullStream)
	, m_solution(unknowns,1)
	, m_provisionals(unknowns,1)
	, m_numObs(0)
	, m_numUnknowns(unknowns)
	, m_minx( std::numeric_limits<double>::max())
	, m_maxx(-std::numeric_limits<double>::max())
	, m_miny( std::numeric_limits<double>::max())
	, m_maxy(-std::numeric_limits<double>::max())
{
}

BestFit::BestFit(int unknowns, std::ostream &oStream)
	: m_verbosity(1)
	, m_oStream(oStream)
	, m_solution(unknowns,1)
	, m_provisionals(unknowns,1)
	, m_numObs(0)
	, m_numUnknowns(unknowns)
	, m_minx( std::numeric_limits<double>::max())
	, m_maxx(-std::numeric_limits<double>::max())
	, m_miny( std::numeric_limits<double>::max())
	, m_maxy(-std::numeric_limits<double>::max())
{
}

BestFit::~BestFit()
{
}

// Set the level of detail required for output to stdout
void BestFit::SetVerbosity(int verbosity)
{
	m_verbosity = verbosity;
}

bool BestFit::Compute(BestFitIO &in, BestFitIO &out)
{
	SetVerbosity(in.verbosity);

	if (!in.points || in.numPoints == 0)
		{
		m_oStream << "No solution. No input points." << std::endl;
		return false;
		}
	if (in.numPoints < m_numUnknowns + 1)
		{
		m_oStream << "No solution. Too few input points, need " << m_numUnknowns + 1 - in.numPoints << " or more." << std::endl;
		return false;
		}

	m_numObs = in.numPoints;

	ResizeMatrices();

	for (int i = 0; i < m_numObs; ++i)
	{
		double x = in.points[i * 2 + 0];
		double y = in.points[i * 2 + 1];
		AddObservation(i, x, y);
	}

	Compute();

	FillOutput(out);

	return true;
}

void BestFit::ResizeMatrices()
{
	m_residuals.resize(m_numObs, 1);
	m_design.resize(m_numObs, m_numUnknowns);
	m_l.resize(m_numObs, 1);
	m_qweight.resize(m_numObs, m_numObs, 0, 0);
	m_observations.resize(m_numObs, 2);
	m_b.resize(m_numObs, m_numObs * 2);
}

// Add another observable (coordinate) to the computation object
void BestFit::AddObservation(int count, double x, double y)
{
	m_observations(count, 0) = x;
	m_observations(count, 1) = y;

	m_minx = std::min<double>(m_minx, x);
	m_maxx = std::max<double>(m_maxx, x);
	m_miny = std::min<double>(m_miny, y);
	m_maxy = std::max<double>(m_maxy, y);
}

// Do the least-squares adjustment
bool BestFit::Compute()
{
	if (m_verbosity > 1)
		m_oStream << "Observations:    " << m_observations << std::endl;

	GenerateProvisionals();

	bool successful = true;

	int iteration = 0;

	while (true)
		{
		FormulateMatrices();

		if (m_verbosity > 1)
			{
			m_oStream << "Provisionals:    " << m_provisionals << std::endl;
			m_oStream << "l-matrix:        " << m_l << std::endl;
			}

		// evaluate the unknowns - small corrections to be applied to the provisional unknowns
		if (EvaluateUnknowns())
			{
			++iteration;

			// add the solution to the provisional unknowns
			EvaluateAdjustedUnknowns();

			if (HasConverged())
				break;
			if (IsDegenerate(iteration))
				break;
			}
		else
			{
			successful = false;
			break;
			}
		}

	successful = successful && (iteration > 0 && iteration < MAX_ITERATIONS);
	if (successful)
		{
		// Give ellipse chance to normalise it's axes.
		NormaliseAdjustedUnknowns();

		// evaluate the residuals
		EvaluateResiduals();

		// add the residuals to the provisional observations
		EvaluateAdjustedObservations();

		// Check that the adjusted unknowns and adjusted observations satisfy
		// the original line/circle/ellipse equation.
		GlobalCheck();

		// Subsequent error analysis and statistical output
		ErrorAnalysis(iteration);

		if (0 == m_verbosity)
			OutputSimpleSolution();
		}

	return successful;
}

// Matrix inversion routine using LU decomposition
bool BestFit::InvertMatrix(const ublas::matrix<double> &input, ublas::matrix<double> &inverse)
{
	// create a working copy of the input
	ublas::matrix<double> A(input);

	// create a permutation matrix for the LU-factorization
	ublas::permutation_matrix<double> pm(A.size1());

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

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

	try
		{
		// back-substitute to get the inverse
		ublas::lu_substitute(A, pm, inverse);
		}
	catch (std::logic_error &)
		{
		return false;
		}

	return true;
}

// Determine solution vector
bool BestFit::EvaluateUnknowns()
{
	ublas::matrix<double> pa = ublas::prod(m_qweight, m_design);
	ublas::matrix<double> atpa = ublas::prod(ublas::trans(m_design), pa);

	ublas::matrix<double> inverse(atpa.size1(), atpa.size2());
	//if (CholeskyInversion(atpa, inverse))
	if (InvertMatrix(atpa, inverse))
		{
		ublas::matrix<double> pl = ublas::prod(m_qweight, m_l);
		ublas::matrix<double> atpl = ublas::prod(ublas::trans(m_design), pl);

		m_solution = ublas::prod(inverse, atpl);

		if (m_verbosity > 2)
			m_oStream << "Iter. solution   " << m_solution << std::endl;
		return true;
		}
	else
		m_oStream << "No solution. Cannot invert matrix." << std::endl;
	return false;
}

// Add solution to the provisionals
void BestFit::EvaluateAdjustedUnknowns()
{
	m_provisionals += m_solution;

	if (m_verbosity > 2)
		m_oStream << "Iter adj unknowns" << m_provisionals << std::endl;
}

// Residuals to the initial observations
void BestFit::EvaluateResiduals()
{
	// v = Ax-l, quasi-residuals in the case of circle and ellipse
	m_residuals = ublas::prod(m_design, m_solution) - m_l;

	if (m_verbosity > 1)
		m_oStream << "Residuals:       " << m_residuals << std::endl;
}

void BestFit::EvaluateFinalResiduals(int point, double &vxi, double &vyi) const
{
	// In the case of the quasi-parametric case (circle and ellipse) the actual
	// residuals for both the x and y coordinate need to be extracted.
	vxi = m_design(point, 0) * m_qweight(point, point) * m_residuals(point, 0);
	vyi = m_design(point, 1) * m_qweight(point, point) * m_residuals(point, 0);

	//// Method 1: ref (2:35)
	//ublas::matrix<double> bt = ublas::trans(m_b);
	//ublas::matrix<double> pv = ublas::prod(m_qweight, m_residuals);
	//ublas::matrix<double> btpv = ublas::prod(bt, pv);
	//double avxi = -btpv(point * 2 + 0,0);
	//double avyi = -btpv(point * 2 + 1,0);
	//m_oStream << "New residuals (Method 1) differ by: " << avxi - vxi << "," << avyi - vyi << std::endl;

	//// Method 2: ref (2:28)
	//double dx = m_observations(point, 0) - m_provisionals(0, 0);
	//double dy = m_observations(point, 1) - m_provisionals(1, 0);
	//double r = m_provisionals(2, 0);
	//double quasiv = -2.0 * dx * m_solution(0, 0) - 2.0 * dy * m_solution(1, 0) - 2.0 * r * m_solution(2, 0) - m_l(point, 0);
	//double bvxi = - (quasiv * 2.0 * dx) / (4.0 * (dx * dx + dy * dy));
	//double bvyi = - (quasiv * 2.0 * dy) / (4.0 * (dx * dx + dy * dy));
	//m_oStream << "New residuals (Method 2) differ by: " << bvxi - vxi << "," << bvyi - vyi << std::endl;
}

// Add residuals to initial observations
void BestFit::EvaluateAdjustedObservations()
{
	for (int i = 0; i < m_numObs; ++i)
		{
		double vxi = 0.0;
		double vyi = 0.0;
		EvaluateFinalResiduals(i, vxi, vyi); // overridden for normal (non-quasi-parametric) BestFitLine

		m_observations(i, 0) += vxi;
		m_observations(i, 1) += vyi;
		}

	if (m_verbosity > 1)
		m_oStream << "Adj. observations" << m_observations << std::endl;
}

// Global check on the quality of the adjustment
void BestFit::GlobalCheck()
{
	ublas::vector<double> global(m_numObs, 0);
	bool pass = (m_numObs > 0);

	for (int i = 0; i < m_numObs; ++i)
		{
		double x = m_observations(i, 0);
		double y = m_observations(i, 1);

		global(i) = SolveAt(x, y); // should be zero
		pass = pass && fabs(global(i)) < 0.01; // TODO: Is this too lax?
		}

	if (m_verbosity > 1)
		m_oStream << "Global check:    " << global << std::endl;
	if (m_verbosity > 0)
		m_oStream << "Global check of the adjustment     ***" << (pass ? "PASSES***" : "FAILS***") << std::endl;

	// Secondary test is to check that aTPv is zero too, really only neccessary
	// if the above global check fails.
	ublas::matrix<double> atp = ublas::prod(ublas::trans(m_design), m_qweight);
	ublas::matrix<double> atpv = ublas::prod(atp, m_residuals);

	pass = true;
	for (int j = 0; j < m_numUnknowns; ++j)
		pass = pass && Double::IsZero(atpv(j, 0));

	if (!pass && m_verbosity > 1)
		m_oStream << "aTPv             " << atpv << std::endl;
	if (m_verbosity > 0)
		m_oStream << "Check of the evaluated unknowns    ***" << (pass ? "PASSES***" : "FAILS***") << std::endl;
}

// Predicate for seeing whether the solution has become sufficiently small
bool BestFit::HasConverged() const
{
	for (int i = 0; i < m_numUnknowns; ++i)
		{
		if (fabs(m_solution(i, 0)) > CONVERGENCE_CRITERIA)
			return false;
		}
	return true;
}

// Predicate for seeing whether the solution is diverging?
bool BestFit::IsDegenerate(int iteration) const
{
	bool degenerate = (iteration >= MAX_ITERATIONS);
	if (degenerate)
		m_oStream << "No solution. Does not converge." << std::endl;

	return degenerate;
}

// Output stats
// TODO: Variance-covariance matrices
void BestFit::ErrorAnalysis(int iterations)
{
	ublas::matrix<double> pv = ublas::prod(m_qweight, m_residuals);
	ublas::matrix<double> vtpv = ublas::prod(ublas::trans(m_residuals), pv);
	int degreesFreedom = m_numObs - m_numUnknowns;
	double variance = vtpv(0, 0) / degreesFreedom;
	double stddev = sqrt(variance);

	if (m_verbosity > 0)
		{
		m_oStream << std::setprecision(6) << std::setiosflags(std::ios::fixed)
			<< "Number of observations             " << m_numObs << std::endl
			<< "Number of unknowns                 " << m_numUnknowns << std::endl
			<< "Degrees of freedom                 " << degreesFreedom << std::endl
			<< "Iterations until convergence       " << iterations << std::endl
			<< "Variance                           " << variance << std::endl
			<< "Std. dev. observation unit weight  " << stddev << std::endl;

		m_oStream << "***********************************" << std::endl;

		OutputAdjustedUnknowns(m_oStream);
		}
}

// Output at it's very simplest
void BestFit::OutputSimpleSolution() const
{
	for (int i = 0; i < m_numUnknowns; ++i)
		m_oStream << m_provisionals(i, 0) << std::endl;
}

// Return selected values back to caller
void BestFit::FillOutput(BestFitIO &out) const
{
	out.numPoints = m_numObs;
	out.numOutputFields = m_numUnknowns;
	out.verbosity = m_verbosity;

	//memset(out.outputFields, 0, sizeof(outputFields));
	for (int i = 0; i < m_numUnknowns; ++i)
		out.outputFields[i] = m_provisionals(i, 0);

	if (out.wantResiduals)
		out.residuals = new double[m_numObs];

	for (int j = 0; j < m_numObs; ++j)
	{
		if (out.wantAdjustedObs)
		{
			out.points[j * 2 + 0] = m_observations(j, 0);
			out.points[j * 2 + 1] = m_observations(j, 1);
		}
		if (out.wantResiduals)
			out.residuals[j] = m_residuals(j, 0);
	}
}

void BestFit::LowerTriangularModifyInversion(const ublas::matrix<double> &l, ublas::matrix<double> &m)
{
	typedef ublas::matrix<double>::size_type INDEX;
	INDEX i, j, n = l.size1();

	for (i = 0; i < n; ++i)
		m(i, i) = 1.0 / l(i, i);

	for (j = 0; j < n; ++j)
	{
		for (i = j + 1; i < n; ++i)
		{
			assert(i > j);

			double sum = 0.0;
			for (INDEX k = j; k <= i - 1; ++k)
				sum += l(i, k) * m(k, j);

			m(i, j) = -m(i, i) * sum;
		}
	}
}

bool BestFit::CholeskyInversion(const ublas::matrix<double> &input, ublas::matrix<double> &inverse)
{
	ublas::matrix<double> l(input.size1(), input.size2());
	ublas::matrix<double> m(input.size1(), input.size2(), 0.0);

	if (0 == cholesky_decompose(input, l))
	{
		LowerTriangularModifyInversion(l, m);

		inverse = ublas::prod(ublas::trans(m), m);

		return true;
	}

	return false;
}


#include "Camera.h"

#include <Eigen>
#include <vector>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <math.h>
#include "Shape.h"
#include "Intersection.h"

#define HEIGHT 600
#define WIDTH 800
#define FLENGTH 1.0
#define PWIDTH 4.0
#define PHEIGHT 3.0
#define MAXRECDEPTH 4
#define INF 100000000

using namespace Eigen;
using namespace std;

float clamp(float a, float b, float c){
    if (c < a)
        return a;
    else if (c > b)
        return b;
    else
        return c;
}

Vector3f getr(Vector3f sray, Vector3f norm){
    Vector3f t2 = sray - 2*(sray.dot(norm)) * norm;
    return t2;
}

//Based on Scratchapixel version
Vector3f getfrac(Vector3f incom, Vector3f norm, float indx){
    float cosi = clamp(-1, 1, incom.dot(norm));
    float et = 1, ea = indx;
    Vector3f n = norm;
    if (cosi < 0)
        cosi = -cosi;
    else{
        swap(et, ea);
        n = -norm;
    }
    float eta = ea/et;
    float k = 1 - eta*eta * (1-cosi * cosi);
    if (k < 0){ //total refrac
        return getr(incom, -norm);
    }
    else{
        return eta * incom + (eta * cosi - sqrt(k)) * n;
    }
}

Camera::Camera(Vector3f la, Vector3f u,
               Vector3f p)
{
    lookat = la;
    up = u;
    pos = p;
    lookat.normalize();
    up.normalize();
    pos.normalize();
    //col major
    Vector3f matn = pos - lookat;
    matn.normalize();
    Vector3f matu = up.cross(matn);
    matu.normalize();
    Vector3f matv = matn.cross(matu);
    this->tra << matu(0), matv(0), matn(0), 0.0f,
                 matu(1), matv(1), matn(1), 0.0f,
                 matu(2), matv(2), matn(2), 0.0f,
                -(pos.dot(matu)), -(pos.dot(matv)),
                -(pos.dot(matn)), 1.0;

}

Vector3f getrtarg(Vector3f illum, float aol, float ldmax){
    Vector3f cs = aol * illum;
    float rr, gr, br;
    rr = cs[0] / (1 + cs[0]);
    gr = cs[1] / (1 + cs[1]);
    br = cs[2] / (1 + cs[2]);

    float rt,gt,bt;
    rt = rr * ldmax;
    gt = gr * ldmax;
    bt = br * ldmax;

    Vector3f targ;
    targ << rt, gt, bt;
    return targ;

}

float getlogavg(vector<float> illlist){
    float lsum = 0;
    float delta = 0.00001;
    for (float f : illlist){
        lsum += log(delta + f);
    }
    return exp(lsum / ((float)HEIGHT * WIDTH));

}

//go from RGB to Illuminance
//CRT formula
float toill(Vector3f rgb){
    return abs(.27*rgb[0] + .67*rgb[1]+.06*rgb[2]);
}

std::vector<Vector3f> torgb(vector<Vector3f> lum, float ldmax, char whichTR){
        vector<float> illum;
        for (Vector3f vf : lum){
            illum.push_back(toill(vf));
        }
        float lwa = getlogavg(illum);
    if (whichTR == 'w'){
        float powa = pow(lwa,.4);
        float ldterm = pow(ldmax/2, 0.4);
        float brackterm = (1.219 + ldterm) / (1.219 + powa);
        float sf = pow(brackterm,2.5);
        vector<Vector3f> trd;
        for (Vector3f rgb : lum){
            trd.push_back(sf * rgb);
        }
        return trd;
    }
    else{
        float aol = .18/lwa;
        vector<Vector3f> targv;
        for (Vector3f vl : lum){
            targv.push_back(getrtarg(vl,aol,ldmax));
        }
        return targv;
    }
}

void writetofile(vector<Vector3f> image, float ldmax)
{
    ofstream filewtt;
    filewtt.open("output.ppm");
    filewtt << "P3\n" << WIDTH << " ";
    filewtt << HEIGHT << "\n";
    filewtt << 255 << "\n";
    for (Vector3f pix : image){
        int r = abs((int) (pix[0] / ldmax) * 255);
        int g = abs((int) (pix[1] / ldmax) * 255);
        int b = abs((int) (pix[2] / ldmax) * 255);
        filewtt << r << " " << g << " ";
        filewtt << b << " \n";
    }
    filewtt.close();

}

Camera::~Camera()
{
    //dtor
}

Vector3f Camera::getcolor(Ray * r, int recdepth){
    vector<Shape *> clones = wld->getshapes();
    vector<Light *> lits = wld->getlits();
    Vector3f colr;
    bool foundcol = false;
    float newt;
    float champt = INF;
    Shape * coll;
    for (Shape * s : clones){
        if (s->checkcollision(r,newt)){
            foundcol = true;
            if (newt < champt){
                champt = newt;
                coll = s;
            }
        }
        }
        if (!foundcol){
            Vector3f dflt;
            dflt << 0.0, 30, 70;
            colr = dflt;
        }
        else
        {
            //colr = coll->getcolor();
            Vector3f colpt = r->getpos() + r->getdir().normalized() * champt;
            Vector3f SS =  lits.at(0)->getpos() - colpt;
            SS.normalize();
            Ray * shadow = new Ray(colpt,SS);
            bool seelight = true;
            float dummyt; //if dist = 0, hitting self
            for (Shape * s : clones){
                if (s != coll){
                    if (s->checkcollision(shadow, dummyt)){
                        if (dummyt != 0){
                            seelight = false;
                            //ambi
                            Vector3f blk;
                            blk << 0.1,0.1,0.1;
                            colr = blk;
                        }
                    }
                }
            }
            if (seelight){
                Vector3f VV = r->getdir();
                VV.normalize();
                Vector3f NN = coll->getnorm(colpt);
                NN.normalize();
                Vector3f RR = getr(VV,NN);
                Intersection * is = new Intersection();
                is->pt = colpt;
                is->incom = SS;
                is->norm = NN;
                is->reflex = RR;
                is->litlist = lits;
                colr = coll->getcolor(is);
                if (recdepth < MAXRECDEPTH){
                    if (coll->kr > 0){
                        Vector3f refr = getr(VV,NN);
                        refr.normalize();
                        Ray * nr = new Ray(colpt, refr);
                        colr += coll->kr * this->getcolor(nr, recdepth + 1);
                    }
                    if (coll->kt > 0){
                        Vector3f frac = getfrac(VV, NN, 1.01);
                        frac.normalize();
                        Ray * fr = new Ray(colpt, frac);
                        colr += coll->kt * this->getcolor(fr, recdepth + 1);
                    }
                }
            }
        }
        return colr;
}

void Camera::captureworld(World * wld){

    //Yo dawg...
    vector<Vector3f>image;
    this->wld = wld;
    float pixh = PHEIGHT / HEIGHT;
    float pixw = PWIDTH / WIDTH;
    float tx = -PWIDTH/2;
    float ty = PHEIGHT/2;

    Vector3f p;
    p << 0.0, 0.0, 0.0;

    //Generate rays and check
    for (int i = 0; i < HEIGHT; i++){
        float ypx = ty - ((.5 + float(i)) * pixh);
        for (int c = 0; c < WIDTH; c++){
                float xpx =  tx + ((.5 + float(c)) * pixw);
                Vector3f raypos;
                raypos << xpx, ypx, FLENGTH;
                raypos.normalize();
                Ray * pxr = new Ray(p,raypos);
                Vector3f colr = this->getcolor(pxr,1);
                image.push_back(colr);

        }
    }
    float ldmax = 1;
    vector<Vector3f> srgb = torgb(image, ldmax, 'w');
    writetofile(srgb,ldmax);
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_STUDENT_T_LCCDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_STUDENT_T_LCCDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/lbeta.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_beta.hpp>
#include <stan/math/prim/scal/fun/inc_beta.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <boost/random/student_t_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <limits>

namespace stan {
namespace math {

template <typename T_y, typename T_dof, typename T_loc, typename T_scale>
typename return_type<T_y, T_dof, T_loc, T_scale>::type student_t_lccdf(
    const T_y& y, const T_dof& nu, const T_loc& mu, const T_scale& sigma) {
  typedef typename stan::partials_return_type<T_y, T_dof, T_loc, T_scale>::type
      T_partials_return;

  if (size_zero(y, nu, mu, sigma))
    return 0.0;

  static const char* function = "student_t_lccdf";

  using std::exp;

  T_partials_return P(0.0);

  check_not_nan(function, "Random variable", y);
  check_positive_finite(function, "Degrees of freedom parameter", nu);
  check_finite(function, "Location parameter", mu);
  check_positive_finite(function, "Scale parameter", sigma);

  scalar_seq_view<T_y> y_vec(y);
  scalar_seq_view<T_dof> nu_vec(nu);
  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_scale> sigma_vec(sigma);
  size_t N = max_size(y, nu, mu, sigma);

  operands_and_partials<T_y, T_dof, T_loc, T_scale> ops_partials(y, nu, mu,
                                                                 sigma);

  // Explicit return for extreme values
  // The gradients are technically ill-defined, but treated as zero
  for (size_t i = 0; i < stan::length(y); i++) {
    if (value_of(y_vec[i]) == -std::numeric_limits<double>::infinity())
      return ops_partials.build(0.0);
  }

  using std::exp;
  using std::log;
  using std::pow;

  T_partials_return digammaHalf = 0;

  VectorBuilder<!is_constant_struct<T_dof>::value, T_partials_return, T_dof>
      digamma_vec(stan::length(nu));
  VectorBuilder<!is_constant_struct<T_dof>::value, T_partials_return, T_dof>
      digammaNu_vec(stan::length(nu));
  VectorBuilder<!is_constant_struct<T_dof>::value, T_partials_return, T_dof>
      digammaNuPlusHalf_vec(stan::length(nu));

  if (!is_constant_struct<T_dof>::value) {
    digammaHalf = digamma(0.5);

    for (size_t i = 0; i < stan::length(nu); i++) {
      const T_partials_return nu_dbl = value_of(nu_vec[i]);

      digammaNu_vec[i] = digamma(0.5 * nu_dbl);
      digammaNuPlusHalf_vec[i] = digamma(0.5 + 0.5 * nu_dbl);
    }
  }

  for (size_t n = 0; n < N; n++) {
    // Explicit results for extreme values
    // The gradients are technically ill-defined, but treated as zero
    if (value_of(y_vec[n]) == std::numeric_limits<double>::infinity()) {
      return ops_partials.build(negative_infinity());
    }

    const T_partials_return sigma_inv = 1.0 / value_of(sigma_vec[n]);
    const T_partials_return t
        = (value_of(y_vec[n]) - value_of(mu_vec[n])) * sigma_inv;
    const T_partials_return nu_dbl = value_of(nu_vec[n]);
    const T_partials_return q = nu_dbl / (t * t);
    const T_partials_return r = 1.0 / (1.0 + q);
    const T_partials_return J = 2 * r * r * q / t;
    const T_partials_return betaNuHalf = exp(lbeta(0.5, 0.5 * nu_dbl));
    T_partials_return zJacobian = t > 0 ? -0.5 : 0.5;

    if (q < 2) {
      T_partials_return z
          = inc_beta(0.5 * nu_dbl, (T_partials_return)0.5, 1.0 - r);
      const T_partials_return Pn = t > 0 ? 0.5 * z : 1.0 - 0.5 * z;
      const T_partials_return d_ibeta
          = pow(r, -0.5) * pow(1.0 - r, 0.5 * nu_dbl - 1) / betaNuHalf;

      P += log(Pn);

      if (!is_constant_struct<T_y>::value)
        ops_partials.edge1_.partials_[n]
            += zJacobian * d_ibeta * J * sigma_inv / Pn;

      if (!is_constant_struct<T_dof>::value) {
        T_partials_return g1 = 0;
        T_partials_return g2 = 0;

        grad_reg_inc_beta(g1, g2, 0.5 * nu_dbl, (T_partials_return)0.5, 1.0 - r,
                          digammaNu_vec[n], digammaHalf,
                          digammaNuPlusHalf_vec[n], betaNuHalf);

        ops_partials.edge2_.partials_[n]
            -= zJacobian * (d_ibeta * (r / t) * (r / t) + 0.5 * g1) / Pn;
      }

      if (!is_constant_struct<T_loc>::value)
        ops_partials.edge3_.partials_[n]
            -= zJacobian * d_ibeta * J * sigma_inv / Pn;
      if (!is_constant_struct<T_scale>::value)
        ops_partials.edge4_.partials_[n]
            -= zJacobian * d_ibeta * J * sigma_inv * t / Pn;

    } else {
      T_partials_return z
          = 1.0 - inc_beta((T_partials_return)0.5, 0.5 * nu_dbl, r);
      zJacobian *= -1;

      const T_partials_return Pn = t > 0 ? 0.5 * z : 1.0 - 0.5 * z;

      T_partials_return d_ibeta
          = pow(1.0 - r, 0.5 * nu_dbl - 1) * pow(r, -0.5) / betaNuHalf;

      P += log(Pn);

      if (!is_constant_struct<T_y>::value)
        ops_partials.edge1_.partials_[n]
            -= zJacobian * d_ibeta * J * sigma_inv / Pn;

      if (!is_constant_struct<T_dof>::value) {
        T_partials_return g1 = 0;
        T_partials_return g2 = 0;

        grad_reg_inc_beta(g1, g2, (T_partials_return)0.5, 0.5 * nu_dbl, r,
                          digammaHalf, digammaNu_vec[n],
                          digammaNuPlusHalf_vec[n], betaNuHalf);

        ops_partials.edge2_.partials_[n]
            -= zJacobian * (-d_ibeta * (r / t) * (r / t) + 0.5 * g2) / Pn;
      }

      if (!is_constant_struct<T_loc>::value)
        ops_partials.edge3_.partials_[n]
            += zJacobian * d_ibeta * J * sigma_inv / Pn;
      if (!is_constant_struct<T_scale>::value)
        ops_partials.edge4_.partials_[n]
            += zJacobian * d_ibeta * J * sigma_inv * t / Pn;
    }
  }
  return ops_partials.build(P);
}

}  // namespace math
}  // namespace stan
#endif


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example std_copy.cpp \file std_copy.cpp
    \brief Fill interval containers using std::copy.

    Example std_copy shows how algorithm std::copy can be used to
    fill interval containers from other std::containers and how copying
    to interval containers differs from other uses of std::copy.

    \include std_copy_/std_copy.cpp
*/
//[example_std_copy
#include <iostream>
#include <vector>
#include <algorithm>
#include <boost/icl/interval_map.hpp>

using namespace std;
using namespace boost;
using namespace boost::icl;

// 'make_segments' returns a vector of interval value pairs, which
// are not sorted. The values are taken from the minimal example
// in section 'interval combining styles'.
vector<pair<discrete_interval<int>, int> > make_segments()
{
    vector<pair<discrete_interval<int>, int> > segment_vec;
    segment_vec.push_back(make_pair(discrete_interval<int>::right_open(2,4), 1));
    segment_vec.push_back(make_pair(discrete_interval<int>::right_open(4,5), 1));
    segment_vec.push_back(make_pair(discrete_interval<int>::right_open(1,3), 1));
    return segment_vec;
}

// 'show_segments' displays the source segements.
void show_segments(const vector<pair<discrete_interval<int>, int> >& segments)
{
    vector<pair<discrete_interval<int>, int> >::const_iterator iter = segments.begin();
    while(iter != segments.end())
    {
        cout << "(" << iter->first << "," << iter->second << ")";
        ++iter;
    }
}

void std_copy()
{
    // So we have some segments stored in an std container.
    vector<pair<discrete_interval<int>, int> > segments = make_segments();
    // Display the input
    cout << "input sequence: "; show_segments(segments); cout << "\n\n";

    // We are going to 'std::copy' those segments into an interval_map:
    interval_map<int,int> segmap;

    // Use an 'icl::inserter' from <boost/icl/iterator.hpp> to call
    // insertion on the interval container.
    std::copy(segments.begin(), segments.end(),
              icl::inserter(segmap, segmap.end()));
    cout << "icl::inserting: " << segmap << endl;
    segmap.clear();

    // When we are feeding data into interval_maps, most of the time we are
    // intending to compute an aggregation result. So we are not interested
    // the std::insert semantincs but the aggregating icl::addition semantics.
    // To achieve this there is an icl::add_iterator and an icl::adder function
    // provided in <boost/icl/iterator.hpp>.
    std::copy(segments.begin(), segments.end(),
              icl::adder(segmap, segmap.end())); //Aggregating associated values
    cout << "icl::adding   : " << segmap << endl;

    // In this last case, the semantics of 'std::copy' transforms to the
    // generalized addition operation, that is implemented by operator
    // += or + on itl maps and sets.
}

int main()
{
    cout << ">>    Interval Container Library: Example std_copy.cpp    <<\n";
    cout << "-----------------------------------------------------------\n";
    cout << "Using std::copy to fill an interval_map:\n\n";

    std_copy();
    return 0;
}

// Program output:
/*---------------------------------------------------------
>>    Interval Container Library: Example std_copy.cpp    <<
-----------------------------------------------------------
Using std::copy to fill an interval_map:

input sequence: ([2,4),1)([4,5),1)([1,3),1)

icl::inserting: {([1,5)->1)}
icl::adding   : {([1,2)->1)([2,3)->2)([3,5)->1)}
---------------------------------------------------------*/
//]


#include <boost\filesystem.hpp>
#include <boost\lexical_cast.hpp>
#include "genetic_programming.h"

int Print(fstream &output_file, string &program, int node)
{
	int node1 = 0, node2;

	if (program[node] < FSET_START) {
		if (program[node] <= num_vars)
			output_file << "x" << (int)program[node];
		else
			output_file << args[program[node] - 1];
		return node + 1;
	}
	else {
		switch (program[node]) {
			//case EXP:
			//	output_file << "exp(";
			//	node2 = Print(output_file, program, node + 1);
			//	output_file << ")";
			//	return node2;
			//case SIN:
			//	output_file << "sin(";
			//	node2 = Print(output_file, program, node + 1);
			//	output_file << ")";
			//	return node2;
			//case COS:
			//	output_file << "cos(";
			//	node2 = Print(output_file, program, node + 1);
			//	output_file << ")";
			//	return node2;
			//case LOG:
			//	output_file << "log(abs(";
			//	node2 = Print(output_file, program, node + 1);
			//	output_file << "))";
			//	return node2;
			case ADD:
				output_file << "(";
				node1 = Print(output_file, program, node + 1);
				output_file << " + ";
				break;
			case SUB:
				output_file << "(";
				node1 = Print(output_file, program, node + 1);
				output_file << " - ";
				break;
			case MUL:
				output_file << "(";
				node1 = Print(output_file, program, node + 1);
				output_file << " * ";
				break;
			case DIV:
				output_file << "(";
				node1 = Print(output_file, program, node + 1);
				output_file << " / ";
				break;
			//case POW:
			//	output_file << "(";
			//	node1 = Print(output_file, program, node + 1);
			//	output_file << " ^ ";
			//	break;
		}
	}

	node2 = Print(output_file, program, node1);
	output_file << ")";

	return node2;
}

void PrintInfixProgram(string &program, string &output_filename)
{
	fstream output_file(output_filename, ios::out);
	int node = 0;
	Print(output_file, program, node);
	output_file.close();
}

void SaveProgram(string &program, string filename)
{
	fstream args_file(filename + "_args.txt", ios::out), prog_file(filename + ".txt", ios::out);

	int i = 0;
	for (; i < args.size(); ++i)
		args_file << args[i] << "\n";

	for (i = 0; i < program.size(); ++i)
		prog_file << (int)program[i] << "\n";

	args_file.close();
	prog_file.close();

	PrintInfixProgram(program, filename + "_infix.txt");
}

void TestProgram(string &program, string output_filename)
{
	fstream output_file(output_filename + ".csv", ios::out);

	output_file << "x,y\n";

	for (int fit_case = 0; fit_case < input_data.size(); ++fit_case) {
		for (int var = 0; var < num_vars; ++var) {
			args[var] = input_data[fit_case][var];
			output_file << args[var] << ",";
		}

		current_node = program.begin();
		output_file << Next() << "\n";
	}

	output_file.close();
}

string LoadProgram(string &program_name)
{
	fstream program_file(program_name, ios::in);

	string program, line;

	while (true) {
		getline(program_file, line);
		if (program_file.eof())
			break;
		program += stoi(line);
	}

	program_file.close();

	return program;
}

int main() {
	boost::filesystem::path fullpath(boost::filesystem::current_path());
	string temp = boost::lexical_cast<string>(fullpath);
	string curr_dir = string(temp.begin() + 1, temp.end() - 1);

	SetUp(curr_dir + "\\config.ini");

	cout << "SYMBOLIC REGRESSION w/ GENETIC PROGRAMMING\n" << endl;

	cout << "Pseudo random number generator seed\t= " << seed << endl;
	cout << "Population size\t\t\t\t= " << popsize << endl;
	cout << "Number of generations\t\t\t= " << generations << endl;
	cout << "Max depth at the initialisation\t\t= " << max_depth << endl;
	cout << "Max program length\t\t\t= " << max_length << endl;
	cout << "Tournament size\t\t\t\t= " << tournament_size << endl;
	cout << "Mutation probability per node\t\t= " << pm_per_node << endl;
	cout << "Crossover probability\t\t\t= " << pcrossover << endl;
	cout << "Data filename\t\t\t\t= " << fitness_data_filename << endl;
	cout << "Number of independent variables\t\t= " << num_vars << endl;
	cout << "Number of random constants\t\t= " << num_rand_nums << endl;
	if (num_rand_nums) {
		cout << "Min constant\t\t\t\t= " << min_rand_num << endl;
		cout << "Max constant\t\t\t\t= " << max_rand_num << endl;
	}
	if (seed_w_program)
		cout << "Seed for the initial population\t\t= " << prog_name << endl;

	printf("\nPress any key to continue...\n");
	cin.get();

	vector<Individual> population(popsize);
	Individual best;

	if (seed_w_program) {
		best.program = LoadProgram(prog_name);
		Evaluate(best);
	}

	for (int i = 0; i < popsize; ++i) {
		population[i].program = BuildTree(random(2, max_depth), random(0, 1)); //Ramped half-and-half.
		Evaluate(population[i]);

		if (population[i] < best)
			best = population[i];
	}

	int epochs = generations * popsize, break_flag = 0;

	for (int epoch = 0; epoch < epochs; ++epoch) {
		Individual &offspring = population[Tournament(population, tournament_size, IsGreater<Individual>)]
			= population[Tournament(population, tournament_size, IsLess<Individual>)];

		if (random<float>() < pcrossover)
			SubtreeXO(offspring, population[Tournament(population, tournament_size, IsLess<Individual>)]);
		else
			PtMutation(offspring.program);

		Evaluate(offspring);

		if (offspring < best)
			best = offspring;

		if (((epoch + 1) % popsize) == 0)
			printf("Gen: %d, Program size: %d, Performance: %.4f\n", (1 + epoch / popsize), best.program.size(), best.fitness);

		if (best.fitness < 0.0001)
			break;

		if (break_flag = GetAsyncKeyState(VK_ESCAPE)) {
			char c = 0;

			printf("\n");

			while (c != 'Y' && c != 'y' && c != 'N' && c != 'n') {
				printf("\rDo you want quit the program? (Y/N): ");
				cin >> c;
			}

			if (c == 'Y' || c == 'y') {
				break;
			}
			else {
				break_flag = 0;
				printf("\n");
			}
		}
	}

	if (!break_flag)
		printf("\n");

	char c = 0;
	while (c != 'Y' && c != 'y' && c != 'N' && c != 'n') {
		printf("\rSave the best program? (Y/N): ");
		cin >> c;
	}

	if (c == 'Y' || c == 'y') {
		string filename;
		printf("Save as: ");
		cin >> filename;
		SaveProgram(best.program, filename);
	}

	c = 0;
	while (c != 'Y' && c != 'y' && c != 'N' && c != 'n') {
		printf("\rTest the best program? (Y/N): ");
		cin >> c;
	}

	if (c == 'Y' || c == 'y') {
		string filename;
		printf("Output file: ");
		cin >> filename;
		TestProgram(best.program, filename);
	}

	printf("\nPress any key to quit...\n");
	cin.get();
	cin.get();

	return 0;
}