#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/imgproc/types_c.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/highgui/highgui_c.h"
#include "opencv2/core/core.hpp"

using namespace cv;

/* complex
mul: (a + bi)(c + di) = (ac - bd) + (bc + ad)i
div (a + bi)/(c + di) = ((ac + bd) + (bc - ad)i)/(c^2 + d^2)
*/
void hornerZ(double &resRe, double &resIm, double xRe, double xIm, int n, double *P)
{
	resRe = P[n];
	resIm = 0;
	for (int i = n - 1; i >= 0; i--)
	{
		double origResRe = resRe;
		resRe = (resRe*xRe - resIm*xIm);
		resIm = resIm*xRe + origResRe*xIm;
		resRe += P[i];
	}
}

void dhornerZ(double &resRe, double &resIm, double a, double b, int n, double *P)
{
	resRe = P[n] * n;
	resIm = 0;
	for (int i = n - 1; i >= 1; i--)
	{
		double origResRe = resRe;
		resRe = (resRe*a - resIm*b);
		resIm = resIm*a + origResRe*b;
		resRe += P[i] * i;
	}
}

void d2hornerZ(double &resRe, double &resIm, double a, double b, int n, double *P)
{
	resRe = P[n] * n * (n - 1);
	resIm = 0;
	for (int i = n - 1; i >= 2; i--)
	{
		double origResRe = resRe;
		resRe = (resRe*a - resIm*b);
		resIm = resIm*a + origResRe*b;
		resRe += P[i] * i *(i - 1);
	}
}

int LaguerreZ(double &resRe, double &resIm, double xRe, double xIm, int n, double *P)
{
	int loopCnt = 0;
	while (true)
	{
		double hRe, hIm;
		hornerZ(hRe, hIm, xRe, xIm, n, P);
		double dhRe, dhIm;
		dhornerZ(dhRe, dhIm, xRe, xIm, n, P);
		if (sqrt(hRe*hRe + hIm*hIm) < DBL_EPSILON*sqrt(dhRe*dhRe + dhIm*dhIm)) break;
		double GRe, GIm;
		double denomh = hRe*hRe + hIm*hIm;
		GRe = (dhRe*hRe + dhIm*hIm) / denomh;
		GIm = (dhIm*hRe - dhRe*hIm) / denomh;
		double G2Re, G2Im;
		G2Re = GRe*GRe - GIm*GIm;
		G2Im = 2 * GRe * GIm;
		double d2hRe, d2hIm;
		d2hornerZ(d2hRe, d2hIm, xRe, xIm, n, P);
		double GRe2, GIm2;
		GRe2 = (d2hRe*hRe + d2hIm*hIm) / denomh;
		GIm2 = (d2hIm*hRe - d2hRe*hIm) / denomh;
		double HRe, HIm;
		HRe = G2Re - GRe2;
		HIm = G2Im - GIm2;
		double vRe, vIm;
		vRe = (n - 1)*(n*HRe - G2Re);
		vIm = (n - 1)*(n*HIm - G2Im);
		double modv = sqrt(vRe*vRe + vIm*vIm);
		double sRe, sIm;
		sRe = sqrt((modv + vRe) / 2);
		sIm = sqrt((modv - vRe) / 2);
		if (vIm < 0) sIm = -sIm;
		double denom1Re, denom1Im;
		denom1Re = GRe + sRe;
		denom1Im = GIm + sIm;
		double denom2Re, denom2Im;
		denom2Re = GRe - sRe;
		denom2Im = GIm - sIm;
		double denomRe, denomIm;
		if (denom1Re*denom1Re + denom1Im*denom1Im > denom2Re*denom2Re + denom2Im*denom2Im)
		{
			denomRe = denom1Re;
			denomIm = denom1Im;
		}
		else
		{
			denomRe = denom2Re;
			denomIm = denom2Im;
		}
		double aRe, aIm;
		double denoma = denomRe*denomRe + denomIm*denomIm;
		aRe = n*denomRe / denoma;
		aIm = -n*denomIm / denoma;
		if (sqrt(aRe*aRe + aIm*aIm) < 2 * DBL_EPSILON*sqrt(xRe*xRe + xIm*xIm))
		{
			xRe -= 0.5*aRe;
			xIm -= 0.5*aIm;
			break;
		}
		xRe -= aRe;
		xIm -= aIm;
		loopCnt++;
	}
	resRe = xRe;
	resIm = xIm;
	return loopCnt;
}

Mat src;

const int degP = 4;
double P[degP+1] = { 1,4,3,2,1 }; //x^4 + 2*x^3 + 3*x^2 + 4*x + 1
double roots[degP][2] = {
{ -0.3092124060750119,0 },{ -1.487258116300765,0 },
{ -0.1017647388121114,  1.471098423067639 },
{ -0.1017647388121114, -1.471098423067639 } };

int findNearestRoot(double xRe, double xIm)
{
	double minDist = INFINITY;
	int choose = -1;
	for (int i = 0; i < degP; i++)
	{
		double dRe = xRe - roots[i][0];
		double dIm = xIm - roots[i][1];
		double Dist = dRe*dRe + dIm*dIm;
		if (Dist < minDist)
		{
			minDist = Dist;
			choose = i;
		}
	}
	return choose;
}

double colors[degP][3] = { {0, 255, 255},{0, 0, 255},{255, 0, 255},{255, 255, 0}};

int main()
{
	const int matX = 512, matY = 512;
	double ScaleX = 100/3.0;
	double ScaleY = ScaleX;
	double centerX = -4;
	double centerY = 0;

	Mat mat(matY, matX, CV_8UC3);
	Vec3b color;
	for (int i = 0; i < matY; i++)
		for (int j = 0; j < matX; j++)
		{
			double xRe, xIm;
			xRe = (j-matX/2)/ScaleX + centerX; 
			xIm = (i-matY/2)/ScaleY + centerY;
			LaguerreZ(xRe, xIm, xRe, xIm, degP, P);
			int numColor = findNearestRoot(xRe, xIm);
			color = Vec3b(colors[numColor][0], colors[numColor][1], colors[numColor][2]);
			mat.at<Vec3b>(i, j) = color;
		}

	for (int i = 0; i < degP; i++)
	{
		Rect R;		
		R.x = (roots[i][0] - centerX) *  ScaleX + matX / 2 - 2;
		R.y = (roots[i][1] - centerY) *  ScaleY + matY / 2 - 2;
		R.height = 4;
		R.width = 4;
		rectangle(mat, R, Scalar(colors[i][0] * 0.78, colors[i][1] * 0.78, colors[i][2]*0.78), 2, 2, 0);
	}
	imshow("mat", mat);
	cvWaitKey(0);
    return 0;
}