#pragma once

#include <ros/ros.h>
#include "roboy_communication_middleware/InverseKinematics.h"
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <vector>
#include <boost/numeric/odeint.hpp>
#include "common_utilities/CommonDefinitions.h"
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <thread>
#include <mutex>
#include "common_utilities/rviz_visualization.hpp"

using namespace Eigen;
using namespace std;

class PaBiRoboyInverseKinematics:public rviz_visualization{
public:
    PaBiRoboyInverseKinematics();
    ~PaBiRoboyInverseKinematics(){};

    template<typename _Matrix_Type_>
    _Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
    {
        Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
        double tolerance = epsilon * std::max(a.cols(), a.rows()) *svd.singularValues().array().abs()(0);
        return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
    }

    bool inverseKinematics(roboy_communication_middleware::InverseKinematics::Request &req,
                               roboy_communication_middleware::InverseKinematics::Response &res);
     /**
      * Calculates the Jacobian of a lighthouse sensor
      * @param out 3x5 Matrix will be filled with the values
      * @param sensor for this sensor
      * @param ankle_left
      * @param ankle_right
      * @param theta0 knee_left angle
      * @param theta1 hip_left angle
      * @param theta2 hip_right angle
      * @param theta3 knee_right angle
      * @param phi angle of lower left leg wrt to inertial_frame.z
      */
    void Jacobian(double *out, int sensor, double ankle_left, double ankle_right, double theta0, double theta1, double theta2, double theta3, double phi);
    /**
     * forward kinematics of joints
     * @param out position
     * @param joint for this joint
     * @param ankle_x ankle_left position x
     * @param ankle_y ankle_left position y
     * @param theta0 knee_left angle
     * @param theta1 hip_left angle
     * @param theta2 hip_right angle
     * @param theta3 knee_right angle
     * @param phi angle of lower left leg wrt to inertial_frame.z
     */
    void joint_position(double *out, int joint, double ankle_x, double ankle_y, double theta0, double theta1, double theta2, double theta3, double phi);

    /**
     * forward kinematics of lighthouse sensors
     * @param out
     * @param sensor for this lighthouse sensor
     * @param ankle_x ankle_left position x
     * @param ankle_y ankle_left position y
     * @param theta0 knee_left angle
     * @param theta1 hip_left angle
     * @param theta2 hip_right angle
     * @param theta3 knee_right angle
     * @param phi angle of lower left leg wrt to inertial_frame.z
     */
    void lighthouse_sensor(double *out, int sensor, double ankle_x, double ankle_y, double phi, double theta0, double theta1, double theta2, double theta3);
    geometry_msgs::Vector3 lighthouse_sensor(int sensor, double ankle_x, double ankle_y, double phi, double theta0, double theta1, double theta2, double theta3);

    void visualize();

    enum JOINTS{
        ANKLE_LEFT = 0,
        KNEE_LEFT,
        HIP_LEFT,
        HIP_CENTER,
        HIP_RIGHT,
        KNEE_RIGHT,
        ANKLE_RIGHT,
        ANKLE_RIGHT_JACOBIAN = 9,
    };
private:
    ros::NodeHandlePtr nh;
    ros::ServiceServer ik_srv;
    const double l1 = 0.34;
    const double l2 = 0.40;
    const double l3 = 0.18;
    const double error_threshold = 0.005, max_number_iterations = 100;
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    map<string,Vector3d> initial_position, result_position;
};

int main(int argc, char* argv[]);


#include "polyscope/polyscope.h"

#include <igl/PI.h>
#include <igl/avg_edge_length.h>
#include <igl/barycenter.h>
#include <igl/boundary_loop.h>
#include <igl/exact_geodesic.h>
#include <igl/gaussian_curvature.h>
#include <igl/invert_diag.h>
#include <igl/lscm.h>
#include <igl/massmatrix.h>
#include <igl/per_vertex_normals.h>
#include <igl/readOBJ.h>
#include <igl/writeOBJ.h>
#include <igl/doublearea.h>
#include <igl/file_dialog_open.h>
#include <igl/file_dialog_save.h>
#include <igl/boundary_loop.h>
#include <igl/cotmatrix_entries.h>
#include <igl/triangle/triangulate.h>
#include <filesystem>
#include "polyscope/messages.h"
#include "polyscope/point_cloud.h"
#include "polyscope/surface_mesh.h"

#include <iostream>
#include <fstream>
#include <unordered_set>
#include <utility>
#include <Eigen/SparseQR>

#include "../../include/CommonTools.h"
#include "../../include/MeshLib/MeshConnectivity.h"
#include "../../include/MeshLib/MeshUpsampling.h"
#include "../../include/Visualization/PaintGeometry.h"
#include "../../include/InterpolationScheme/VecFieldSplit.h"
#include "../../include/Optimization/NewtonDescent.h"
#include "../../include/Optimization/LinearConstrainedSolver.h"
#include "../../include/IntrinsicFormula/InterpolateZvalsFromEdgeOmega.h"
#include "../../include/DynamicInterpolation/ComputeZandZdot.h"
#include "../../include/DynamicInterpolation/InterpolateKeyFrames.h"
//#include "../../include/IntrinsicFormula/ComputeZdotFromEdgeOmega.h"
#include "../../include/IntrinsicFormula/ComputeZdotFromHalfEdgeOmega.h"
//#include "../../include/IntrinsicFormula/IntrinsicKeyFrameInterpolationFromEdge.h"
#include "../../include/IntrinsicFormula/IntrinsicKeyFrameInterpolationFromHalfEdge.h"
#include "../../include/IntrinsicFormula/KnoppelStripePattern.h"

Eigen::MatrixXd triV, upsampledTriV;
Eigen::MatrixXi triF, upsampledTriF;
MeshConnectivity triMesh, upsampledTriMesh;
std::vector<std::pair<int, Eigen::Vector3d>> bary;

Eigen::MatrixXd sourceOmegaFields, tarOmegaFields;
Eigen::MatrixXd theoOmega, tarTheoOmega;

std::vector<std::complex<double>> sourceZvals, sourceTheoZVals, upsampledTheoZVals;
std::vector<std::complex<double>> tarZvals, tarTheoZVals, upsampledTarTheoZVals;
std::vector<Eigen::Vector2cd> sourceTheoGradZvals, tarTheoGradZvals;

std::vector<Eigen::MatrixXd> omegaList;
std::vector<Eigen::MatrixXd> theoOmegaList;
std::vector<std::vector<std::complex<double>>> zList;
std::vector<std::vector<std::complex<double>>> theoZList;

std::vector<Eigen::VectorXd> phaseFieldsList;
std::vector<Eigen::VectorXd> ampFieldsList;

std::vector<Eigen::VectorXd> theoPhaseFieldsList;
std::vector<Eigen::VectorXd> theoAmpFieldsList;

Eigen::MatrixXd dataV;
Eigen::MatrixXi dataF;
Eigen::MatrixXd dataVec;
Eigen::MatrixXd curColor;

int loopLevel = 1;
bool isFixedSource = true;
bool isFixedTar = true;

bool isForceOptimize = false;
bool isTwoTriangles = true;

PaintGeometry mPaint;

int numFrames = 2;
int curFrame = 0;

int singIndSource = 1, singIndSource1 = 1;
int singIndTar = 1, singIndTar1 = 1;
int numWavesSource = 2, numWaveTar = 4;

double globalAmpMax = 1;

double dragSpeed = 0.5;

double triarea = 0.04;

float vecratio = 0.1;

double sourceCenter1x = 0, sourceCenter1y = 0, sourceCenter2x = 0.8, sourceCenter2y = -0.2, targetCenter1x = 0, targetCenter1y = 0, targetCenter2x = 0.3, targetCenter2y = -0.7;
double sourceDirx = 1.0, sourceDiry = 0, targetDirx = 1, targetDiry = 0;

double gradTol = 1e-6;
double xTol = 0;
double fTol = 0;
int numIter = 1000;
int quadOrder = 4;

enum FunctionType {
	Whirlpool = 0,
	PlaneWave = 1,
	Summation = 2,
	YShape = 3,
	TwoWhirlPool = 4,
};

enum InitializationType{
  Random = 0,
  Linear = 1,
  Theoretical = 2
};

bool isUseUpMesh = false;

FunctionType functionType = FunctionType::PlaneWave;
FunctionType tarFunctionType = FunctionType::PlaneWave;
InitializationType initializationType = InitializationType::Linear;

void generateSquare(double length, double width, double triarea, Eigen::MatrixXd& irregularV, Eigen::MatrixXi& irregularF)
{
	double area = length * width;
	int N = (0.25 * std::sqrt(area / triarea));
	N = N > 1 ? N : 1;
	double deltaX = length / (4.0 * N);
	double deltaY = width / (4.0 * N);

	Eigen::MatrixXd planeV;
	Eigen::MatrixXi planeE;

//	planeV.resize(10, 2);
//	planeE.resize(10, 2);

//	for (int i = -2; i <= 2; i++)
//	{
//		planeV.row(i + 2) << length / 4.0 * i, -width / 2.0;
//	}
//
//	for (int i = 2; i >= -2; i--)
//	{
//		planeV.row(5 + 2 - i) << length / 4.0 * i, width / 2.0;
//	}
//
//	for (int i = 0; i < 10; i++)
//	{
//		planeE.row(i) << i, (i + 1) % 10;
//	}

	int M = 2 * N + 1;
	planeV.resize(4 * M - 4, 2);
	planeE.resize(4 * M - 4, 2);

	for (int i = 0; i < M; i++)
	{
	    planeV.row(i) << -length / 2, i * width / (M - 1) - width / 2;
	}
	for (int i = 1; i < M; i++)
	{
	    planeV.row(M - 1 + i) << i * length / (M - 1)-length / 2, width / 2;
	}
	for (int i = 1; i < M; i++)
	{
	    planeV.row(2 * (M - 1) + i) << length / 2, width/2 - i * width / (M - 1);
	}
	for (int i = 1; i < M - 1; i++)
	{
	    planeV.row(3 * (M - 1) + i) << length / 2- i * length / (M - 1), - width / 2;
	}

	for (int i = 0; i < 4 * (M - 1); i++)
	{
	    planeE.row(i) << i, (i + 1) % (4 * (M - 1));
	}

	Eigen::MatrixXd V2d;
	Eigen::MatrixXi F;
	Eigen::MatrixXi H(0, 2);
	std::cout << triarea << std::endl;
	// Create an output string stream
	std::ostringstream streamObj;
	//Add double to stream
	streamObj << triarea;
	const std::string flags = "q20a" + std::to_string(triarea);

	igl::triangle::triangulate(planeV, planeE, H, flags, V2d, F);

	if (isTwoTriangles)
	{
		/*V2d.resize(4, 3);
		V2d << -1, -1, 0,
			1, -1, 0,
			1, 1, 0,
			-1, 1, 0;

		F.resize(2, 3);
		F << 0, 1, 2,
			2, 3, 0;*/

		V2d.resize(3, 3);
		V2d << 0, 0, 0,
			1, 0, 0,
			0, 1, 0;

		F.resize(1, 3);
		F << 0, 1, 2;
	}

	irregularV.resize(V2d.rows(), 3);
	irregularV.setZero();
	irregularV.block(0, 0, irregularV.rows(), 2) = V2d.block(0, 0, irregularV.rows(), 2);
	irregularF = F;
	igl::writeOBJ("irregularPlane.obj", irregularV, irregularF);
}

void generateWhirlPool(double centerx, double centery, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, int pow = 1, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
	z.resize(triV.rows());
	w.resize(triV.rows(), 2);
	std::cout << "whirl pool center: " << centerx << ", " << centery << std::endl;
	bool isnegative = false;
	if(pow < 0)
	{
	    isnegative = true;
	    pow *= -1;
	}

	for (int i = 0; i < z.size(); i++)
	{
		double x = triV(i, 0) - centerx;
		double y = triV(i, 1) - centery;
		double rsquare = x * x + y * y;

		if(isnegative)
		{
		    z[i] = std::pow(std::complex<double>(x, -y), pow);

		    if (std::abs(std::sqrt(rsquare)) < 1e-10)
		        w.row(i) << 0, 0;
		    else
		        w.row(i) << pow * y / rsquare, -pow * x / rsquare;
		}
		else
		{
		    z[i] = std::pow(std::complex<double>(x, y), pow);

		    if (std::abs(std::sqrt(rsquare)) < 1e-10)
		        w.row(i) << 0, 0;
		    else
		        //			w.row(i) << -y / rsquare, x / rsquare;
		        w.row(i) << -pow * y / rsquare, pow * x / rsquare;
		}
	}

	if(upsampledZ)
	{
	    upsampledZ->resize(upsampledTriV.rows());
	    for(int i = 0; i < upsampledZ->size(); i++)
	    {
	        double x = upsampledTriV(i, 0) - centerx;
	        double y = upsampledTriV(i, 1) - centery;
	        double rsquare = x * x + y * y;

			upsampledZ->at(i) = std::pow(std::complex<double>(x, y), pow);
			if(isnegative)
			    upsampledZ->at(i) = std::pow(std::complex<double>(x, -y), pow);
	    }
	}
	if(gradZ)
	{
	    gradZ->resize(triV.rows());
	    for(int i = 0; i < gradZ->size(); i++)
	    {
	        double x = upsampledTriV(i, 0) - centerx;
	        double y = upsampledTriV(i, 1) - centery;

	        Eigen::Vector2cd tmpGrad;
	        tmpGrad << 1, std::complex<double>(0, 1);
	        if(isnegative)
	            tmpGrad(1) *= -1;

	        (*gradZ)[i] = std::pow(std::complex<double>(x, y), pow - 1) * tmpGrad;
	        (*gradZ)[i] *= pow;

	    }

	}
}

void generatePlaneWave(Eigen::Vector2d v, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
    z.resize(triV.rows());
    w.resize(triV.rows(), 2);
    std::cout << "plane wave direction: " << v.transpose() << std::endl;

    for (int i = 0; i < z.size(); i++)
    {
        double theta = v.dot(triV.row(i).segment<2>(0));
        double x = std::cos(theta);
        double y = std::sin(theta);
        z[i] = std::complex<double>(x, y);
        w.row(i) = v;
    }

    if(upsampledZ)
    {
        upsampledZ->resize(upsampledTriV.rows());
        for(int i = 0; i < upsampledZ->size(); i++)
        {
            double theta = v.dot(upsampledTriV.row(i).segment<2>(0));
            double x = std::cos(theta);
            double y = std::sin(theta);
            upsampledZ->at(i) = std::complex<double>(x, y);
        }
    }
    if(gradZ)
    {
        gradZ->resize(triV.rows());
        for(int i = 0; i < gradZ->size(); i++)
        {
            double theta = v.dot(triV.row(i).segment<2>(0));
            double x = std::cos(theta);
            double y = std::sin(theta);
            std::complex<double> tmpZ = std::complex<double>(x, y);
            std::complex<double> I = std::complex<double>(0, 1);

            (*gradZ)[i] << I * tmpZ * v(0), I * tmpZ * v(1);
        }

    }
}

void generatePeriodicWave(int waveNum, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
    Eigen::Vector2d v(2 * M_PI * waveNum, 0);
    generatePlaneWave(v, w, z, gradZ, upsampledZ);
}

void generateTwoWhirlPool(double centerx0, double centery0, double centerx1, double centery1, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, int n0 = 1, int n1 = 1, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>>* upsampledZ = NULL)
{
	Eigen::MatrixXd w0, w1;
	std::vector<Eigen::Vector2cd> gradZ0, gradZ1;
	std::vector<std::complex<double>> z0, z1, upsampledZ0, upsampledZ1;

	generateWhirlPool(centerx0, centery0, w0, z0, n0, gradZ ? &gradZ0 : NULL, upsampledZ ? &upsampledZ0 : NULL);
	generateWhirlPool(centerx1, centery1, w1, z1, n1, gradZ ? &gradZ1 : NULL, upsampledZ ? &upsampledZ1 : NULL);

	std::cout << "whirl pool center: " << centerx0 << ", " << centery0 << std::endl;
	std::cout << "whirl pool center: " << centerx1 << ", " << centery1 << std::endl;

	z.resize(triV.rows());
	w.resize(triV.rows(), 2);

	w = w0 + w1;

	for (int i = 0; i < z.size(); i++)
	{
		z[i] = z0[i] * z1[i];
	}

	if (upsampledZ)
	{
		upsampledZ->resize(upsampledTriV.rows());
		for (int i = 0; i < upsampledZ->size(); i++)
		{
			upsampledZ->at(i) = upsampledZ0[i] * upsampledZ1[i];
		}
	}

	if(gradZ)
	{
	    *gradZ = gradZ0;
	    for(int i = 0; i < gradZ0.size(); i++)
	    {
	        (*gradZ)[i] = z0[i] * gradZ1[i] + z1[i] * gradZ0[i];
	    }
	}
}

void generatePlaneSumWhirl(double centerx, double centery, Eigen::Vector2d v, Eigen::MatrixXd& w, std::vector<std::complex<double>>& z, int pow = 1, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
    z.resize(triV.rows());
    w.resize(triV.rows(), 2);
    std::cout << "whirl pool center: " << centerx << ", " << centery << std::endl;
    std::cout << "plane wave direction: " << v.transpose() << std::endl;

    std::vector<Eigen::Vector2cd> gradWZ, gradPZ;
    Eigen::MatrixXd whw, plw;
    std::vector<std::complex<double>> wz, pz, upWz, upPz;

    generatePlaneWave(v, plw, pz, gradZ ? &gradPZ : NULL, upsampledZ ? &upPz : NULL);
    generateWhirlPool(centerx, centery, whw, wz, pow, gradZ? &gradWZ : NULL, upsampledZ? &upWz : NULL);

    for (int i = 0; i < z.size(); i++)
    {
        z[i] = pz[i] * wz[i];
        w = plw + whw;
    }

    if(upsampledZ)
    {
        upsampledZ->resize(upsampledTriV.rows());

        for(int i = 0; i < upsampledZ->size(); i++)
        {
            (*upsampledZ)[i] = upPz[i] * upWz[i];
        }
    }

    if(gradZ)
    {
        *gradZ = gradWZ;
        for(int i = 0; i < gradWZ.size(); i++)
        {
            (*gradZ)[i] = pz[i] * gradWZ[i] + wz[i] * gradPZ[i];
        }
    }

}

void generateYshape(Eigen::Vector2d w1, Eigen::Vector2d w2, Eigen::MatrixXd &w, std::vector<std::complex<double>> &z, std::vector<Eigen::Vector2cd> *gradZ = NULL, std::vector<std::complex<double>> *upsampledZ = NULL)
{
    z.resize(triV.rows());
    w.resize(triV.rows(), 2);

    if(gradZ)
        gradZ->resize(triV.rows());

    std::cout << "w1: " << w1.transpose() << std::endl;
    std::cout << "w2: " << w2.transpose() << std::endl;

    Eigen::MatrixXd pw1, pw2;
    std::vector<std::complex<double>> pz1, pz2;
    std::vector<Eigen::Vector2cd> gradPZ1, gradPZ2;
    std::vector<std::complex<double>> upsampledPZ1, upsampledPZ2;

    generatePlaneWave(w1, pw1, pz1, gradZ ? & gradPZ1 : NULL, upsampledZ? &upsampledPZ1 : NULL);
    generatePlaneWave(w2, pw2, pz2, gradZ ? & gradPZ2 : NULL, upsampledZ? &upsampledPZ2 : NULL);

    double ymax = triV.col(1).maxCoeff();
    double ymin = triV.col(1).minCoeff();

    for (int i = 0; i < z.size(); i++)
    {

        double weight = (triV(i, 1) - triV.col(1).minCoeff()) / (triV.col(1).maxCoeff() - triV.col(1).minCoeff());
        z[i] = (1 - weight) * pz1[i] + weight * pz2[i];
        Eigen::Vector2cd dz = (1 - weight) * gradPZ1[i] + weight * gradPZ2[i];
        if(gradZ)
            (*gradZ)[i] = dz;

        double wx = 0;
        double wy = 1 / (ymax - ymin);

        w.row(i) = (std::conj(z[i]) * dz).imag() / (std::abs(z[i]) * std::abs(z[i]));
    }

    if(upsampledZ)
    {
        upsampledZ->resize(upsampledTriV.rows());
        for(int i = 0; i < upsampledZ->size(); i++)
        {
            double theta = w1.dot(upsampledTriV.row(i).segment<2>(0));
            double x = std::cos(theta);
            double y = std::sin(theta);
            std::complex<double> z1 = std::complex<double>(x, y);

            theta = w2.dot(upsampledTriV.row(i).segment<2>(0));
            x = std::cos(theta);
            y = std::sin(theta);
            std::complex<double> z2 = std::complex<double>(x, y);

            double weight = (upsampledTriV(i, 1) - upsampledTriV.col(1).minCoeff()) / (upsampledTriV.col(1).maxCoeff() - upsampledTriV.col(1).minCoeff());
            upsampledZ->at(i) = (1 - weight) * z1 + weight * z2;
        }
    }
}

void initialization()
{
	generateSquare(2.0, 2.0, triarea, triV, triF);

	Eigen::SparseMatrix<double> S;
	std::vector<int> facemap;

	meshUpSampling(triV, triF, upsampledTriV, upsampledTriF, loopLevel, &S, &facemap, &bary);
	std::cout << "upsampling finished" << std::endl;

	triMesh = MeshConnectivity(triF);
	upsampledTriMesh = MeshConnectivity(upsampledTriF);

    std::cout << "nverts: " << triV.rows() << ", nedges: " << triMesh.nEdges() << std::endl;
}

void generateValues(FunctionType funType, Eigen::MatrixXd &vecFields, std::vector<std::complex<double>> &zvalues, std::vector<Eigen::Vector2cd> &gradZvals, std::vector<std::complex<double>> &upZvals, int singularityInd1 = 1, int singularityInd2 = 1, bool isFixedGenerator = false, double fixedx = 0, double fixedy = 0, Eigen::Vector2d fixedv = Eigen::Vector2d::Constant(1.0))
{
	Eigen::MatrixXd vertFields;
	if (funType == FunctionType::Whirlpool)
	{
		Eigen::Vector2d center = Eigen::Vector2d::Random();
		if (isFixedGenerator)
			center << fixedx, fixedy;
		generateWhirlPool(center(0), center(1), vertFields, zvalues, singularityInd1, &gradZvals, &upZvals);
	}
	else if (funType == FunctionType::PlaneWave)
	{
		Eigen::Vector2d v = Eigen::Vector2d::Random();
		if (isFixedGenerator)
			v = fixedv;
		generatePlaneWave(v, vertFields, zvalues, &gradZvals, &upZvals);
	}
	else if (funType == FunctionType::Summation)
	{
		Eigen::Vector2d center = Eigen::Vector2d::Random();
		Eigen::Vector2d v = Eigen::Vector2d::Random();
		if (isFixedGenerator)
		{
			v = fixedv;
			center << fixedx, fixedy;
		}
		generatePlaneSumWhirl(center(0), center(1), v, vertFields, zvalues, singularityInd1, &gradZvals, &upZvals);
	}
	else if (funType == FunctionType::YShape)
	{
		Eigen::Vector2d w1(1, 0);
		Eigen::Vector2d w2(1, 0);

		w1(0) = 2 * 3.1415926;
		w2(0) = 4 * 3.1415926;
		generateYshape(w1, w2, vertFields, zvalues, &gradZvals, &upZvals);
	}
	else if (funType == FunctionType::TwoWhirlPool)
	{
		Eigen::Vector2d center0 = Eigen::Vector2d::Random();
		Eigen::Vector2d center1 = Eigen::Vector2d::Random();
		if (isFixedGenerator)
		{
			center0 << fixedx, fixedy;
			center1 << 0.8, -0.3;
		}
		generateTwoWhirlPool(center0(0), center0(1), center1(0), center1(1), vertFields, zvalues, singularityInd1, singularityInd2, &gradZvals, &upZvals);
	}
	Eigen::MatrixXd vertFields3D(triV.rows(), 3);
	vertFields3D.block(0, 0, triV.rows(), 2) = vertFields;
	vertFields3D.col(2).setZero();

	vecFields = vertexVec2IntrinsicHalfEdgeVec(vertFields3D, triV, MeshConnectivity(triF));
}

// TODO: remove this
Eigen::MatrixXd vertexVec2IntrinsicHalfEdgeVec2D(const Eigen::MatrixXd& v, const Eigen::MatrixXd& pos, const MeshConnectivity& mesh, std::vector<Eigen::Triplet<double>> &T)
{
    int nedges = mesh.nEdges();
    Eigen::MatrixXd edgeOmega(nedges, 2);
    T.clear();

    for (int i = 0; i < nedges; i++)
    {
        int vid0 = mesh.edgeVertex(i, 0);
        int vid1 = mesh.edgeVertex(i, 1);

        Eigen::Vector2d e = (pos.row(vid1) - pos.row(vid0)).segment(0, 2);
        edgeOmega(i, 0) = v.row(vid0).dot(e);
        edgeOmega(i, 1) = -v.row(vid1).dot(e);

        T.push_back({2 * i, 2 * vid0, e(0)});
        T.push_back({2 * i, 2 * vid0 + 1, e(1)});

        T.push_back({2 * i + 1, 2 * vid1, -e(0)});
        T.push_back({2 * i + 1, 2 * vid1 + 1, -e(1)});
    }
    return edgeOmega;
}

void solveKeyFrames(const Eigen::MatrixXd& sourceVec, const Eigen::MatrixXd& tarVec, const std::vector<std::complex<double>>& sourceZvals, const std::vector<std::complex<double>>& tarZvals, const int numKeyFrames, std::vector<Eigen::MatrixXd>& wFrames, std::vector<std::vector<std::complex<double>>>& zFrames)
{
	Eigen::VectorXd faceArea;
	igl::doublearea(triV, triF, faceArea);
	faceArea /= 2;
	IntrinsicFormula::IntrinsicKeyFrameInterpolationFromHalfEdge interpModel = IntrinsicFormula::IntrinsicKeyFrameInterpolationFromHalfEdge(MeshConnectivity(triF), faceArea, numFrames, quadOrder, sourceZvals, sourceVec, tarZvals, tarVec);
	Eigen::VectorXd x;
	interpModel.convertList2Variable(x);        // linear initialization

    std::vector<std::complex<double>> testzvals;
    Eigen::MatrixXd cotEntries;
    igl::cotmatrix_entries(triV, triF, cotEntries);
    IntrinsicFormula::roundVertexZvalsFromHalfEdgeOmega(triMesh, sourceVec, faceArea, cotEntries, triV.rows(), testzvals);
    IntrinsicFormula::testRoundingEnergy(triMesh, sourceVec, faceArea, cotEntries, triV.rows(), testzvals);
	for (auto& z : testzvals)
	{
		Eigen::Vector2d rndvec;
		rndvec.setRandom();
		z = std::complex<double>(rndvec(0), rndvec(1));
	}
	IntrinsicFormula::testRoundingEnergy(triMesh, sourceVec, faceArea, cotEntries, triV.rows(), testzvals);

	//interpModel.testEnergy(x);
	//		std::cout << "starting energy: " << interpModel.computeEnergy(x) << std::endl;
	if (initializationType == InitializationType::Theoretical)
	{
		interpModel.setwzLists(theoZList, theoOmegaList);
		interpModel.convertList2Variable(x);
	}
	else if (initializationType == InitializationType::Random)
	{
		x.setRandom();
		interpModel.convertVariable2List(x);
		interpModel.convertList2Variable(x);
	}
	else
	{
		// do nothing, since it is initialized as the linear interpolation.
	}

	auto initWFrames = interpModel.getWList();
	auto initZFrames = interpModel.getVertValsList();
	if (isForceOptimize)
	{
		auto funVal = [&](const Eigen::VectorXd& x, Eigen::VectorXd* grad, Eigen::SparseMatrix<double>* hess, bool isProj) {
			Eigen::VectorXd deriv;
			Eigen::SparseMatrix<double> H;
			double E = interpModel.computeEnergy(x, grad ? &deriv : NULL, hess ? &H : NULL, isProj);

			if (grad)
			{
				(*grad) = deriv;
			}

			if (hess)
			{
				(*hess) = H;
			}

			return E;
		};
		auto maxStep = [&](const Eigen::VectorXd& x, const Eigen::VectorXd& dir) {
			return 1.0;
		};

		auto getVecNorm = [&](const Eigen::VectorXd& x, double& znorm, double& wnorm) {
			interpModel.getComponentNorm(x, znorm, wnorm);
		};

		OptSolver::testFuncGradHessian(funVal, x);

		auto x0 = x;
		OptSolver::newtonSolver(funVal, maxStep, x, numIter, gradTol, xTol, fTol, true, getVecNorm);
		std::cout << "before optimization: " << x0.norm() << ", after optimization: " << x.norm() << ", difference: " << (x - x0).norm() << std::endl;
		std::cout << "x norm: " << x.norm() << std::endl;

		/*Eigen::VectorXd deriv;
		Eigen::SparseMatrix<double> H;
		double E = interpModel.computeEnergy(x, &deriv, &H, false);

		std::cout << deriv.norm() << std::endl;*/
		//std::cout << "hessian: \n" << H.toDense() << std::endl;

		/*Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
		es.compute(H.toDense());
		Eigen::VectorXd evals = es.eigenvalues();
		std::cout << "evals: " << evals.transpose() << std::endl;*/

	}
	interpModel.convertVariable2List(x);

	wFrames = interpModel.getWList();
	zFrames = interpModel.getVertValsList();

	for (int i = 0; i < wFrames.size() - 1; i++)
	{
		double zdotNorm = interpModel._zdotModel.computeZdotIntegration(zFrames[i], wFrames[i], zFrames[i + 1], wFrames[i + 1], NULL, NULL);

		double initZdotNorm = interpModel._zdotModel.computeZdotIntegration(initZFrames[i], initWFrames[i], initZFrames[i + 1], initWFrames[i + 1], NULL, NULL);

		std::cout << "frame " << i << ", before optimization: ||zdot||^2: " << initZdotNorm << ", after optimization, ||zdot||^2 = " << zdotNorm << std::endl;
	}

}

void updateMagnitudePhase(const std::vector<Eigen::MatrixXd>& wFrames, const std::vector<std::vector<std::complex<double>>>& zFrames, std::vector<Eigen::VectorXd>& magList, std::vector<Eigen::VectorXd>& phaseList)
{
	std::vector<std::vector<std::complex<double>>> interpZList(wFrames.size());
	magList.resize(wFrames.size());
	phaseList.resize(wFrames.size());

	MeshConnectivity mesh(triF);

	auto computeMagPhase = [&](const tbb::blocked_range<uint32_t>& range) {
		for (uint32_t i = range.begin(); i < range.end(); ++i)
		{
			interpZList[i] = IntrinsicFormula::upsamplingZvals(mesh, zFrames[i], wFrames[i], bary);
			magList[i].setZero(interpZList[i].size());
			phaseList[i].setZero(interpZList[i].size());

			for (int j = 0; j < magList[i].size(); j++)
			{
				magList[i](j) = std::abs(interpZList[i][j]);
				phaseList[i](j) = std::arg(interpZList[i][j]);
			}
		}
	};

	tbb::blocked_range<uint32_t> rangex(0u, (uint32_t)interpZList.size(), GRAIN_SIZE);
	tbb::parallel_for(rangex, computeMagPhase);
}

void updateTheoMagnitudePhase(const std::vector<std::complex<double>>& sourceZvals, const std::vector<std::complex<double>>& tarZvals,
                              const std::vector<Eigen::Vector2cd>& sourceGradZvals, const std::vector<Eigen::Vector2cd>& tarGradZvals,
                              const std::vector<std::complex<double>>& upSourceZvals, const std::vector<std::complex<double>>& upTarZvals,
                              const int num, std::vector<Eigen::VectorXd>& magList, std::vector<Eigen::VectorXd>& phaseList,    // upsampled information
                              std::vector<Eigen::MatrixXd>& wList, std::vector<std::vector<std::complex<double>>> &zvalList        // raw information
                              )
{
	magList.resize(num + 2);
	phaseList.resize(num + 2);
	wList.resize(num + 2);
	zvalList.resize(num + 2);

	double dt = 1.0 / (num + 1);

	auto computeMagPhase = [&](const tbb::blocked_range<uint32_t>& range) {
		for (uint32_t i = range.begin(); i < range.end(); ++i)
		{
			double w = i * dt;
			magList[i].setZero(upSourceZvals.size());
			phaseList[i].setZero(upSourceZvals.size());
			for (int j = 0; j < upSourceZvals.size(); j++)
			{
			    std::complex<double> z = (1 - w) * upSourceZvals[j] + w * upTarZvals[j];
			    magList[i][j] = std::abs(z);
			    phaseList[i][j] = std::arg(z);
			}
			Eigen::MatrixXd vertOmega = Eigen::MatrixXd::Zero(sourceZvals.size(), 3);
			zvalList[i].resize(sourceZvals.size());

			for(int j = 0; j < sourceZvals.size(); j++)
			{
			    Eigen::Vector2cd gradf = (1 - w) * sourceGradZvals[j] + w * tarGradZvals[j];
			    std::complex<double> fbar = (1 - w) * std::conj(sourceZvals[j]) + w * std::conj(tarZvals[j]);

				vertOmega.row(j).segment<2>(0) = ((gradf * fbar) / (std::abs(fbar) * std::abs(fbar))).imag();
			    zvalList[i][j] = (1 - w) * sourceZvals[j] + w * tarZvals[j];
			}
			wList[i] = vertexVec2IntrinsicHalfEdgeVec(vertOmega, triV, triMesh);
		}
	};

	tbb::blocked_range<uint32_t> rangex(0u, (uint32_t)(num + 2), GRAIN_SIZE);
	tbb::parallel_for(rangex, computeMagPhase);
}

void registerMeshByPart(const Eigen::MatrixXd& basePos, const Eigen::MatrixXi& baseF,
	const Eigen::MatrixXd& upPos, const Eigen::MatrixXi& upF, const double& shifty, const double& ampMax,
	Eigen::VectorXd ampVec, const Eigen::VectorXd& phaseVec,
	Eigen::VectorXd theoAmpVec, const Eigen::VectorXd& theoPhaseVec,
	Eigen::MatrixXd& renderV, Eigen::MatrixXi& renderF, Eigen::MatrixXd& renderColor)
{
	int nverts = basePos.rows();
	int nfaces = baseF.rows();

	int nupverts = upPos.rows();
	int nupfaces = upF.rows();

	int ndataVerts = nverts + 4 * nupverts;
	int ndataFaces = nfaces + 4 * nupfaces;

	renderV.resize(ndataVerts, 3);
	renderF.resize(ndataFaces, 3);
	renderColor.setZero(ndataVerts, 3);

	renderColor.col(0).setConstant(1.0);
	renderColor.col(1).setConstant(1.0);
	renderColor.col(2).setConstant(1.0);

	int curVerts = 0;
	int curFaces = 0;

	Eigen::VectorXd normalizedAmp = ampVec / ampMax, normalizedTheoAmp = theoAmpVec / ampMax;

	Eigen::MatrixXd shiftV = basePos;
	shiftV.col(0).setConstant(0);
	shiftV.col(1).setConstant(shifty);
	shiftV.col(2).setConstant(0);

	renderV.block(0, 0, nverts, 3) = basePos - shiftV;
	renderF.block(0, 0, nfaces, 3) = baseF;
	curVerts += nverts;
	curFaces += nfaces;

	double shiftx = 1.5 * (basePos.col(0).maxCoeff() - basePos.col(0).minCoeff());

	shiftV = upPos;
	shiftV.col(0).setConstant(shiftx);
	shiftV.col(1).setConstant(shifty);
	shiftV.col(2).setConstant(0);

	Eigen::MatrixXi shiftF = upF;
	shiftF.setConstant(curVerts);

	// interpolated phase
	renderV.block(curVerts, 0, nupverts, 3) = upPos - shiftV;
	renderF.block(curFaces, 0, nupfaces, 3) = upF + shiftF;

	mPaint.setNormalization(false);
	Eigen::MatrixXd phiColor = mPaint.paintPhi(phaseVec);
	renderColor.block(curVerts, 0, nupverts, 3) = phiColor;

	curVerts += nupverts;
	curFaces += nupfaces;

	// interpolated amp
	shiftF.setConstant(curVerts);
	shiftV.col(0).setConstant(2 * shiftx);
	renderV.block(curVerts, 0, nupverts, 3) = upPos - shiftV;
	renderF.block(curFaces, 0, nupfaces, 3) = upF + shiftF;

	mPaint.setNormalization(false);
	Eigen::MatrixXd ampColor = mPaint.paintAmplitude(ampVec / globalAmpMax);
	renderColor.block(curVerts, 0, nupverts, 3) = ampColor;

	curVerts += nupverts;
	curFaces += nupfaces;

	// theoretical phase
	shiftF.setConstant(curVerts);
	shiftV.col(0).setConstant(3 * shiftx);
	renderV.block(curVerts, 0, nupverts, 3) = upPos - shiftV;
	renderF.block(curFaces, 0, nupfaces, 3) = upF + shiftF;

	mPaint.setNormalization(false);
	phiColor = mPaint.paintPhi(theoPhaseVec);
	renderColor.block(curVerts, 0, nupverts, 3) = phiColor;

	curVerts += nupverts;
	curFaces += nupfaces;

	// theoretical amp
	shiftF.setConstant(curVerts);
	shiftV.col(0).setConstant(4 * shiftx);
	renderV.block(curVerts, 0, nupverts, 3) = upPos - shiftV;
	renderF.block(curFaces, 0, nupfaces, 3) = upF + shiftF;

	mPaint.setNormalization(false);
	ampColor = mPaint.paintAmplitude(theoAmpVec / globalAmpMax);
	renderColor.block(curVerts, 0, nupverts, 3) = ampColor;

}

void registerMesh(int frameId)
{
	Eigen::MatrixXd sourceP, tarP, interpP;
	Eigen::MatrixXi sourceF, tarF, interpF;
	Eigen::MatrixXd sourceColor, tarColor, interpColor;

	double shiftx = 1.5 * (triV.col(0).maxCoeff() - triV.col(0).minCoeff());
	double shifty = 1.5 * (triV.col(1).maxCoeff() - triV.col(1).minCoeff());
	int totalfames = ampFieldsList.size();
	registerMeshByPart(triV, triF, upsampledTriV, upsampledTriF, 0, globalAmpMax, ampFieldsList[0], phaseFieldsList[0], theoAmpFieldsList[0], theoPhaseFieldsList[0], sourceP, sourceF, sourceColor);
	registerMeshByPart(triV, triF, upsampledTriV, upsampledTriF, shifty, globalAmpMax, ampFieldsList[totalfames - 1], phaseFieldsList[totalfames - 1], theoAmpFieldsList[totalfames - 1], theoPhaseFieldsList[totalfames - 1], tarP, tarF, tarColor);
	registerMeshByPart(triV, triF, upsampledTriV, upsampledTriF, 2 * shifty, globalAmpMax, ampFieldsList[frameId], phaseFieldsList[frameId], theoAmpFieldsList[frameId], theoPhaseFieldsList[frameId], interpP, interpF, interpColor);

	Eigen::MatrixXi shifF = sourceF;

	int nPartVerts = sourceP.rows();
	int nPartFaces = sourceF.rows();
	int nverts = triV.rows();
	int nfaces = triF.rows();

	dataV.setZero(3 * nPartVerts + nverts, 3);
	curColor.setZero(3 * nPartVerts + nverts, 3);
	dataF.setZero(3 * nPartFaces + nfaces, 3);

	shifF.setConstant(nPartVerts);

	dataV.block(0, 0, nPartVerts, 3) = sourceP;
	curColor.block(0, 0, nPartVerts, 3) = sourceColor;
	dataF.block(0, 0, nPartFaces, 3) = sourceF;

	dataV.block(nPartVerts, 0, nPartVerts, 3) = tarP;
	curColor.block(nPartVerts, 0, nPartVerts, 3) = tarColor;
	dataF.block(nPartFaces, 0, nPartFaces, 3) = tarF + shifF;

	dataV.block(nPartVerts * 2, 0, nPartVerts, 3) = interpP;
	curColor.block(nPartVerts * 2, 0, nPartVerts, 3) = interpColor;
	dataF.block(nPartFaces * 2, 0, nPartFaces, 3) = interpF + 2 * shifF;

	Eigen::MatrixXd shiftV = triV;
	shiftV.col(0).setConstant(shiftx);
	shiftV.col(1).setConstant(-2 * shifty);
	shiftV.col(2).setZero();

	shifF = triF;
	shifF.setConstant(3 * nPartVerts);

	dataV.block(nPartVerts * 3, 0, nverts, 3) = triV + shiftV;
	curColor.block(nPartVerts * 3, 0, nverts, 3).setConstant(1.0);
	dataF.block(nPartFaces * 3, 0, nfaces, 3) = triF + shifF;

	polyscope::registerSurfaceMesh("input mesh", dataV, dataF);

}

void updateFieldsInView(int frameId)
{
	registerMesh(frameId);
	polyscope::getSurfaceMesh("input mesh")->addVertexColorQuantity("VertexColor", curColor);
	polyscope::getSurfaceMesh("input mesh")->getQuantity("VertexColor")->setEnabled(true);

	/*polyscope::getSurfaceMesh("input mesh")->addVertexVectorQuantity("vertex vector field", dataVec * vecratio, polyscope::VectorType::AMBIENT);
	polyscope::getSurfaceMesh("input mesh")->getQuantity("vertex vector field")->setEnabled(true);*/
}

void callback() {
	ImGui::PushItemWidth(100);
	if (ImGui::Button("Reset", ImVec2(-1, 0)))
	{
		curFrame = 0;
		updateFieldsInView(curFrame);
	}

	if (ImGui::InputDouble("triangle area", &triarea))
	{
		if (triarea > 0)
			initialization();
	}
	if (ImGui::Checkbox("Two Triangle Mesh", &isTwoTriangles))
	{
		initialization();
	}
	if (ImGui::InputInt("upsampled times", &loopLevel))
	{
		if (loopLevel >= 0)
			initialization();
	}

	if (ImGui::CollapsingHeader("source Vector Fields Info", ImGuiTreeNodeFlags_DefaultOpen))
	{
		if (ImGui::Combo("source vec types", (int*)&functionType, "Whirl pool\0plane wave\0sum\0Y shape\0Two Whirl Pool\0Periodic\0\0")) {}
		if (ImGui::Checkbox("Fixed source center and dir", &isFixedSource)) {}

		if (ImGui::CollapsingHeader("source whirl  pool Info"))
		{
		    if (ImGui::InputInt("source singularity index 1", &singIndSource)){}
		    if (ImGui::InputDouble("source center 1 x: ", &sourceCenter1x)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("source center 1 y: ", &sourceCenter1y)) {}

		    if (ImGui::InputInt("source singularity index 2", &singIndSource1)){}
		    if (ImGui::InputDouble("source center 2 x: ", &sourceCenter2x)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("source center 2 y: ", &sourceCenter2y)) {}
		}

		if (ImGui::CollapsingHeader("source plane wave Info"))
		{
		    if (ImGui::InputInt("source num waves", &numWavesSource)){}
		    if (ImGui::InputDouble("source dir x: ", &sourceDirx)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("source dir y: ", &sourceDiry)) {}
		}

	}
	if (ImGui::CollapsingHeader("target Vector Fields Info", ImGuiTreeNodeFlags_DefaultOpen))
	{
		if (ImGui::Combo("target vec types", (int*)&tarFunctionType, "Whirl pool\0plane wave\0sum\0Y shape\0Two Whirl Pool\0Periodic\0\0")) {}

		if (ImGui::Checkbox("Fixed target center and dir", &isFixedTar)) {}
		if (ImGui::CollapsingHeader("target whirl  pool Info"))
		{
		    if (ImGui::InputInt("target singularity index 1", &singIndTar)) {}
		    if (ImGui::InputDouble("target center 1 x: ", &targetCenter1x)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("target center 1 y: ", &targetCenter1y)) {}

		    if (ImGui::InputInt("target singularity index 2", &singIndTar1)) {}
		    if (ImGui::InputDouble("target center 2 x: ", &targetCenter2x)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("target center 2 y: ", &targetCenter2y)) {}
		}

		if (ImGui::CollapsingHeader("target plane wave Info"))
		{
		    if (ImGui::InputInt("target num waves", &numWaveTar)){}
		    if (ImGui::InputDouble("target dir x: ", &sourceDirx)) {}
		    ImGui::SameLine();
		    if (ImGui::InputDouble("target dir y: ", &sourceDiry)) {}
		}

	}

	if (ImGui::InputInt("num of frames", &numFrames))
	{
		if (numFrames <= 0)
			numFrames = 10;
	}

	if (ImGui::InputDouble("drag speed", &dragSpeed))
	{
		if (dragSpeed <= 0)
			dragSpeed = 0.5;
	}

	if (ImGui::DragInt("current frame", &curFrame, dragSpeed, 0, numFrames + 1))
	{
		updateFieldsInView(curFrame);
	}
	if (ImGui::DragFloat("vec ratio", &(vecratio), 0.005, 0, 1))
	{
		updateFieldsInView(curFrame);
	}
	if (ImGui::CollapsingHeader("optimzation parameters", ImGuiTreeNodeFlags_DefaultOpen))
	{
		if (ImGui::InputInt("num iterations", &numIter))
		{
			if (numIter < 0)
				numIter = 1000;
		}
		if (ImGui::InputDouble("grad tol", &gradTol))
		{
			if (gradTol < 0)
				gradTol = 1e-6;
		}
		if (ImGui::InputDouble("x tol", &xTol))
		{
			if (xTol < 0)
				xTol = 0;
		}
		if (ImGui::InputDouble("f tol", &fTol))
		{
			if (fTol < 0)
				fTol = 0;
		}
		if (ImGui::InputInt("quad order", &quadOrder))
		{
		    if (quadOrder <= 0 || quadOrder > 20)
		        quadOrder = 4;
		}
		ImGui::Checkbox("use upsampled mesh", &isUseUpMesh);

	}
	if (ImGui::Combo("initialization types", (int*)&initializationType, "Random\0Linear\0Theoretical\0")) {}

	ImGui::Checkbox("Try Optimization", &isForceOptimize);

	if (ImGui::Button("update values", ImVec2(-1, 0)))
	{
		double fixedx = 0;
		double fixedy = 0;
		Eigen::Vector2d fixedv(1, 0);
		// source vector fields
		if(isFixedSource)
		{
		    fixedx = sourceCenter1x;
		    fixedy = sourceCenter1y;
		    fixedv << sourceDirx, sourceDiry;
		    fixedv *= numWavesSource * 2 * M_PI;
		}
		generateValues(functionType, sourceOmegaFields, sourceZvals, sourceTheoGradZvals, upsampledTheoZVals, singIndSource, singIndSource1, isFixedSource, fixedx, fixedy, fixedv);
		/*std::cout << "triV: \n" << triV << std::endl;

		std::cout << fixedv.transpose() << std::endl;
		for (int i = 0; i < sourceOmegaFields.rows(); i++)
		{
			std::cout << "edge id: " << i << ", vid: " << triMesh.edgeVertex(i, 0) << ", zval: " << sourceZvals[triMesh.edgeVertex(i, 0)] << ",  " << triMesh.edgeVertex(i, 1) << " " << sourceZvals[triMesh.edgeVertex(i, 1)] << ", w: " << sourceOmegaFields(i) << std::endl;
		}*/

		if(isFixedTar)
		{
		   fixedx = targetCenter1x;
		   fixedy = targetCenter1y;
		   fixedv << targetDirx, targetDiry;
		   fixedv *= numWaveTar * 2 * M_PI;
		}
		// target vector fields
		generateValues(tarFunctionType, tarOmegaFields, tarZvals, tarTheoGradZvals, upsampledTarTheoZVals, singIndTar, singIndTar1, isFixedTar, fixedx, fixedy, fixedv);

		// update the theoretic ones
		updateTheoMagnitudePhase(sourceZvals, tarZvals, sourceTheoGradZvals, tarTheoGradZvals, upsampledTheoZVals, upsampledTarTheoZVals, numFrames, theoAmpFieldsList, theoPhaseFieldsList, theoOmegaList, theoZList);

		// solve for the path from source to target
		solveKeyFrames(sourceOmegaFields, tarOmegaFields, sourceZvals, tarZvals, numFrames, omegaList, zList);
		// get interploated amp and phase frames
		updateMagnitudePhase(omegaList, zList, ampFieldsList, phaseFieldsList);

		// update amp max
		if (theoAmpFieldsList.size() > 0)
		{
			globalAmpMax = theoAmpFieldsList[0].maxCoeff();
			for (int i = 0; i < theoAmpFieldsList.size(); i++)
			{
				globalAmpMax = std::max(theoAmpFieldsList[i].maxCoeff(), globalAmpMax);
				globalAmpMax = std::max(ampFieldsList[i].maxCoeff(), globalAmpMax);
			}
		}

		updateFieldsInView(curFrame);

	}

	ImGui::PopItemWidth();
}

int main(int argc, char** argv)
{
	initialization();
//
//	Eigen::MatrixXd testV(3, 3);
//	testV << 0, 0, 0,
//		1, 0, 0,
//		0, 1, 0;
//	Eigen::MatrixXi testF(1, 3);
//	testF << 0, 1, 2;
//
//	MeshConnectivity testMesh(testF);
//
//	Eigen::MatrixXd testw(3, 3), testw1;
//	testw << 1, -1, 0,
//		1, 2, 0,
//		-1, 0, 0;
//
//	testw1 = testw;
//	testw1.block<3, 2>(0, 0).setRandom();
//
//	Eigen::Vector3d testbary;
//	std::vector<std::complex<double>> testZvals(3), testZvals1(3);
//	testZvals[0] = std::complex<double>(0.1, 0.8);
//	testZvals[1] = std::complex<double>(0.7, 0.3);
//	testZvals[2] = std::complex<double>(0.34, 7.8);
//
//	testZvals1[0] = std::complex<double>(0.5, 0.6);
//	testZvals1[1] = std::complex<double>(0.7, 0.4);
//	testZvals1[2] = std::complex<double>(0.4, 0.8);
//
//
//	Eigen::Matrix<double, 3, 2> edgew = vertexVec2IntrinsicHalfEdgeVec(testw, testV, testMesh);
//	Eigen::Matrix<double, 3, 2> edgew1 = vertexVec2IntrinsicHalfEdgeVec(testw1, testV, testMesh);
//
//	for (int i = 0; i < 3; i++)
//	{
//	    int eid = testMesh.faceEdge(0, i);
//		std::cout << "eid: " << eid << ", v0: " << testMesh.edgeVertex(eid, 0) << ", v1: " << testMesh.edgeVertex(eid, 1) << std::endl;
//		std::cout << "edge w: " << edgew.row(eid) << std::endl;
//	}
//	int testqid = 3;
//	ComputeZandZdot preModel(testV, testF, 4);
//	testbary << 1 - preModel.getQuadPts()[testqid].u - preModel.getQuadPts()[testqid].v, preModel.getQuadPts()[testqid].u, preModel.getQuadPts()[testqid].v;
//
//	Eigen::VectorXd testFaceArea;
//	igl::doublearea(testV, testF, testFaceArea);
//	testFaceArea /= 2;
//	IntrinsicFormula::ComputeZdotFromHalfEdgeOmega zdotmodel1(MeshConnectivity(testF), testFaceArea, 4, 0.1);
//
//	double zdotnorm = zdotmodel1.computeZdotIntegration(testZvals, edgew, testZvals1, edgew1, NULL, NULL);
//	double zdotnorm1 = preModel.zDotSquareIntegration(testw.block<3, 2>(0, 0), testw1.block<3, 2>(0, 0), testZvals, testZvals1, 0.1, NULL, NULL);
//
//	std::cout << "zdot: " << zdotnorm << ", " << "zdot1: " << zdotnorm1 << std::endl;
//
//	IntrinsicFormula::IntrinsicKeyFrameInterpolationFromHalfEdge halfEdgeFormula(testMesh, testFaceArea, 50, 4, testZvals, edgew, testZvals1, edgew1);
//	Eigen::VectorXd x;
//	halfEdgeFormula.convertList2Variable(x);
//	zdotnorm = halfEdgeFormula.computeEnergy(x);
//
//	Eigen::MatrixXd uptestV;
//	Eigen::MatrixXi uptestF;
//
//	std::vector<std::pair<int, Eigen::Vector3d>> testBary;
//    meshUpSampling(testV, testF, uptestV, uptestF, 2, NULL, NULL, &testBary);
//
//    InterpolateKeyFrames interpModelTest(testV, testF, uptestV, uptestF, testBary, testw.block<3, 2>(0, 0), testw1.block<3, 2>(0, 0), testZvals, testZvals1, 50, 4, false);
//    interpModelTest.convertList2Variable(x);
//    zdotnorm1 = interpModelTest.computeEnergy(x);

//    std::cout << "zdot: " << zdotnorm << ", " << "zdot1: " << zdotnorm1 << std::endl;
//	IntrinsicFormula::testZvalsFromHalfEdgeOmega(testbary, testZvals, edgew);
//	Eigen::VectorXd faceArea;
//
//	igl::doublearea(triV, triF, faceArea);
//	std::vector<std::complex<double>> testZvals0(triV.rows()), testZvals1(triV.rows());
//	for (int i = 0; i < triV.rows(); i++)
//	{
//		Eigen::Vector2d randvec;
//		randvec.setRandom();
//		testZvals0[i] = std::complex<double>(randvec(0), randvec(1));
//
//		randvec.setRandom();
//		testZvals1[i] = std::complex<double>(randvec(0), randvec(1));
//	}
//	Eigen::MatrixXd edgew0, edgew1;
//	edgew0.setRandom(triMesh.nEdges(), 2);
//	edgew1.setRandom(triMesh.nEdges(), 2);
//
//	IntrinsicFormula::ComputeZdotFromHalfEdgeOmega testmodel(triMesh, faceArea, 4, 1);
//	testmodel.testZdotIntegrationPerface(testZvals0, edgew0, testZvals1, edgew1, 0);
//
//	IntrinsicFormula::IntrinsicKeyFrameInterpolationFromHalfEdge halfedgemodel(triMesh, faceArea, 3, 4, testZvals0, edgew0, testZvals1, edgew1);
//	Eigen::VectorXd testx;
//	halfedgemodel.convertList2Variable(testx);
//	halfedgemodel.testEnergy(testx);

	// Options
    polyscope::options::autocenterStructures = true;
    polyscope::view::windowWidth = 1024;
    polyscope::view::windowHeight = 1024;

    // Initialize polyscope
    polyscope::init();

    // Register the mesh with Polyscope
    polyscope::registerSurfaceMesh("input mesh", triV, triF);

    // Add the callback
    polyscope::state::userCallback = callback;

    polyscope::options::groundPlaneHeightFactor = 0.25; // adjust the plane height
    // Show the gui
    polyscope::show();

    return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_TENPOWER_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_TENPOWER_HPP_INCLUDED

#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/ten.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>
#include <boost/config.hpp>
#include <boost/mpl/if.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( tenpower_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    using result_t = bd::as_floating_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 exp) const BOOST_NOEXCEPT
    {

      result_t result = One<result_t>();
      result_t base = Ten<result_t>();
      auto neg = is_ltz(exp);
      exp =  boost::simd::abs(exp);
      while(exp)
      {
        if (is_odd(exp)) result *= base;
        exp >>= 1;
        base = sqr(base);
      }
      return neg ? rec(result) : result;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( tenpower_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::uint_<A0> >
                          )
  {
    using result_t = bd::as_floating_t<A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 exp) const BOOST_NOEXCEPT
    {
      result_t result = One<result_t>();
      result_t base = Ten<result_t>();
      while(exp)
      {
        if (is_odd(exp)) result *= base;
        exp >>= 1;
        base = sqr(base);
      }
      return result;
    }
  };
} } }

#endif


#pragma once

#include <boost/random/random_number_generator.hpp>
#include <boost/random/uniform_int_distribution.hpp>

namespace calotypes
{

/*! \brief Boost-based reservoir sampling implementation. Best used when sampling
 * a medium subset. */
template<class Engine>
void ReservoirSampling( unsigned int numItems, unsigned int subsetSize,
						std::vector<unsigned int>& inds, Engine& engine )
{
	inds.resize( subsetSize );
	for( unsigned int i = 0; i < subsetSize; i++ )
	{
		inds[i] = i;
	}

	for( unsigned int i = subsetSize; i < numItems; i++ )
	{
		boost::random::uniform_int_distribution<> dist( 0, i ); // i inclusive
		unsigned int j = (unsigned int) dist( engine );
		std::cout << "j: " << j << " ";
		if( j < subsetSize )
		{
			inds[j] = i;
		}
	}
	std::cout << std::endl;
}

/*! \brief Boost-based brute force sampling implementation. Best used when
 * sampling a small subset. */
template<class Engine>
void BitmapSampling( unsigned int numItems, unsigned int subsetSize,
					 std::vector<unsigned int>& inds, Engine& engine )
{
	inds.resize( subsetSize );

	std::vector<bool> bitmap( numItems, false );
	unsigned int count = 0;
	boost::random::uniform_int_distribution<> dist( 0, numItems - 1 );
	while( count < subsetSize )
	{
		unsigned int j = (unsigned int) dist( engine );
		if( bitmap[j] ) { continue; }
		bitmap[j] = true;
		inds[count] = j;
		count++;
	}

}

} // end namespace calotypes


#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main()
{
    int t;
    cin >> t;
    while (t--)
    {
        int n;
        cpp_int fact = 1;
        cin >> n;
        while (n != 0)
        {
            fact *= n--;
        }
        cout << fact << endl;
    }
    return 0;
}


#ifndef HAMILTONIANS_XYZNNNSTO2_HPP
#define HAMILTONIANS_XYZNNNSTO2_HPP
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

/* Stoquastic for |a|, |b| > 1*/
class XYZNNNSto2
{
private:
	int n_;
	double a_;
	double b_;
	constexpr static double J1 = -1.0;
	constexpr static double J2 = -1.0;
public:

	XYZNNNSto2(int n, double a, double b)
		: n_(n), a_(a), b_(b)
	{
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "XYZNNNSto2"},
			{"n", n_},
			{"a", a_},
			{"b", b_}
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		typename State::Scalar s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			double yysign = -smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += -J1*b_*yysign; //zz
			s += J1*(a_+1.0*yysign)*smp.ratio(i, (i+1)%n_); //xx+yy
		}
		//Next-nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			double yysign = -smp.sigmaAt(i)*smp.sigmaAt((i+2)%n_);
			s += -J2*a_*yysign; //zz
			s += J2*(b_+1.0*yysign)*smp.ratio(i, (i+2)%n_); //xx+yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t, double> m;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			m[col ^ x] += J1*(a_ - 1.0*sgn);
			m[col] += J1*b_*sgn;
		}
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+2)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+2)%(n_)));
			m[col ^ x] += J2*(b_ - sgn*1.0);
			m[col] += J2*a_*sgn;
		}
		return m;
	}
};
#endif//HAMILTONIANS_XYZNNNSTO2_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

// TODO: once change this to proper strategy
// It is different from current segment intersection because these are not segments but lines
// If we have the Line concept, we can create a strategy
template <typename Point, typename Line1, typename Line2 = Line1>
struct line_line_intersection
{
    template <typename A, typename B, typename C, typename D>
    static inline A det(A const& a, B const& b, C const& c, D const& d)
    {
        return a * d - b * c;
    }

    static inline bool apply(Line1 const& line1, Line2 const& line2, Point& p)
    {
        // See http://mathworld.wolfram.com/Line-LineIntersection.html
        typedef typename coordinate_type<Point>::type coordinate_type;
        coordinate_type x1 = get<0,0>(line1), y1 = get<0,1>(line1);
        coordinate_type x2 = get<1,0>(line1), y2 = get<1,1>(line1);
        coordinate_type x3 = get<0,0>(line2), y3 = get<0,1>(line2);
        coordinate_type x4 = get<1,0>(line2), y4 = get<1,1>(line2);

        coordinate_type denominator = det(x1 - x2, y1 - y2, x3 - x4, y3 - y4);

        // TODO: use something else then denominator (sides?) to determine this.

        // If denominator is zero, segments are parallel.
        // We have context information, so know that it should then
        // be the case that line1.p2 == line2.p1, and that is the
        // intersection point.
        if (geometry::math::equals(denominator, 0.0))
        {
            set<0>(p, x2);
            set<1>(p, y2);
            return false;
        }

        coordinate_type d1 = det(x1, y1, x2, y2);
        coordinate_type d2 = det(x3, y3, x4, y4);
        coordinate_type px = det(d1, x1 - x2, d2, x3 - x4) / denominator;
        coordinate_type py = det(d1, y1 - y2, d2, y3 - y4) / denominator;

        set<0>(p, px);
        set<1>(p, py);

#ifdef BOOST_GEOMETRY_DEBUG_BUFFER
        if (geometry::math::abs(denominator) < 1.0e-7)
        {
            std::cout << "small " << denominator << std::endl;
        }
#endif
        return geometry::math::abs(denominator) > 1.0e-7;
    }
};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP


#ifndef mesh_reader
#define mesh_reader

#include <fstream>
#include <sstream>
#include <iostream>
#include <string>
#include <math.h>
#include <Eigen/Dense>

using namespace Eigen;
typedef Matrix<int,Dynamic,Dynamic> MatrixXi;

namespace mesh
{

    MatrixXd read_vertices(std::string location)
    {
        /* index # is vertix id, contains x and y
        coordinate respectively of vertix, and
        a flag, 1 if boundary point 0 if not */
        std::ifstream infile(location);
        std::string line;
        double a, b, c, d, line_nb = 0;
        std::getline(infile, line);
        std::istringstream iss(line);
        iss >> a >> b >> c;
        int nb_nodes = a;
        MatrixXd vertices(nb_nodes, 3);
        while (std::getline(infile, line))
        {
            std::istringstream iss(line);
            if (iss >> a >> b >> c >> d)
            {
                vertices(line_nb, 0) = b/1000.;
                vertices(line_nb, 1) = c/1000.;
                vertices(line_nb, 2) = d;
            }
            else
            {
                break;
            }
            line_nb += 1;
        }
        return vertices;
    }

    MatrixXd read_triangles(MatrixXd& vertices,std::string location)
    {
        /* index # is triangle id, contains
        the 3 vertix id's of the corner nodes  and
        the area of the triangle, calculated using
        the vertices matrix */
        std::ifstream infile(location);
        std::string line;
        int a, b, c, d, line_nb = 0;
        std::getline(infile, line);
        std::istringstream iss(line);
        iss >> a >> b >> c;
        int nb_triangles = a;
        MatrixXd triangles(nb_triangles, 4);
        while (std::getline(infile, line))
        {
            std::istringstream iss(line);
            if (iss >> a >> b >> c >> d)
            {
                triangles(line_nb, 0) = b-1;
                triangles(line_nb, 1) = c-1;
                triangles(line_nb, 2) = d-1;
                triangles(line_nb, 3) = 0.5*fabs(vertices(b-1, 0)*(vertices(c-1, 1) - vertices(d-1, 1)) +
                    vertices(c-1, 0)*(vertices(d-1, 1) - vertices(b-1, 1)) +
                    vertices(d-1, 0)*(vertices(b-1, 1) - vertices(c-1, 1)));
            }
            else
            {
                break;
            }
            line_nb += 1;
        }
        return triangles;
    }

    MatrixXi read_boundaries(MatrixXd& vertices,std::string location) {
        /* index # is triangle id, contains
        the 3 vertix id's of the corner nodes  and
        the area of the triangle, calculated using
        the vertices matrix */
        std::ifstream infile(location);
        std::string line;
        int a, b, c, d, count = 0;
        std::getline(infile, line);
        std::getline(infile, line);
        std::istringstream iss(line);
        iss >> a >> b;
        int nb_boundaries = a;
        MatrixXi boundaries(nb_boundaries, 2);
        while (std::getline(infile, line))
        {
            std::istringstream iss(line);
            if (iss >> a >> b >> c >> d)
            {
                if (vertices(b-1, 0) != 0 || vertices(c-1, 0) != 0) {
                  boundaries(count, 0) = b-1;
                  boundaries(count, 1) = c-1;
                  count += 1;
                }
            }
            else
            {
                break;
            }
        }

        MatrixXi boundaries_red(count, 2);
        boundaries_red = boundaries.block(0,0,count,2);
        return boundaries_red;
    }

    void write_result(VectorXd& u,VectorXd& v)
    {
      std::ofstream output_u;
      std::ofstream output_v;
      output_u.open("../mesh/result_u.out");
      output_u << u;
      output_v.open("../mesh/result_v.out");
      output_v << v;
      output_u.close();
      output_v.close();
    }

}

#endif


#include <bits/types/FILE.h>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <string>
#include <tgmath.h>
#include "image_ppm.h"
#include <filesystem>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef Matrix<u_char, Dynamic, Dynamic> MatrixImg;
typedef Matrix<double, Dynamic, Dynamic> TempMatrixImg;
typedef Vector<u_char,Dynamic> ImgLine;
typedef Vector<double,Dynamic> TempImgLine;
//typedef Matrix<ImgLine,Dynamic,Dynamic> FlattenedImages;

unsigned char max(u_char a, u_char b){
    if (a<b) return b;
    else return a;
}

unsigned char min(u_char a, u_char b){
    if (a>b) return b;
    else return a;
}

double max(double a, double b){
    if (a<b) return b;
    else return a;
}

double min(double a, double b){
    if (a>b) return b;
    else return a;
}

vector<double> projectOnEigenSpace(vector<TempImgLine> eigenfaces, TempImgLine imToProj,int K){
    vector<double> res = vector<double>();
    for (int i=0;i<K;i++){
        res.push_back(eigenfaces[i].dot(imToProj));
    }
    return res;
}

ImgLine octToVec(OCTET* im, int nH, int nW){
    ImgLine res(nH*nW);
    for (int i=0; i<nH*nW;i++){
        res(i)=im[i];
    }
    return res;
}

int main(){
    vector<TempImgLine> eigenfaces;
    int K =100; int nH;int nW;
    for (int i=0; i<K;i++){
        OCTET* im;
        char name[50];
        sprintf(name,"eigenfaces/im%d.pgm",i);
        lire_nb_lignes_colonnes_image_pgm(name,&nH,&nW);
        allocation_tableau(im,OCTET,nH*nW);
        lire_image_pgm(name,im,nH*nW);
        TempImgLine eigenFace(nH*nW) ;
        double sum=0.0;
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=im[j]-127;
            sum+=(double)(im[j]-127)*(double)(im[j]-127);
        }
        for (int j=0;j<nH*nW;j++){eigenFace(j)=eigenFace(j)/sqrt(sum);}
        eigenfaces.push_back(eigenFace);
        free(im);
    }

    OCTET* meanIm; allocation_tableau(meanIm,OCTET, nH*nW); lire_image_pgm("MEAN.pgm",meanIm,nH*nW);
    OCTET* felix; allocation_tableau(felix,OCTET,nH*nW); //lire_image_pgm("../Databases/yalefaces/test/pgms/subject01.noglasses.pgm",felix,nH*nW);
    lire_image_pgm("550.pgm",felix,nH*nW);
    //lire_image_pgm("felix320243.pgm",felix,nH*nW);

    //for (int i=0; i<nH*nW;i++){felix[i]=min(255,max(0,felix[i]-meanIm[i]));}

    //ecrire_image_pgm("meanLessFix.pgm",felix,nH,nW);

    //cout<<eigenfaces[1]<<endl;
    double sumF=0.0;
    for (int i=0; i<nH*nW;i++){sumF+=(double)(felix[i]-127)*(double)(felix[i]-127);}
    TempImgLine fixLine(nH*nW) ;for (int i=0;i<nH*nW;i++){fixLine(i)=(double)(felix[i]-127)/sumF;}
    //cout<<fixLine<<endl;
    //cout<<eigenfaces[0]<<endl;
    //cout<<fixLine.dot(eigenfaces[0])<<endl;
    vector<double> projs = projectOnEigenSpace(eigenfaces,fixLine,K);
    double sum=0.0;
    for (int i=0;i<K;i++){
        //cout<<i<<" : "<<projs[i]<<endl;
        sum+=projs[i];
    }
    //cout<<sum<<endl;

    // for (int i=0;i<K;i++){
    //     OCTET* im;allocation_tableau(im,OCTET,nH*nW);
    //     for (int pix=0;pix<nH*nW;pix++){
    //         im[pix]=eigenfaces[i](pix);

    //     }
    //     char name2[50];
    //     sprintf(name2,"testEigenFaces/im%d.pgm",i);
    //     ecrire_image_pgm(name2,im,nH,nW);
    // }

    OCTET* reco; allocation_tableau(reco,OCTET, nH*nW);

    TempImgLine recoTemp(nH*nW); for (int i=0;i<nH*nW;i++){recoTemp(i)=0.0;}

    for (int i=0; i<K;i++){
        for (int pix=0; pix<nH*nW;pix++){
            recoTemp(pix)+=(projs[i]) * (eigenfaces[i])(pix);
        }
        //cout<<recoTemp<<endl;
    }

    // for (int i=0;i<nH*nW;i++){
    //     recoTemp(i)+=meanIm[i];
    // }

    double minR=recoTemp(0); double maxR=recoTemp(0);
    for (int i=0; i<nH*nW;i++){
        minR=min(minR,recoTemp(i));
        maxR=max(maxR,recoTemp(i));
    }
    cout<<minR<<"  "<<maxR<<endl;

    for (int i=0;i<nH*nW;i++){
        reco[i]=(unsigned char)((1.0/2.0)*(((recoTemp(i)-minR)*(255.0/(maxR-minR)))+(meanIm[i])));
        //reco[i]=min(255,max(0,recoTemp(i)));
        //cout<<(int)reco[i]<<endl;
    }

    ecrire_image_pgm("RecoFelix.pgm",reco,nH,nW);

}


#ifndef STAN_MATH_PRIM_FUN_BESSEL_FIRST_KIND_HPP
#define STAN_MATH_PRIM_FUN_BESSEL_FIRST_KIND_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <stan/math/prim/functor/apply_scalar_binary.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{bessel\_first\_kind}(v, x) =
   \begin{cases}
     J_v(x) & \mbox{if } -\infty\leq x \leq \infty \\[6pt]
     \textrm{error} & \mbox{if } x = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{bessel\_first\_kind}(v, x)}{\partial x} =
   \begin{cases}
     \frac{\partial\, J_v(x)}{\partial x} & \mbox{if } -\infty\leq x\leq \infty
 \\[6pt] \textrm{error} & \mbox{if } x = \textrm{NaN} \end{cases} \f]

   \f[
   J_v(x)=\left(\frac{1}{2}x\right)^v
   \sum_{k=0}^\infty \frac{\left(-\frac{1}{4}x^2\right)^k}{k!\, \Gamma(v+k+1)}
   \f]

   \f[
   \frac{\partial \, J_v(x)}{\partial x} = \frac{v}{x}J_v(x)-J_{v+1}(x)
   \f]
 *
 */
template <typename T2, require_arithmetic_t<T2>* = nullptr>
inline T2 bessel_first_kind(int v, const T2 z) {
  check_not_nan("bessel_first_kind", "z", z);
  return boost::math::cyl_bessel_j(v, z);
}

/**
 * Enables the vectorised application of the bessel first kind function, when
 * the first and/or second arguments are containers.
 *
 * @tparam T1 type of first input
 * @tparam T2 type of second input
 * @param a First input
 * @param b Second input
 * @return Bessel first kind function applied to the two inputs.
 */
template <typename T1, typename T2, require_any_container_t<T1, T2>* = nullptr,
          require_not_var_matrix_t<T2>* = nullptr>
inline auto bessel_first_kind(const T1& a, const T2& b) {
  return apply_scalar_binary(a, b, [&](const auto& c, const auto& d) {
    return bessel_first_kind(c, d);
  });
}

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


// SPDX-License-Identifier: MIT
// Copyright (c) 2019-2021 Thomas Vanderbruggen <th.vanderbruggen@gmail.com>

#ifndef SCICPP_POLYNOMIALS_POLYNOMIAL
#define SCICPP_POLYNOMIALS_POLYNOMIAL

#include "scicpp/core/equal.hpp"
#include "scicpp/core/functional.hpp"
#include "scicpp/core/macros.hpp"
#include "scicpp/core/meta.hpp"
#include "scicpp/core/numeric.hpp"
#include "scicpp/linalg/solve.hpp"
#include "scicpp/linalg/utils.hpp"
#include "scicpp/signal/convolve.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <algorithm>
#include <array>
#include <cmath>
#include <complex>
#include <functional>
#include <initializer_list>
#include <iterator>
#include <limits>
#include <tuple>
#include <utility>
#include <vector>

namespace scicpp::polynomial {

//---------------------------------------------------------------------------------
// Polyval
//---------------------------------------------------------------------------------

template <class T, class Array>
auto polyval(T &&x, const Array &coeffs) {
    if constexpr (meta::is_iterable_v<T>) {
        return map([&](auto v) { return polyval(v, coeffs); },
                   std::forward<T>(x));
    } else {
        using scalar_t = std::remove_reference_t<T>;
        static_assert(std::is_same_v<scalar_t, typename Array::value_type>);

        // https://en.wikipedia.org/wiki/Horner%27s_method
        auto res = scalar_t{0};

        std::for_each(coeffs.crbegin(), coeffs.crend(), [&](auto c) {
            res = std::fma(res, x, c);
        });

        return res;
    }
}

//---------------------------------------------------------------------------------
// polyadd
//---------------------------------------------------------------------------------

namespace detail {

template <class U, class V, class W>
void add_arrays(const U &P1, const V &P2, W &P) {
    using T = typename U::value_type;
    static_assert(std::is_same_v<typename U::value_type, T>);
    static_assert(std::is_same_v<typename W::value_type, T>);

    if (P1.size() <= P2.size()) {
        scicpp_require(P.size() >= P2.size());
        std::transform(
            P1.cbegin(), P1.cend(), P2.cbegin(), P.begin(), std::plus<T>());
        std::copy(P2.cbegin() + int(P1.size()),
                  P2.cend(),
                  P.begin() + int(P1.size()));
    } else {
        scicpp_require(P.size() >= P1.size());
        std::transform(
            P2.cbegin(), P2.cend(), P1.cbegin(), P.begin(), std::plus<T>());
        std::copy(P1.cbegin() + int(P2.size()),
                  P1.cend(),
                  P.begin() + int(P2.size()));
    }
}

} // namespace detail

template <typename T, std::size_t M, std::size_t N>
auto polyadd(const std::array<T, M> &P1, const std::array<T, N> &P2) {
    std::array<T, (M > N ? M : N)> P{};
    detail::add_arrays(P1, P2, P);
    return P;
}

template <typename T>
auto polyadd(const std::vector<T> &P1, const std::vector<T> &P2) {
    std::vector<T> P(P1.size() > P2.size() ? P1.size() : P2.size());
    detail::add_arrays(P1, P2, P);
    return P;
}

//---------------------------------------------------------------------------------
// polysub
//---------------------------------------------------------------------------------

namespace detail {

template <class U, class V, class W>
void sub_arrays(const U &P1, const V &P2, W &P) {
    using T = typename U::value_type;
    static_assert(std::is_same_v<typename U::value_type, T>);
    static_assert(std::is_same_v<typename W::value_type, T>);

    if (P1.size() <= P2.size()) {
        scicpp_require(P.size() >= P2.size());
        std::transform(
            P1.cbegin(), P1.cend(), P2.cbegin(), P.begin(), std::minus<T>());
        std::transform(P2.cbegin() + int(P1.size()),
                       P2.cend(),
                       P.begin() + int(P1.size()),
                       [](auto v) { return -v; });
    } else {
        scicpp_require(P.size() >= P1.size());
        std::transform(P2.cbegin(),
                       P2.cend(),
                       P1.cbegin(),
                       P.begin(),
                       [](auto v2, auto v1) { return v1 - v2; });
        std::copy(P1.cbegin() + int(P2.size()),
                  P1.cend(),
                  P.begin() + int(P2.size()));
    }
}

} // namespace detail

template <typename T, std::size_t M, std::size_t N>
auto polysub(const std::array<T, M> &P1, const std::array<T, N> &P2) {
    std::array<T, (M > N ? M : N)> P{};
    detail::sub_arrays(P1, P2, P);
    return P;
}

template <typename T>
auto polysub(const std::vector<T> &P1, const std::vector<T> &P2) {
    std::vector<T> P(P1.size() > P2.size() ? P1.size() : P2.size());
    detail::sub_arrays(P1, P2, P);
    return P;
}

//---------------------------------------------------------------------------------
// polymul
//---------------------------------------------------------------------------------

template <signal::ConvMethod method, class U, class V>
auto polymul(const U &P1, const V &P2) {
    return signal::convolve<method>(P1, P2);
}

// Specialization for the default convolution method
template <class U, class V>
auto polymul(const U &P1, const V &P2) {
    return polymul<signal::DIRECT>(P1, P2);
}

//---------------------------------------------------------------------------------
// polydiv
//---------------------------------------------------------------------------------

namespace detail {

// https://stackoverflow.com/questions/44770632/fft-division-for-fast-polynomial-division
// http://web.cs.iastate.edu/~cs577/handouts/polydivide.pdf

template <class U, class V>
constexpr auto polydiv_impl(const U &u, const V &v) {
    using T = typename U::value_type;
    static_assert(std::is_same_v<T, typename V::value_type>);

    auto tmp(u);
    const auto m = signed_size_t(u.size()) - 1;
    const auto n = signed_size_t(v.size()) - 1;
    const auto scale = T{1} / v[std::size_t(n)];

    for (signed_size_t k = m - n; k >= 0; --k) {
        const auto d = scale * tmp[std::size_t(n + k)];

        for (signed_size_t j = n + k - 1; j >= k; --j) {
            tmp[std::size_t(j)] -= d * v[std::size_t(j - k)];
        }
    }

    return tmp;
}

} // namespace detail

template <typename T, std::size_t M, std::size_t N>
constexpr auto polydiv(const std::array<T, M> &u, const std::array<T, N> &v) {
    static_assert(M > 0);
    static_assert(N > 0);

    if constexpr (N == 1) {
        using namespace scicpp::operators;
        return std::make_tuple(u / v[0], std::array<T, 1>{});
    } else if constexpr (M < N) {
        return std::make_tuple(std::array<T, 1>{}, u);
    } else {
        const auto tmp = detail::polydiv_impl(u, v);

        std::array<T, M - N + 1> q{};
        std::array<T, N - 1> r{};
        std::move(tmp.begin(), tmp.begin() + r.size(), r.begin());
        std::move(tmp.begin() + r.size(), tmp.end(), q.begin());
        return std::make_tuple(q, r);
    }
}

template <typename T>
auto polydiv(const std::vector<T> &u, const std::vector<T> &v) {
    scicpp_require(!u.empty());
    scicpp_require(!v.empty());

    if (v.size() == 1) {
        using namespace scicpp::operators;
        return std::make_tuple(u / v[0], std::vector<T>(1, T{0}));
    }

    if (u.size() < v.size()) {
        return std::make_tuple(std::vector<T>(1, T{0}), u);
    }

    const auto tmp = detail::polydiv_impl(u, v);
    const auto len = signed_size_t(v.size()) - 1;

    return std::make_tuple(
        std::vector<T>(std::make_move_iterator(tmp.begin() + len),
                       std::make_move_iterator(tmp.end())),
        std::vector<T>(std::make_move_iterator(tmp.begin()),
                       std::make_move_iterator(tmp.begin() + len)));
}

//---------------------------------------------------------------------------------
// polymulx
//---------------------------------------------------------------------------------

template <typename T, std::size_t N>
auto polymulx(const std::array<T, N> &P) {
    std::array<T, N + 1> res{};
    std::copy(P.cbegin(), P.cend(), res.begin() + 1);
    return res;
}

template <typename T>
auto polymulx(const std::vector<T> &P) {
    std::vector<T> res(P.size() + 1);
    std::copy(P.cbegin(), P.cend(), res.begin() + 1);
    return res;
}

template <typename T>
auto polymulx(std::vector<T> &&P) {
    P.push_back(T{0});
    std::rotate(P.rbegin(), P.rbegin() + 1, P.rend());
    return std::move(P);
}

//---------------------------------------------------------------------------------
// polypow
//---------------------------------------------------------------------------------

// https://en.wikipedia.org/wiki/Exponentiation_by_squaring
// - Recursive implementation is used for fixed size arrays (std::array).
// - Iterative implementation is used for dynamic size arrays (std::vector).

template <std::size_t pow, typename T, std::size_t N>
auto polypow(const std::array<T, N> &P) {
    // Return a std::array<T, pow * (N - 1) + 1>
    if constexpr (pow == 0) {
        return std::array{T{1}};
    } else if constexpr (pow == 1) {
        return std::array{P};
    } else {
        if constexpr (pow % 2 == 0) {
            return polypow<pow / 2>(polymul(P, P));
        } else {
            return polymul(P, polypow<(pow - 1) / 2>(polymul(P, P)));
        }
    }
}

template <signal::ConvMethod method, typename T>
auto polypow(const std::vector<T> &P, std::size_t pow) {
    if (pow == 0) {
        return std::vector(1, T{1});
    } else if (pow == 1) {
        return std::vector<T>(P.cbegin(), P.cend());
    }

    std::vector<T> x(P.cbegin(), P.cend());
    std::vector<T> y(1, T(1));
    x.reserve(pow * (P.size() - 1) + 1);
    y.reserve(pow * (P.size() - 1) + 1);

    while (pow > 1) {
        if (pow % 2 == 1) {
            y = std::move(polymul<method>(x, y));
        }

        pow = pow / 2;
        x = std::move(polymul<method>(x, x));
    }

    return polymul(x, y);
}

// Specialization for the default convolution method (DIRECT)
template <typename T>
auto polypow(const std::vector<T> &P, std::size_t pow) {
    return polypow<signal::DIRECT>(P, pow);
}

//---------------------------------------------------------------------------------
// polyder
//---------------------------------------------------------------------------------

namespace detail {

template <typename T, std::size_t N>
constexpr auto polyder_once(const std::array<T, N> &P) {
    static_assert(N >= 1);
    std::array<T, N - 1> res{};

    for (std::size_t i = 0; i < res.size(); ++i) {
        res[i] = T(N - 1 - i) * P[i];
    }

    return res;
}

template <typename T>
void polyder_once(std::vector<T> &P) {
    scicpp_require(P.size() >= 1);
    const auto N = P.size();

    for (std::size_t i = 0; i < (N - 1); ++i) {
        P[i] *= T(N - 1 - i);
    }

    P.resize(N - 1);
}

} // namespace detail

template <signed_size_t m, typename T, std::size_t N>
constexpr auto polyder(const std::array<T, N> &P) {
    static_assert(m >= 0);

    if constexpr (m == 0) {
        return std::array{P};
    } else if constexpr (m >= N) {
        return std::array<T, 0>{};
    } else {
        return polyder<m - 1>(detail::polyder_once(P));
    }
}

template <typename T, std::size_t N>
constexpr auto polyder(const std::array<T, N> &P) {
    return polyder<1>(P);
}

template <typename T>
auto polyder(const std::vector<T> &P, signed_size_t m = 1) {
    scicpp_require(m >= 0);

    if (m == 0) {
        return std::vector<T>(P);
    }

    if (m >= int(P.size())) {
        return std::vector<T>(0);
    }

    std::vector<T> res(P);

    while (m--) {
        detail::polyder_once(res);
    }

    return res;
}

//---------------------------------------------------------------------------------
// polyint
//---------------------------------------------------------------------------------

namespace detail {

template <typename T, std::size_t N>
constexpr auto polyint_once(const std::array<T, N> &P) {
    std::array<T, N + 1> res{};

    for (std::size_t i = 0; i < N; ++i) {
        res[i + 1] = P[i] / T(i + 1);
    }

    return res;
}

template <typename T>
void polyint_once(std::vector<T> &P) {
    const auto N = P.size();
    P.resize(N + 1);

    for (signed_size_t i = signed_size_t(N) - 1; i >= 0; --i) {
        P[std::size_t(i) + 1] = P[std::size_t(i)] / T(i + 1);
    }

    P[0] = T{0};
}

} // namespace detail

template <signed_size_t m, typename T, std::size_t N>
constexpr auto polyint(const std::array<T, N> &P) {
    static_assert(N > 0);
    static_assert(m >= 0);

    if constexpr (m == 0) {
        return std::array{P};
    } else {
        return polyint<m - 1>(detail::polyint_once(P));
    }
}

template <typename T, std::size_t N>
constexpr auto polyint(const std::array<T, N> &P) {
    return polyint<1>(P);
}

template <typename T>
auto polyint(const std::vector<T> &P, signed_size_t m = 1) {
    scicpp_require(P.size() > 0);
    scicpp_require(m >= 0);

    if (m == 0) {
        return std::vector<T>(P);
    }

    std::vector<T> res(P);
    res.reserve(P.size() + std::size_t(m));

    while (m--) {
        detail::polyint_once(res);
    }

    return res;
}

//---------------------------------------------------------------------------------
// polycompanion
//---------------------------------------------------------------------------------

template <typename T, std::size_t N>
auto polycompanion(const std::array<T, N> &P) {
    constexpr int deg = N - 1;
    Eigen::Matrix<T, deg, deg> res{};
    res.setZero();
    res.diagonal(-1).setOnes();
    res.col(deg - 1) = -linalg::to_eigen_array<deg>(P) / P[deg];
    return res;
}

template <typename T>
auto polycompanion(const std::vector<T> &P) {
    const int deg = int(P.size()) - 1;
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> res(deg, deg);
    res.setZero();
    res.diagonal(-1).setOnes();
    res.col(deg - 1) = -linalg::to_eigen_matrix(P, deg) / P[std::size_t(deg)];
    return res;
}

//---------------------------------------------------------------------------------
// polyroots
//---------------------------------------------------------------------------------

template <class Array>
auto polyroots(const Array &P) {
    return linalg::to_std_container(polycompanion(P).eigenvalues());
}

//---------------------------------------------------------------------------------
// polyvander
//---------------------------------------------------------------------------------

namespace detail {

template <class Matrix, class Vector>
void polyvander_filler(Matrix &res, const Vector &x) {
    static_assert(meta::is_eigen_matrix_v<Matrix>);

    res.col(0).setOnes();
    res.col(1) = linalg::to_eigen_array(x);

    for (int i = 2; i < res.cols(); ++i) {
        res.col(i) = res.col(i - 1).array() * res.col(1).array();
    }
}

} // namespace detail

template <int deg, typename T, std::size_t N>
auto polyvander(const std::array<T, N> &x) {
    Eigen::Matrix<T, int(N), deg + 1> res{};
    detail::polyvander_filler(res, x);
    return res;
}

template <typename T>
auto polyvander(const std::vector<T> &x, int deg) {
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> res(int(x.size()),
                                                         deg + 1);
    detail::polyvander_filler(res, x);
    return res;
}

//---------------------------------------------------------------------------------
// polyfit
//---------------------------------------------------------------------------------

template <int deg, typename T, std::size_t N>
auto polyfit(const std::array<T, N> &x, const std::array<T, N> &y) {
    return linalg::lstsq(polyvander<deg>(x), y);
}

template <typename T>
auto polyfit(const std::vector<T> &x, const std::vector<T> &y, int deg) {
    return linalg::lstsq(polyvander(x, deg), y);
}

//---------------------------------------------------------------------------------
// polytrim
//---------------------------------------------------------------------------------

template <typename T>
void polytrim(std::vector<T> &P, T tol = T{0}) {
    P.erase(
        std::find_if(P.crbegin(), P.crend(), [=](auto c) { return c > tol; })
            .base(),
        P.end());

    if (P.empty()) {
        P.push_back(T{0});
    }
}

//---------------------------------------------------------------------------------
// Polynomial class
//---------------------------------------------------------------------------------

template <typename T>
class Polynomial {
  public:
    using value_type = typename std::vector<T>::value_type;
    using size_type = typename std::vector<T>::size_type;

    explicit Polynomial(size_type deg) : m_coef(deg + 1, T{0}) {}

    explicit Polynomial(const std::vector<T> &coef) : m_coef(coef) {}
    explicit Polynomial(std::vector<T> &&coef) : m_coef(std::move(coef)) {}

    template <size_type N>
    explicit Polynomial(const std::array<T, N> &coef)
        : m_coef(coef.cbegin(), coef.cend()) {}

    template <class Iterator>
    Polynomial(Iterator first, Iterator last) : m_coef(first, last) {}

    Polynomial(std::initializer_list<T> l)
        : Polynomial(std::begin(l), std::end(l)) {}

    size_type degree() const { return m_coef.size() - 1; }

    const auto &data() const { return m_coef; }

    void mulx() { m_coef = polymulx(std::move(m_coef)); }

    void trim(T tol = T(0)) { polytrim(m_coef, tol); }

    T operator()(T x) const { return polyval(x, m_coef); }

    void reserve(size_type n_coefs) { m_coef.reserve(n_coefs); }

    Polynomial &operator+=(const Polynomial &rhs) {
        if (rhs.degree() > this->degree()) {
            const long old_size = long(m_coef.size());
            m_coef.resize(rhs.degree() + 1);
            std::transform(rhs.m_coef.begin(),
                           rhs.m_coef.begin() + old_size,
                           m_coef.begin(),
                           m_coef.begin(),
                           std::plus<T>());
            std::copy(rhs.m_coef.begin() + old_size,
                      rhs.m_coef.end(),
                      m_coef.begin() + old_size);
        } else {
            std::transform(rhs.m_coef.begin(),
                           rhs.m_coef.end(),
                           m_coef.begin(),
                           m_coef.begin(),
                           std::plus<T>());
        }

        return *this;
    }

    Polynomial &operator-=(const Polynomial &rhs) {
        if (rhs.degree() > this->degree()) {
            const auto old_size = m_coef.size();
            m_coef.resize(rhs.degree() + 1);

            for (size_type i = 0; i < old_size; ++i) {
                m_coef[i] -= rhs.m_coef[i];
            }

            for (size_type i = old_size; i < m_coef.size(); ++i) {
                m_coef[i] = -rhs.m_coef[i];
            }
        } else {
            for (size_type i = 0; i < rhs.degree() + 1; ++i) {
                m_coef[i] -= rhs.m_coef[i];
            }
        }

        return *this;
    }

    Polynomial &operator*=(T scalar) {
        for (auto &c : m_coef) {
            c *= scalar;
        }

        return *this;
    }

    Polynomial &operator*=(const Polynomial &rhs) {
        m_coef = std::move(polymul(m_coef, rhs.m_coef));
        return *this;
    }

    Polynomial operator+(const Polynomial &rhs) const {
        auto tmp(*this);
        tmp += rhs;
        return tmp;
    }

    Polynomial operator-(const Polynomial &rhs) const {
        auto tmp(*this);
        tmp -= rhs;
        return tmp;
    }

    Polynomial operator*(const Polynomial &rhs) const {
        auto tmp(*this);
        tmp *= rhs;
        return tmp;
    }

    bool scicpp_pure operator==(const Polynomial &rhs) const {
        return m_coef == rhs.m_coef;
    }

    bool scicpp_pure operator!=(const Polynomial &rhs) const {
        return m_coef != rhs.m_coef;
    }

    template <int rel_tol = 1>
    bool is_approx(const Polynomial &rhs) const {
        return almost_equal<rel_tol>(m_coef, rhs.m_coef);
    }

  private:
    std::vector<T> m_coef;

}; // class Polynomial

template <typename T>
Polynomial<T> operator*(const Polynomial<T> &P, T scalar) {
    auto tmp(P);
    tmp *= scalar;
    return tmp;
}

template <typename T>
Polynomial<T> operator*(T scalar, const Polynomial<T> &P) {
    return P * scalar;
}

//---------------------------------------------------------------------------------
// polyfromroots
//---------------------------------------------------------------------------------

// https://stackoverflow.com/questions/33594384/find-the-coefficients-of-the-polynomial-given-its-roots
template <class Array>
auto polyfromroots(const Array &roots) {
    using T = typename Array::value_type;

    auto P = Polynomial<T>({T{1}});
    auto P_ = Polynomial<T>({T{1}});
    P.reserve(roots.size() + 1);
    P_.reserve(roots.size());

    for (const auto &r : roots) {
        P_ = P * r;
        P.mulx();
        P -= P_;
    }

    return P;
}

} // namespace scicpp::polynomial

#endif // SCICPP_POLYNOMIALS_POLYNOMIAL