/*
 * iir.c
 *
 *  Created on: 1 апр. 2021 г.
 *      Author: Toporov
 */


#include "my.h"
#include <math.h>
#include "arm_math.h"
#include "FFTCode.h"



extern volatile uint32_t Fs;

#define NUM_PLOT_PTS 128



 TIIRCoeff IIR; // This gets returned.



//---------------------------------------------------------------------------
// H(s) = ( Ds^2 + Es + F ) / ( As^2 + Bs + C )
// H(z) = ( b2z^2 + b1z + b0 ) / ( a2z^2 + a1z + a0 )
TIIRCoeff CalcIIRFilterCoeff(TIIRFilterTypes ProtoType, double Beta, TIIRPassTypes PassType, int NumPoles, double Fc, double BW)
{
 int j, k;
 double Scalar;
 double Coeff[5], RealRoot[4], ImagRoot[4];
 double A, B, C, D, E, F, T, Q, Arg;
 double a2[ARRAY_DIM], a1[ARRAY_DIM], a0[ARRAY_DIM], b2[ARRAY_DIM], b1[ARRAY_DIM], b0[ARRAY_DIM];
 double OmegaC;
 TSPlaneCoeff SPlaneCoeff;

 OmegaC = ((Fc * 2) / ((float64_t) Fs));
 // Init the IIR structure. a3, a4, b3,and b4 are only used to calculate the 2nd order
 // sections for the bandpass and notch filters. They get discarded.
 for(j=0; j<ARRAY_DIM; j++)
  {
   IIR.a0[j] = 0.0;  IIR.b0[j] = 0.0;
   IIR.a1[j] = 0.0;  IIR.b1[j] = 0.0;
   IIR.a2[j] = 0.0;  IIR.b2[j] = 0.0;
   IIR.a3[j] = 0.0;  IIR.b3[j] = 0.0;
   IIR.a4[j] = 0.0;  IIR.b4[j] = 0.0;
  }
 IIR.NumSections = 0;

 // This gets the 2nd order s plane filter coefficients from the tables in FilterCoefficients.hpp.
 // If you have code that computes these coefficients, then use that instead.
 SPlaneCoeff = GetSPlaneCoefficients(ProtoType, NumPoles, Beta);
 IIR.NumSections = SPlaneCoeff.NumSections;  // NumSections = (NumPoles + 1)/2

 // T sets the filter's corner frequency, or center freqency.
 // The Bilinear transform is defined as:  s = 2/T * tan(Omega/2) = 2/T * (1 - z)/(1 + z)
 T = 2.0 * tan(OmegaC * M_PI_2);
 Q = 1.0 + OmegaC;
 if(Q > 1.95)Q = 1.95;
 Q = 0.8 * tan(Q * M_PI_4); // This is a correction factor for Q. Q is used with band pass and notch flters.
 Q = OmegaC / BW / Q;       // This is the corrected Q.

 k = 0;
 for(j=0; j<SPlaneCoeff.NumSections; j++)
  {
   A = SPlaneCoeff.A[j];   // We use A - F to make the code easier to read.
   B = SPlaneCoeff.B[j];
   C = SPlaneCoeff.C[j];
   D = SPlaneCoeff.D[j];
   E = SPlaneCoeff.E[j];
   F = SPlaneCoeff.F[j];

  // b's are the numerator  a's are the denominator
  if(PassType == iirLPF)
   {
	if(A == 0.0 && D == 0.0) // 1 pole case
	 {
	  Arg = (2.0*B + C*T);
	  IIR.a2[j] = 0.0;
	  IIR.a1[j] = (-2.0*B + C*T) / Arg;
	  IIR.a0[j] = 1.0;     // The filter implementation depends on a0 = 1.

	  IIR.b2[j] = 0.0;
	  IIR.b1[j] = (-2.0*E + F*T) / Arg * C/F;
	  IIR.b0[j] = ( 2.0*E + F*T) / Arg * C/F;
	 }
	else // 2 poles
	 {
	  Arg = (4.0*A + 2.0*B*T + C*T*T);
	  IIR.a2[j] = (4.0*A - 2.0*B*T + C*T*T) / Arg;
	  IIR.a1[j] = (2.0*C*T*T - 8.0*A) / Arg;
	  IIR.a0[j] = 1.0;     // The filter implementation depends on a0 = 1.

	  // With all pole filters, our LPF numerator is (z+1)^2, so all our Z Plane zeros are at -1
	  IIR.b2[j] = (4.0*D - 2.0*E*T + F*T*T) / Arg * C/F;
	  IIR.b1[j] = (2.0*F*T*T - 8.0*D) / Arg * C/F;
	  IIR.b0[j] = (4*D + F*T*T + 2.0*E*T) / Arg * C/F;
	 }
   }

  if(PassType == iirHPF)
   {
	if(A == 0.0 && D == 0.0) // 1 pole
	 {
	  Arg = 2.0*C + B*T;
	  IIR.a2[j] = 0.0;
	  IIR.a1[j] = (B*T - 2.0*C) / Arg;
	  IIR.a0[j] = 1.0;     // The filter implementation depends on a0 = 1.

	  IIR.b2[j] = 0.0;
	  IIR.b1[j] = (E*T - 2.0*F) / Arg * C/F;
	  IIR.b0[j] = (E*T + 2.0*F) / Arg * C/F;
	 }
	else  // 2 poles
	 {
	  Arg = A*T*T + 4.0*C + 2.0*B*T;
	  IIR.a2[j] = (A*T*T + 4.0*C - 2.0*B*T) / Arg;
	  IIR.a1[j] = (2.0*A*T*T - 8.0*C) / Arg;
	  IIR.a0[j] = 1.0;

	  // With all pole filters, our HPF numerator is (z-1)^2, so all our Z Plane zeros are at 1
	  IIR.b2[j] = (D*T*T - 2.0*E*T + 4.0*F) / Arg * C/F;
	  IIR.b1[j] = (2.0*D*T*T - 8.0*F) / Arg * C/F;
	  IIR.b0[j] = (D*T*T + 4.0*F + 2.0*E*T) / Arg * C/F;
	 }
   }

  if(PassType == iirBPF)
   {
	if(A == 0.0 && D == 0.0) // 1 pole
	 {
	  Arg = 4.0*B*Q + 2.0*C*T + B*Q*T*T;
	  a2[k] = (B*Q*T*T + 4.0*B*Q - 2.0*C*T) / Arg;
	  a1[k] = (2.0*B*Q*T*T - 8.0*B*Q) / Arg ;
	  a0[k] = 1.0;

	  b2[k] = (E*Q*T*T + 4.0*E*Q - 2.0*F*T) / Arg * C/F;
	  b1[k] = (2.0*E*Q*T*T - 8.0*E*Q) / Arg * C/F;
	  b0[k] = (4.0*E*Q + 2.0*F*T + E*Q*T*T) / Arg * C/F;
	  k++;
	 }
	else //2 Poles
	 {
	  IIR.a4[j] = (16.0*A*Q*Q + A*Q*Q*T*T*T*T + 8.0*A*Q*Q*T*T - 2.0*B*Q*T*T*T - 8.0*B*Q*T + 4.0*C*T*T) * F;
	  IIR.a3[j] = (4.0*T*T*T*T*A*Q*Q - 4.0*Q*T*T*T*B + 16.0*Q*B*T - 64.0*A*Q*Q) * F;
	  IIR.a2[j] = (96.0*A*Q*Q - 16.0*A*Q*Q*T*T + 6.0*A*Q*Q*T*T*T*T - 8.0*C*T*T) * F;
	  IIR.a1[j] = (4.0*T*T*T*T*A*Q*Q + 4.0*Q*T*T*T*B - 16.0*Q*B*T - 64.0*A*Q*Q) * F;
	  IIR.a0[j] = (16.0*A*Q*Q + A*Q*Q*T*T*T*T + 8.0*A*Q*Q*T*T + 2.0*B*Q*T*T*T + 8.0*B*Q*T + 4.0*C*T*T) * F;

	  // With all pole filters, our BPF numerator is (z-1)^2 * (z+1)^2 so the zeros come back as +/- 1 pairs
	  IIR.b4[j] = (8.0*D*Q*Q*T*T - 8.0*E*Q*T + 16.0*D*Q*Q - 2.0*E*Q*T*T*T + D*Q*Q*T*T*T*T + 4.0*F*T*T) * C;
	  IIR.b3[j] = (16.0*E*Q*T - 4.0*E*Q*T*T*T - 64.0*D*Q*Q + 4.0*D*Q*Q*T*T*T*T) * C;
	  IIR.b2[j] = (96.0*D*Q*Q - 8.0*F*T*T + 6.0*D*Q*Q*T*T*T*T - 16.0*D*Q*Q*T*T) * C;
	  IIR.b1[j] = (4.0*D*Q*Q*T*T*T*T - 64.0*D*Q*Q + 4.0*E*Q*T*T*T - 16.0*E*Q*T) * C;
	  IIR.b0[j] = (16.0*D*Q*Q + 8.0*E*Q*T + 8.0*D*Q*Q*T*T + 2.0*E*Q*T*T*T + 4.0*F*T*T + D*Q*Q*T*T*T*T) * C;

      // T = 2 makes these values approach 0.0 (~ 1.0E-12) The root solver needs 0.0 for numerical reasons.
      if(fabs(T-2.0) < 0.0005)
       {
        IIR.a3[j] = 0.0;
        IIR.a1[j] = 0.0;
        IIR.b3[j] = 0.0;
        IIR.b1[j] = 0.0;
       }

      // We now have a 4th order poly in the form a4*s^4 + a3*s^3 + a2*s^2 + a2*s + a0
	  // We find the roots of this so we can form two 2nd order polys.
	  Coeff[0] = IIR.a4[j];
	  Coeff[1] = IIR.a3[j];
	  Coeff[2] = IIR.a2[j];
	  Coeff[3] = IIR.a1[j];
	  Coeff[4] = IIR.a0[j];
      QuadCubicRoots(4, Coeff, RealRoot, ImagRoot);

	  // In effect, the root finder scales the poly by 1/a4 so we have to apply
	  // this factor back into the two 2nd order equations we are forming.
	  if(IIR.a4[j] < 0.0)Scalar = -sqrt(-IIR.a4[j]);
	  else               Scalar = sqrt(IIR.a4[j]);

      // Form the 2nd order polys from the roots.
	  a2[k] = Scalar;
	  a1[k] = -(RealRoot[0] + RealRoot[1]) * Scalar;
	  a0[k] =  (RealRoot[0] * RealRoot[1] - ImagRoot[0] * ImagRoot[1]) * Scalar;
	  k++;
	  a2[k] = Scalar;
	  a1[k] = -(RealRoot[2] + RealRoot[3]) * Scalar;
	  a0[k] =  (RealRoot[2] * RealRoot[3] - ImagRoot[2] * ImagRoot[3]) * Scalar;
	  k--;

	  // Now do the same with the numerator.
	  Coeff[0] = IIR.b4[j];
	  Coeff[1] = IIR.b3[j];
	  Coeff[2] = IIR.b2[j];
	  Coeff[3] = IIR.b1[j];
	  Coeff[4] = IIR.b0[j];
	  QuadCubicRoots(4, Coeff, RealRoot, ImagRoot);

	  if(IIR.b4[j] < 0.0)Scalar = -sqrt(-IIR.b4[j]);
	  else               Scalar = sqrt(IIR.b4[j]);

	  b2[k] = Scalar;
	  if(ProtoType == ftINVERSE_CHEBY || ProtoType >= ftELLIPTIC_00)
	   b1[k] = -(RealRoot[0] + RealRoot[1]) * Scalar;
	  else  // else the prototype is an all pole filter
	   b1[k] = 0.0;  // b1 = 0 for all pole filters, but the addition above won't always equal zero exactly.
	  b0[k] =  (RealRoot[0] * RealRoot[1] - ImagRoot[0] * ImagRoot[1]) * Scalar;
	  k++;

	  b2[k] = Scalar;
	  if(ProtoType == ftINVERSE_CHEBY || ProtoType >= ftELLIPTIC_00)
	   b1[k] = -(RealRoot[2] + RealRoot[3]) * Scalar;
	  else
	   b1[k] = 0.0;
	  b0[k] =  (RealRoot[2] * RealRoot[3] - ImagRoot[2] * ImagRoot[3]) * Scalar;
	  k++;
	  // Go below to see where we store these 2nd order polys back into IIR
	 }
   }

  if(PassType == iirNOTCH)
   {
	if(A == 0.0 && D == 0.0) // 1 pole
	 {
	  Arg = 2.0*B*T + C*Q*T*T + 4.0*C*Q;
	  a2[k] = (4.0*C*Q - 2.0*B*T + C*Q*T*T) / Arg;
	  a1[k] = (2.0*C*Q*T*T - 8.0*C*Q) / Arg;
	  a0[k] = 1.0;

	  b2[k] = (4.0*F*Q - 2.0*E*T + F*Q*T*T) / Arg * C/F;
	  b1[k] = (2.0*F*Q*T*T - 8.0*F*Q) / Arg *C/F;
	  b0[k] = (2.0*E*T + F*Q*T*T +4.0*F*Q) / Arg *C/F;
	  k++;
	 }
	else
	 {
	  IIR.a4[j] = (4.0*A*T*T - 2.0*B*T*T*T*Q + 8.0*C*Q*Q*T*T - 8.0*B*T*Q + C*Q*Q*T*T*T*T + 16.0*C*Q*Q) * -F;
	  IIR.a3[j] = (16.0*B*T*Q + 4.0*C*Q*Q*T*T*T*T - 64.0*C*Q*Q - 4.0*B*T*T*T*Q) * -F;
	  IIR.a2[j] = (96.0*C*Q*Q - 8.0*A*T*T - 16.0*C*Q*Q*T*T + 6.0*C*Q*Q*T*T*T*T) * -F;
	  IIR.a1[j] = (4.0*B*T*T*T*Q - 16.0*B*T*Q - 64.0*C*Q*Q + 4.0*C*Q*Q*T*T*T*T) * -F;
	  IIR.a0[j] = (4.0*A*T*T + 2.0*B*T*T*T*Q + 8.0*C*Q*Q*T*T + 8.0*B*T*Q + C*Q*Q*T*T*T*T + 16.0*C*Q*Q) * -F;

	  // Our Notch Numerator isn't simple. [ (4+T^2)*z^2 - 2*(4-T^2)*z + (4+T^2) ]^2
	  IIR.b4[j] = (2.0*E*T*T*T*Q - 4.0*D*T*T - 8.0*F*Q*Q*T*T + 8.0*E*T*Q - 16.0*F*Q*Q - F*Q*Q*T*T*T*T) * C;
	  IIR.b3[j] = (64.0*F*Q*Q + 4.0*E*T*T*T*Q - 16.0*E*T*Q - 4.0*F*Q*Q*T*T*T*T) * C;
	  IIR.b2[j] = (8.0*D*T*T - 96.0*F*Q*Q + 16.0*F*Q*Q*T*T - 6.0*F*Q*Q*T*T*T*T) * C;
	  IIR.b1[j] = (16.0*E*T*Q - 4.0*E*T*T*T*Q + 64.0*F*Q*Q - 4.0*F*Q*Q*T*T*T*T) * C;
	  IIR.b0[j] = (-4.0*D*T*T - 2.0*E*T*T*T*Q - 8.0*E*T*Q - 8.0*F*Q*Q*T*T - F*Q*Q*T*T*T*T - 16.0*F*Q*Q) * C;

      // T = 2 makes these values approach 0.0 (~ 1.0E-12) The root solver needs 0.0 for numerical reasons.
      if(fabs(T-2.0) < 0.0005)
       {
        IIR.a3[j] = 0.0;
        IIR.a1[j] = 0.0;
        IIR.b3[j] = 0.0;
        IIR.b1[j] = 0.0;
       }

	  // We now have a 4th order poly in the form a4*s^4 + a3*s^3 + a2*s^2 + a2*s + a0
	  // We find the roots of this so we can form two 2nd order polys.
	  Coeff[0] = IIR.a4[j];
	  Coeff[1] = IIR.a3[j];
	  Coeff[2] = IIR.a2[j];
	  Coeff[3] = IIR.a1[j];
	  Coeff[4] = IIR.a0[j];
	  QuadCubicRoots(4, Coeff, RealRoot, ImagRoot);

      // In effect, the root finder scales the poly by 1/a4 so we have to apply
	  // this factor back into the two 2nd order equations we are forming.
	  if(IIR.a4[j] < 0.0)Scalar = -sqrt(-IIR.a4[j]);
	  else               Scalar = sqrt(IIR.a4[j]);
	  a2[k] = Scalar;
	  a1[k] = -(RealRoot[0] + RealRoot[1]) * Scalar;
	  a0[k] =  (RealRoot[0] * RealRoot[1] - ImagRoot[0] * ImagRoot[1]) * Scalar;
	  k++;
	  a2[k] = Scalar;
	  a1[k] = -(RealRoot[2] + RealRoot[3]) * Scalar;
	  a0[k] =  (RealRoot[2] * RealRoot[3] - ImagRoot[2] * ImagRoot[3]) * Scalar;
	  k--;

	  // Now do the same with the numerator.
	  Coeff[0] = IIR.b4[j];
	  Coeff[1] = IIR.b3[j];
	  Coeff[2] = IIR.b2[j];
	  Coeff[3] = IIR.b1[j];
	  Coeff[4] = IIR.b0[j];
	  QuadCubicRoots(4, Coeff, RealRoot, ImagRoot);

	  if(IIR.b4[j] < 0.0)Scalar = -sqrt(-IIR.b4[j]);
	  else               Scalar = sqrt(IIR.b4[j]);

	  b2[k] = Scalar;
	  b1[k] = -(RealRoot[0] + RealRoot[1]) * Scalar;
	  b0[k] =  (RealRoot[0] * RealRoot[1] - ImagRoot[0] * ImagRoot[1]) * Scalar;
	  k++;
	  b2[k] = Scalar;
	  b1[k] = -(RealRoot[2] + RealRoot[3]) * Scalar;
	  b0[k] =  (RealRoot[2] * RealRoot[3] - ImagRoot[2] * ImagRoot[3]) * Scalar;
	  k++;
	 }
   }
 }

 if(PassType == iirBPF || PassType == iirNOTCH)
  {
   // In the calcs above for the BPF and Notch, we didn't set a0=1, so we do it here.
   // k will equal the number of poles.
   for(j=0; j<k; j++)
	{
	 b2[j] /= a0[j];
	 b1[j] /= a0[j];
	 b0[j] /= a0[j];
	 a2[j] /= a0[j];
	 a1[j] /= a0[j];
	 a0[j] = 1.0;
	}

   for(j=0; j<k; j++)
	{
	 IIR.a0[j] = a0[j];
	 IIR.a1[j] = a1[j];
	 IIR.a2[j] = a2[j];
	 IIR.b0[j] = b0[j];
	 IIR.b1[j] = b1[j];
	 IIR.b2[j] = b2[j];
	}
  }

 return(IIR);  // IIR is the structure containing the coefficients.
}


TSPlaneCoeff GetSPlaneCoefficients(uint8_t FilterType, int NumPoles, double Beta)
{

 #include "FilterCoefficients.h" // This file contains all the data arrays used below.

 int j, n, p, z, MinNumPoles, MaxNumPoles;
 int ArrayNumber, OuterArrayDim, NumSections;
 double LoopBeta, BetaMin, BetaStep, BetaMax;
 TSPlaneCoeff SPlaneCoeff;

 BetaMin =  BetaMinArray[FilterType];
 BetaStep = BetaStepArray[FilterType];
 BetaMax =  BetaMaxArray[FilterType];
 MinNumPoles = MinNumPolesArray[FilterType];
 MaxNumPoles = MaxNumPolesArray[FilterType];

 // Range check
 if(NumPoles < MinNumPoles)NumPoles = MinNumPoles;
 if(NumPoles > MaxNumPoles)NumPoles = MaxNumPoles;
 if(Beta < BetaMin)Beta = BetaMin;
 if(Beta > BetaMax)Beta = BetaMax;

 // Need the array number that corresponds to Beta.
 // The max array size was originally determined with this piece of code,
 // so we use it here so we can range check ArrayNumber.
 OuterArrayDim = 0;
 for(LoopBeta=BetaMin; LoopBeta<=BetaMax; LoopBeta+=BetaStep)OuterArrayDim++;

 ArrayNumber = 0;
 for(LoopBeta=BetaMin; LoopBeta<=BetaMax; LoopBeta+=BetaStep)
  {
   if( LoopBeta >= Beta - BetaStep/1.9999 &&  LoopBeta <= Beta + BetaStep/1.9999)break;
   ArrayNumber++;
  }
 if(ArrayNumber > OuterArrayDim-1)ArrayNumber = OuterArrayDim-1; // i.e. If the array is dimensioned to N, the highest we can access is N-1

 // n is the array location for the given pole count.
 // The arrays start at 0. MinNumPoles is either 2 or 4.
 n = NumPoles - MinNumPoles;
 if(n < 0)n = 0;

 // NumSections is the number of biquad sections for a given pole count.
 NumSections = (NumPoles + 1) / 2;
 p = z = 0;

 switch(FilterType)
  {
 	 case ftBUTTERWORTH:
   for(j=0; j<NumSections; j++)
    {
     SPlaneCoeff.A[j] = ButterworthDenominator[n][p++];
     SPlaneCoeff.B[j] = ButterworthDenominator[n][p++];
     SPlaneCoeff.C[j] = ButterworthDenominator[n][p++];
     SPlaneCoeff.D[j] = 0.0;
     SPlaneCoeff.E[j] = 0.0;
     SPlaneCoeff.F[j] = 1.0;
    }
   break;

   case ftCHEBYSHEV:
   for(j=0; j<NumSections; j++)
    {
     SPlaneCoeff.A[j] = ChebyshevDenominator[ArrayNumber][n][p++];
     SPlaneCoeff.B[j] = ChebyshevDenominator[ArrayNumber][n][p++];
     SPlaneCoeff.C[j] = ChebyshevDenominator[ArrayNumber][n][p++];
     SPlaneCoeff.D[j] = 0.0;
     SPlaneCoeff.E[j] = 0.0;
     SPlaneCoeff.F[j] = 1.0;
    }
   break;

   case ftINVERSE_CHEBY:
   for(j=0; j<NumSections; j++)
    {
     SPlaneCoeff.A[j] = InvChebyDenominator[ArrayNumber][n][p++];
     SPlaneCoeff.B[j] = InvChebyDenominator[ArrayNumber][n][p++];
     SPlaneCoeff.C[j] = InvChebyDenominator[ArrayNumber][n][p++];
     SPlaneCoeff.D[j] = InvChebyNumerator[ArrayNumber][n][z++];
     SPlaneCoeff.E[j] = InvChebyNumerator[ArrayNumber][n][z++];
     SPlaneCoeff.F[j] = InvChebyNumerator[ArrayNumber][n][z++];
    }
   break;
 }

 SPlaneCoeff.NumSections = NumSections;
 return(SPlaneCoeff);

}

int QuadCubicRoots(int N, double *Coeff, double *RootsReal, double *RootsImag)
{
 if(N <= 1 || N > 4)
  {
   return(0);
  }

 int j, k;
 long double P[5], RealRoot[4], ImagRoot[4];

 // Must init to zero, in case N is reduced.
 for(j=0; j<4; j++)RealRoot[j] = ImagRoot[j] = 0.0;
 for(j=0; j<5; j++)P[j] = 0.0;

 // Reduce the order if there are trailing zeros.
 for(k=N; k>=0; k--)
  {
   if(fabs(Coeff[k]) > ZERO_PLUS)break;  // break on the 1st nonzero coeff
   Coeff[k] = 0.0;
   N--;
  }

 // Mandatory to remove leading zeros.
 while( fabs(Coeff[0]) < ZERO_PLUS && N>0)
  {
   for(k=0; k<N; k++)
    {
     Coeff[k] = Coeff[k+1];
    }
   Coeff[N] = 0.0;
   N--;
  }

 // The functions below modify the coeff array, so we pass P instead of Coeff.
 for(j=0; j<=N; j++)P[j] = Coeff[j];

 // Mandatory to normalize the coefficients
 if(P[0] != 1.0)
  {
   for(k=1; k<=N; k++)
    {
     P[k] /= P[0];
    }
   P[0] = 1.0;
  }

 if(N==4)BiQuadRoots(P, RealRoot, ImagRoot);
 if(N==3)CubicRoots(P, RealRoot, ImagRoot);
 if(N==2)QuadRoots(P, RealRoot, ImagRoot);
 if(N==1)
  {
   RealRoot[0] = -P[1]/P[0];
   ImagRoot[0] = 0.0;
  }

 // We used separate long double arrays in the function calls for interface reasons.
 // if N==0 all zeros are returned (we init RealRoot and ImagRoot to 0).
 // for(j=0; j<4; j++)Roots[j] = ComplexD(RealRoot[j], ImagRoot[j]);
 for(j=0; j<4; j++)RootsReal[j] = RealRoot[j];
 for(j=0; j<4; j++)RootsImag[j] = ImagRoot[j];
 return(N);
}

//---------------------------------------------------------------------------
// This function is the quadratic formula with P[0] = 1
// y = P[0]*x^2 + P[1]*x + P[2]
void QuadRoots(long double *P, long double *RealRoot, long double *ImagRoot)
{
 long double g;

 g = P[1]*P[1] - 4.0*P[2];
 if(fabsl(g) < ZERO_PLUS)g = 0.0;

 if(g >= 0.0)  // 2 real roots
  {
   RealRoot[0] = (-P[1] + sqrtl(g)) / 2.0;
   RealRoot[1] = (-P[1] - sqrtl(g)) / 2.0;
   ImagRoot[0] = 0.0;
   ImagRoot[1] = 0.0;
  }
 else // 2 complex roots
  {
   RealRoot[0] = -P[1] / 2.0;
   RealRoot[1] = -P[1] / 2.0;
   ImagRoot[0] = sqrtl(-g) / 2.0;
   ImagRoot[1] = -sqrtl(-g) / 2.0;
  }
}

//---------------------------------------------------------------------------
// This finds the roots of y = P0x^3 + P1x^2 + P2x+ P3   P[0] = 1
// The return value indicates the location of the largest positive root which is used by BiQuadRoots.
int CubicRoots(long double *P, long double *RealRoot, long double *ImagRoot)
{
 long double  s, t, b, c, d;

 s = P[1] / 3.0;
 b = (6.0*P[1]*P[1]*P[1] - 27.0*P[1]*P[2] + 81.0*P[3]) / 162.0;
 t = (P[1]*P[1] - 3.0*P[2]) / 9.0;
 c = t * t * t;
 d = 2.0*P[1]*P[1]*P[1] - 9.0*P[1]*P[2] + 27.0*P[3];
 d = d * d / 2916.0 - c;

 if(d > ZERO_PLUS) // 1 complex and 1 real root
  {
   d = powl( (sqrtl(d) + fabsl(b)), 1.0/3.0);
   if(d != 0.0)
    {
     if(b>0) b = -d;
     else b = d;
     c = t / b;
    }
   d = M_SQRT3_2 * (b-c);
   b = b + c;
   c = -b/2.0 - s;

   RealRoot[0] = c;
   ImagRoot[0] = -d;
   RealRoot[1] = c;
   ImagRoot[1] = d;
   RealRoot[2] = b-s;
   if( fabsl(RealRoot[2]) < ZERO_PLUS)RealRoot[2] = 0.0;
   ImagRoot[2] = 0.0;
   return(2); // Return 2 because it contains the real root.
  }

 else // d < 0.0  3 real roots
  {
   if(b == 0.0)d = M_PI_2 / 3.0; //  b can be as small as 1.0E-25
   else d = atanl(sqrtl(fabsl(d))/fabsl(b)) / 3.0;

   if(b < 0.0) b =  2.0 * sqrtl(fabsl(t));
   else        b = -2.0 * sqrtl(fabsl(t));

   c = cosl(d) * b;
   t = -M_SQRT3_2 * sinl(d) * b - 0.5 * c;

   RealRoot[0] = t - s;
   RealRoot[1] = c - s;
   RealRoot[2] = -(t + c + s);
   ImagRoot[0] = 0.0;
   ImagRoot[1] = 0.0;
   ImagRoot[2] = 0.0;

   if( fabsl(RealRoot[0]) < ZERO_PLUS)RealRoot[0] = 0.0;
   if( fabsl(RealRoot[1]) < ZERO_PLUS)RealRoot[1] = 0.0;
   if( fabsl(RealRoot[2]) < ZERO_PLUS)RealRoot[2] = 0.0;

   int MaxK = 0;
   if(RealRoot[1] > RealRoot[MaxK])MaxK = 1;
   if(RealRoot[2] > RealRoot[MaxK])MaxK = 2;
   return(MaxK);  // Return the index with the largest real root.
 }

}

//---------------------------------------------------------------------------
// This finds the roots of  y = P0x^4 + P1x^3 + P2x^2 + P3x + P4    P[0] = 1
// This function calls CubicRoots
void BiQuadRoots(long double *P, long double *RealRoot, long double *ImagRoot)
{
 int k, MaxK;
 long double a, b, c, d, e, g, P1, P3Limit;

 P1 = P[1];

 e = P[1]*0.25;
 d = P[1]*P[1]*0.1875;   // 0.1875 = 3/16
 b = P[3] + P[1]*P[1]*P[1]*0.125 - P[1]*P[2]*0.5;
 c = 256.0*P[4] + 16.0*P[1]*P[1]*P[2];
 c += -3.0*P[1]*P[1]*P[1]*P[1] - 64.0*P[1]*P[3];
 c *= 0.00390625;             // 0.00390625 = 1/256
 a = P[2] - P[1]*P[1]*0.375;  // 0.375 = 3/8

 P[1] = P[2]*0.5 - P[1]*P[1]*0.1875;  // 0.1875 = 3/16
 P[2] = 16.0*P[2]*P[2]  - 16.0*P1*P1*P[2] + 3.0*P1*P1*P1*P1;
 P[2] += -64.0*P[4] + 16.0*P1*P[3];
 P[2] *= 3.90625E-3;    // 3.90625E-3 = 1/256
 P[3] = -b*b*0.015625;  // 0.015625 = 1/64
 if(P[3] > 0.0)P[3] = 0.0; // Only numerical errors make P[3] > 0


/* Any inaccuracies in this algorithm are directly attributable to the magnitude of P[3]
 which takes on values of:  -1E30 < P[3] <= 0.0
 If P[3] = 0 we use the quadratic formula to get the roots (i.e. the 3rd root is at 0).
 Inaccuracies are caused in CubicRoots when P[3] is either huge or tiny because of the loss of
 significance that ocurrs there in the calculation of b and d. P[3] can also cause problems when it
 should have calculated to zero (just above) but it is instead ~ -1.0E-17 because of round off errors.
 Consequently, we need to determine whether a tiny P[3] is due to roundoff, or if it is legitimately small.
 It can legitimately have values of ~-1E-28 . When this happens, we assume P2 should also be small.
 We use the following criteria to test for a legitimate tiny P[3], which we must to send to CubicRoots */


 // if P[3] is tiny && legitimately tiny we use 0 as the threshold.
 // else P[3] must be more negative than roundoff errors cause.
 if(P[3] > ZERO_MINUS && fabs(P[2]) < 1.0E-6) P3Limit = 0.0;
 else P3Limit = ZERO_MINUS;

 if(P[3] < P3Limit)
  {
   MaxK = CubicRoots(P, RealRoot, ImagRoot);
   if(RealRoot[MaxK] > 0.0)  // MaxK is the index of the largest real root.
    {
     d = 4.0*RealRoot[MaxK];
     a += d;
     if(a*b < 0.0)P[1] = -sqrtl(d);
     else         P[1] = sqrtl(d);
     b = 0.5 * (a + b/P[1]);
     goto QUAD;
    }
  }

 if(P[2] < -1.0E-8)  // 2 sets of equal imag roots
  {
   b = sqrtl(fabsl(c));
   d = b + b - a;
   if(d > 0.0)P[1] = sqrtl(fabsl(d));
   else       P[1] = 0.0;
  }
 else
  {
   if(P[1] > 0.0)b =  2.0*sqrtl(fabsl(P[2])) + P[1];
   else          b = -2.0*sqrtl(fabsl(P[2])) + P[1];

   if(fabsl(b) < 10.0*ZERO_PLUS) // 4 equal real roots.  Was originally if(b == 0.0)
    {
     for(k=0; k<4; k++)
      {
       RealRoot[k] = -e;
       ImagRoot[k] = 0.0;
      }
     return;
    }
   else P[1] = 0.0;
  }

 // Calc the roots from two 2nd order polys and subtract e from the real part.
 QUAD:
 P[2] = c/b;
 QuadRoots(P, RealRoot, ImagRoot);

 P[1] = -P[1];
 P[2] = b;
 QuadRoots(P, RealRoot+2, ImagRoot+2);

 for(k=0; k<4; k++)RealRoot[k] -= e;

}