// poly.c    polyprt  polycopy polyval  polyderiv  polyintegrate
//           polyadd  polysub  polymul    polydiv
//           polyshrink  polymake  polyroots  polyfit
//           polyfindaroot  polyremovearoot 
//           polynomal stored  p[0] + p[1]*x + ...p[pwr]x^pwr

#include <stdio.h>
#include "simeq.h"

#undef max
#define max(a,b) ((a)>(b)?(a):(b))
#undef abs
#define abs(a) ((a)<0.0?(-(a)):(a))
 
// polyval.c  simple Horners evaluation y = p[0] + p[1]*x + ...p[pwr]x^pwr
double polyval(int pwr, double p[], double x)
{
  int i;
  double y = p[pwr]*x;
  for(i=pwr-1; i>0; i--) y = (p[i]+y)*x;
  return y + p[0];
}

// polycopy  copy polynomial pin to polynomial pout
void polycopy(int pwr, double pin[], double pout[])
{
  for(int i=0; i<=pwr; i++) // pwr is subscript of highest power
  {
    pout[i]= pin[i];
  }
} // end polycpy

//   polyprt   print a polynomial
void polyprt(int pwr, double p[])
{
  printf("p[0]= %f \n", p[0]);
  for(int i=1; i<pwr; i++)
  {
    printf("p[%d]= %f * x^%d \n", i, p[i], i);
  }
} // end polyprt

//   polyderiv   compute derivative coefficients
// pwr of pderiv is one less than pwr input, pderiv[] smaller
// run again on output to get next order derivative
void polyderiv(int pwr, double p[], double pderiv[])
{
  for(int i=1; i<pwr; i++)
  {
    pderiv[i-1] = p[i]*(double)(i);
  }
} // end polyderiv

// pwr of pintg is one more than pwr input, pintg[] bigger
// pintg[0] is returned 0.0, put in your own non zero integration constant
// run again on output to get next order integral
void polyintegrate(int pwr, double p[], double pintg[])
{
  pintg[0] = 0.0;
  for(int i=0; i<pwr; i++)
  {
    pintg[i+1] = p[i]/(double)(i+1);
  }
} // end polyintegrate


// add  polynomial p to polynomial q and place result in polynomial
// npr must be one entry integer arrays to be set
void polyadd(int npp, double p[], int npq, double q[],
             int npr[], double r[])
{
  npr[0] = max(npp,npq);
  for(int i=0; i<=npr[0]; i++)
    if(i>npp)      r[i] = q[i];
    else if(i>npq) r[i] = p[i];
    else           r[i] = p[i] + q[i];
} // end polyadd

// subtract polynomial q from polynomial p and place result in polynomial r 
// npr must be one entry integer arrays to be set
void polysub(int npp, double p[], int npq, double q[],
             int npr[], double r[])
{
  npr[0] = max(npp,npq);
  for(int i=0; i<=npr[0]; i++)
    if(i>npp)      r[i] = -q[i];
    else if(i>npq) r[i] = p[i];
    else           r[i] = p[i] - q[i];
} // end polysub

// multiply polynomial p by polynomial q and place result in polynomial r
// npp is highest power in p, npq is highest power in q
void polymul(int npp, double p[], int npq, double q[],
             int npr[], double r[])
{
  npr[0] = npp+npq;
  for(int i=0; i<=npr[0]; i++) r[i] = 0.0;
  for(int i=0; i<=npp; i++)
    for(int j=0; j<=npq; j++) r[i+j] += p[i]*q[j];
} // end polymul

// divide polynomial p by polynomial d (npp>npd) and place 
// quotient in polynomial q and remainder in polynomial r 
// npq and npr must be one entry integer arrays to be set
void polydiv(int npp, double p[], int npd, double d[],
             int npq[], double q[], int npr[], double r[])
{
  int k;
  npr[0] = npp;
  npq[0] = npp-npd;
  k = npp;
  for(int j=0; j<=npp; j++) r[j] = p[j];
  for(int i=npq[0]; i>=0; i--)
  {
    q[i] = r[k]/d[npd];
    for(int j=npd; j>=0; j--)
      r[k-npd+j] = r[k-npd+j] - q[i]*d[j];
    k--;
  }
} // end polydiv

// shrink size, pwr, such that p[pwr] not zero , return new pwr
int polyshrink(int pwr, double p[])
{
  int npp = pwr;
  while(p[npp] == 0.0 && npp > 0) npp = npp -1;
  return npp;
} // end polyshrink

// polymake make a polygon p from given roots roots x
void polymake(int nr, double x[], int npp[], double p[])
{
  npp[0] = nr;
  p[0] = -x[0];
  p[1] = 1.0;
  if(nr==1) return;
  for(int i=1; i<nr; i++)
  {
    p[i+1] = 0.0;
    for(int j=0; j<=i; j++)
    {
      p[i+1-j] = p[i-j]-p[i+1-j]*x[i];
    }
    p[0] = -p[0]*x[i];
  }
} // end polymake

// fit polynomial to data
void polyfit(int pwr, int n, double x[], double y[], double p[])
{
  int pn = pwr+1;
  double A[pn*pn];
  double Y[pn];
  double xpwr[pn];

  for(int i=0; i<pn; i++) // zero A and Y
  {
    for(int j=0; j<pn; j++)
    {
      A[i*pn+j] = 0.0;
    }
    Y[i] = 0.0;
  }
  for(int k=0; k<n; k++)
  {
    xpwr[0] = 1.0;
    for(int i=1; i<pn; i++) xpwr[i] = xpwr[i-1]*x[k];
    for(int i=0; i<pn; i++)
    {
      for(int j=0; j<pn; j++)
      {
        A[i*pn+j] = A[i*pn+j] + xpwr[i]*xpwr[j];
      }
      Y[i] = Y[i] + y[k]*xpwr[i];
    }
  }
  simeq(pn, A, Y, p);
  return;
} // end polyfit
// end poly.c

// polyfindaroot only read roots, one at a time
// then use  polyremovearoot, one at a time, until npp==1
// last root is x[nr] = -p[0]/p[1]  
double polyfindaroot (int npp, double p[])
{
  double root = 0.0;
  double vm, v, vp, vbest; // vbest is best so far
  double xm, x, xp, xbest; // trail root values
  double tol = 0.001;      // call it a root if abs(poluval) is smaller
  double del = 0.1;        // can get bigger and smaller 
  int maxcnt = 100;
  int cnt = 0;

  xbest = 1.0E50;
  vbest = 1.0E50;
  x = 0.0;
  while(vbest>tol)
  {
    v = polyval(npp, p, x);
    if(abs(v)<tol) return x;
    xm = x - del;
    vm = polyval(npp, p, xm);
    if(abs(vm)<tol) return xm;
    xp = x + del;
    vp = polyval(npp, p, xp);
    if(abs(vp)<tol) return xp;

    // determine direction to move
    if(vm<0.0 && v>0.0 || vm>0.0 && v<0.0) // crossing
    {
      del = 0.5*del;
      x = (xm + x)/2.0;
    }
    else if(vp<0.0 && v>0.0 || vp>0.0 && v<0.0) // crossing
    {
      del = 0.5*del;
      x = (xp + x)/2.0;
    }
    else
    {
      vp = abs(vp);
      v  = abs(v);
      vm = abs(vm);
      if(vm < v && v < vp) // vm best
      {
        del=1.5*del;
        if(del>3.7) del = 3.7;
        x=xm-del;
        if(vm<vbest) {vbest=vm; xbest=xm;} 
      }

      else if(vm > v && v > vp) // vp best
      {
        del=1.4*del;
        if(del>4.0) del = 4.0;
        x=xp+del;
        if(vp<vbest) {vbest=vp; xbest=xp;} 
      }

      else if(v < vm && v < vp) // v best
      {
        if(v<vbest) {vbest=v; xbest=x;} 
        del=0.5*del;
        if(vp < vm) {x = (x+xp)/2.0;}
        else        {x = (x+xm)/2.0;}
      }

      else if(vm < v && vp < v) // hump
      {
        if(vp < vm)
        {
          x=xp+del;
          if(vp<vbest) {vbest=vp; xbest=xp;}
        }
        else
        {
          x=xm-del;
          if(vm<vbest) {vbest=vm; xbest=xm;}
        }
      }

      else
      {
      }
    }
    cnt++;
    if(cnt>maxcnt) break;
    if(abs(vbest)<tol) break;
  } // end while

  return xbest;
} // end find a root

// reduce poly by root x, update npp return reduced poly in p
void polyremovearoot(double x, int npp[], double p[])
{
  double pd[] = {0.0, 1.0}; // 1.0*x - x = 0  x is a root
  pd[0] = -x;               // pd is divisor
  double qu[npp[0]+1];      // quotient
  double re[npp[0]+1];      // remainder should be near zero
  int pqu[1];
  int pre[1];
  polydiv(npp[0], p, 1, pd, pqu, qu, pre, re);
  npp[0] = npp[0]-1;
  for(int i=0; i<=npp[0]; i++) p[i] = qu[i];
} // end polyremovearoot

// polyroots  find nr real roots x of polynomial p with power npp
void polyroots(int npp, double p[], int nr, double x[])
{
  // TBD
} // end polyroots