// *Finding Roots Of Polynominals* by Krzysztof Bosak 1999-01-24

//  Bairstow {complex roots}, 
//  Newton {real roots between x1, x2}, 
//  2ndDegree {ax^2+bx+c==0}, 
//  RQ {x^2-rx-q==0},
//  FindRoot {automated Newton}

// polynominals coefficients are stored as follows: ax^2+bx+c ==> a, b, c

#include <iostream.h>
#include <assert.h>
#include <stdlib.h>
#include <math.h>
#include <memory.h>

void PolyOut(double *poly, char degree)
{
	cout<<endl;
	for(unsigned char i=0; i<=degree; i++)
	{
		if(poly[i]<0.0)
			cout<<" -";
		else
			cout<<" +";
		cout<<fabs(poly[i]);
		cout<<"x^"<<(degree-i);
	}
}

void RootOut(double root, double imag=0.0)
{
	if(imag!=0.0)
	{
		if(imag>=0.0)
			cout<<"\n--> Found root == "<<root<<" + i"<<imag;
		else
			cout<<"\n--> Found root == "<<root<<" - i"<<-imag;
	}
	else
		cout<<"\n--> Found root == "<<root;
}

double ValuePoly(double x, double *poly, char degree)
{//value of polynominal at x
	double result=*poly;
	for(unsigned char i=1; i<=degree; i++)
	{
		poly++;
		result*=x;
		result+=*poly;
	}
	return result;
}

double ValuePolyD(double x, double *poly, unsigned char degree)
{//value of derivative of polynominal at x
	double result=*poly*degree;
	for(unsigned char j=degree-1; j; j--)
	{		
		poly++;
		result*=x;
		result+=*poly*j;
	}
	return result;
}

double ValuePoly_PolyD(double x, double *poly, unsigned char degree)
{//should be equal to ValuePoly(x, poly, degree)/ValuePolyD(x, poly, degree);
	double *opoly=poly;
	double result1=*poly*degree, result2=*poly;
	for(unsigned char j=degree-1; j; j--)
	{
		poly++;
		result2*=x;
		result1*=x;
		result2+=*poly;
		result1+=*poly*j;
	}
	poly++;
	result2*=x;
	result2+=*poly;
	return result2/result1;
}

double NewtonSolve(double x1, double x2, double *poly, unsigned char degree, double precision)
{//Finds one real root of polynominal between x0 and x1, assume that W(x0)*W(x1)<0
//x0 and x1 can be any of x0<x1, but it's better to guess them for faster convergence
//assert(ValuePoly(x1, poly, degree)*ValuePoly(x2, poly, degree)<0);//enough for convergence, but not needful
	assert(x2>x1);
	double x=x2, diff, xold;
	do
	{
		xold=x;
		x-=ValuePoly_PolyD(x, poly, degree);
		diff=x-xold;
		if(diff<0.0)
			diff=-diff;
	}while(diff>precision);
	return x;
}

double* DivRootPoly(double root, double *poly, unsigned char degree)
{//returns poly=W(x)/(x-root)
	double *oldpoly=poly;
	for(unsigned char i=0; i<degree; i++, poly++)
		*(poly+1)+=*poly*root;
	return oldpoly;
}

void FindRoot(double *poly, unsigned char degree)
{//Guessing of real roots using Newton
	cout<<"Finding roots of polynominal: ";
	PolyOut(poly, degree);
	cout<<endl;
	do
	{
		double x1, x2, root;
		do
		{
			if(degree&1)
			{
				double x=16;
				while(ValuePoly(x, poly, degree)*ValuePoly(-x, poly, degree)>=0 && x<1E6)
					x*=2;
				x1=-x;
				x2=x;
			}
			else
			{
				do
				{
					double newx;
					switch(rand()%6)
					{
					case 0:
						newx=double(rand())/double(rand()+1);
						break;
					case 1:
						newx=double(rand());
						break;
					case 2:
						newx=1.0/double(rand());
						break;
					case 3:
						newx=(x1+x2)*0.5;
						break;
					case 4:
						newx=(x1+x2)*-4;
						break;
					case 5:
						newx=(x1+x2)*1000000;
						break;
					}
					if(rand()&1)
						x1=newx;
					else
						x2=newx;
					//cout<<x1<<' '<<x2<<' ';
					if(ValuePoly(x1, poly, degree)*ValuePoly(x2, poly, degree)<0)
						break;
				}while(true);
			}
			root=NewtonSolve(x1, x2, poly, degree, 1E-6);
		}while(root<-100000000 || root>1000000000);
		cout<<"Root = "<<root<<endl;
		DivRootPoly(root, poly, degree);
		degree--;
		PolyOut(poly, degree);
	}while(degree>1);
}

void Degree2Solve(double *poly, unsigned char degree)
{//Finds all roots of ax^2+bx+c
	assert(degree>=0 && degree<=2);
	if(degree==1)
	{
		RootOut(poly[1]/ *poly);
		cout<<endl;
	}
	if(degree==2)
	{
		double delta=poly[1]*poly[1]-4*poly[0]*poly[2];
		if(delta>=0)
		{
			RootOut((-poly[1]-sqrt(delta))/2*poly[0]);
			RootOut((-poly[1]+sqrt(delta))/2*poly[0]);
			cout<<endl;
		}
		else
		{
			RootOut(-poly[1]/2*poly[0], -sqrt(-delta)/2*poly[0]);
			RootOut(-poly[1]/2*poly[0], sqrt(-delta)/2*poly[0]);
			cout<<endl;
		}
	}
}

void RQSolve(double r, double q)
{//Finds all roots of x^2-rx-q
	static double poly[3]={1};
	poly[1]=-r;
	poly[2]=-q;
	double delta=poly[1]*poly[1]-4*poly[0]*poly[2];
	if(delta>=0)
	{
		RootOut((-poly[1]-sqrt(delta))/2*poly[0]);
		RootOut((-poly[1]+sqrt(delta))/2*poly[0]);
		cout<<endl;
	}
	else
	{
		RootOut(-poly[1]/2*poly[0], -sqrt(-delta)/2*poly[0]);
		RootOut(-poly[1]/2*poly[0], sqrt(-delta)/2*poly[0]);
		cout<<endl;
	}
}

void DivRQPoly(double r, double q, double *poly, double *result, unsigned char degree,
			   double &A, double &B)
{//result #1: A==poly[degree-1], B==poly[degree] where Ax +B == W(x) mod (x^2-rx-q) 
 //result #2: result/=(x^2-rx-q) 
	if(degree<=2)
	{
		result[0]=0;
		result[1]=0;
		A=poly[degree-1];
		B=poly[degree];
		return;
	}
	static double copy[256];
	memcpy(copy, poly, (degree+1)*sizeof(double));
	for(int i=0; i<degree-1; i++)
	{
		double div=poly[i];
		result[i]=div;
		poly[i]=0;
		poly[i+1]+=r*div;
		poly[i+2]+=q*div;
	}
	A=poly[degree-1];
	B=poly[degree];
	memcpy(poly, copy, (degree+1)*sizeof(double));
}

void BairstowSolve(double *poly, unsigned char degree, double precision)
{//Finds all roots of polynominal
	if(degree==0)
	{
		if(poly[0]==0)
			RootOut(0);
		return;
	}
	while(ValuePoly(0, poly, degree)==0 && degree>=2)
	{
		DivRootPoly(0, poly, degree--);
		RootOut(0);
		PolyOut(poly, degree);
	}
	do
	{
		if(degree<=2)
		{
			Degree2Solve(poly, degree);
			return;
		}
		double r=1, q=1, dr, dq;//guess q & r
		static double W1[256], W2[256];
		do
		{		
	/*		cout<<"\nStepping: r=="<<r<<" q=="<<q;
			PolyOut(poly, degree);*/

			double A, B;//get from Ax+B = W(x) mod (x^2-rx-q)
			DivRQPoly(r, q, poly, W1, degree, A, B);

	/*		PolyOut(poly, degree);cout<<" \t<-- Testing identity with previous.";
			PolyOut(W1, degree-2);cout<<" \t<-- W(x) / (x^2-rx-q)\n";
			cout<<"A=="<<A<<" B=="<<B<<endl;*/

			double A1, B1;//get from A1x+B1 = W1(x) mod (x^2-rx-q)
			DivRQPoly(r, q, W1, W2, degree-2, A1, B1);

			/*		PolyOut(W1, degree-2);cout<<" \t<-- Testing identity with W(x).";
			PolyOut(W1, degree-4);cout<<" \t<-- W1(x) / (x^2-rx-q)\n";
			cout<<"A1=="<<A1<<" B1=="<<B1<<endl;*/

			double 
				Aq=A1,
				Bq=B1,
				Ar=r*A1+B1, 
				Br=q*A1;
			double det=Ar*Bq-Aq*Br;
			dr=(B*Aq-A*Bq)/det;
			dq=(A*Br-B*Ar)/det;
			r+=dr;
			q+=dq;
		}while(fabs(dr)>precision || fabs(dq)>precision);
	/*	cout<<r<<endl;
		cout<<q<<endl;*/
		RQSolve(r, q);
		double i, j;
		DivRQPoly(r, q, poly, W1, degree, i, j);
		memcpy(poly, W1, (degree+1)*sizeof(double));
		degree-=2;
	}while(1);
}

int main()
{
	double poly[26]={1, 2, 3, 4, 5};
	unsigned char degree=4;
	cout<<"Solving: ";
	PolyOut(poly, degree);cout<<endl;
	//cout<<ValuePoly(2, poly, degree)<<endl;
	//cout<<ValuePolyD(2, poly, degree)<<endl;
	//RootOut(NewtonSolve(0, 10, poly, degree, 1E-6));
	//RQSolve(-0.008,-9);
	BairstowSolve(poly, degree, 1E-6);
	/*
	double result[26];
	DivRQPoly(-4, -3, poly, result, degree);
	PolyOut(result, degree);
	cout<<"\nA == "<<poly[degree-1];
	cout<<"\nB == "<<poly[degree];
	*/
	return 0;
}