#include "machdefs.h"
#include "poly.h"
#include "linfit.h"

//++
// Class	Poly
// Lib  	Outils++ 
// include	poly.h
//
//	Classe de calcul sur polynômes à une variable.
//--

//++
// Links	Parents
// Vector
//--

//++
// Titre	Constructeurs
//--


//////////////////////////////////////////////////////////////////////////
//++
// Poly::Poly(int degre=0)
//
//	Crée un nouveau polynôme, en allouant de la place pour 
//	le degré spécifié. 
//--

Poly::Poly(int degre)
: Vector(degre+1), dirty(0), deg(0)
{
  END_CONSTRUCTOR
}

//++
Poly::Poly(Poly const& a)
//
//	Constructeur par copie.
//--
:Vector(a), dirty(a.dirty), deg(a.deg)
{
  END_CONSTRUCTOR
}


void Poly::UpdateDeg() const
{
  int i = NElts()-1;
  while (Element(i) == 0 && i>0) i--;

  (int&) deg = i;          // bientot mutable dans ANSI C++ 
  (int&) dirty = 0;
}

//++
// Titre	Méthodes
//--

//++
// double& Poly::operator[](int i)
//	Permet d'accéder au coefficient de degré i (avec version 
//	const).
//--

//++
double Poly::operator()(double x) const
//
//	Calcule la valeur du polynôme au point x.
//--
{
  UpdateDegIfDirty();
  double res = Element(deg);
  for (int i=deg-1; i>=0; i--) {
    res *= x;
    res += Element(i);
  }
  return res;
}

//++
void Poly::Derivate()
//
//	Remplace le polynôme par le polynôme dérivé.
//--
{
  UpdateDegIfDirty();
  if (deg == 0) { Element(0) = 0; return;}
  for (int i=1; i<=deg; i++)
    Element(i-1) = Element(i)*i;
  Element(deg) = 0;
  deg--;
}


//++
void Poly::Derivate(Poly& der) const
//
//	Retourne dans der le polynôme dérivé.
//--
{
  UpdateDegIfDirty();
  der.Realloc(deg);
//  der.Zero();    // on sait que Realloc met a zero le reste.
  for (int i=1; i<=deg; i++)
    der.Element(i-1) = Element(i)*i;
}


//++
int Poly::Roots(Vector& roots) const
//
//	Retourne dans roots les racines réelles, si on sait
//	les calculer. Retourne le nombre de racines.
//--
{
  UpdateDegIfDirty();

  switch (deg)
    {
    case 0 :           // degre 0
      return 0;
    case 1 :           // degre 1
      roots.Realloc(1);
      return Root1(roots(0));
    case 2 :           // degre 2
      roots.Realloc(2);
      return Root2(roots(0),roots(1));
    default :
      THROW(parmErr);
    }
}


//++
int Poly::Root1(double& r) const
//
//	Seulement si le polynôme est de degré 1: retourne
//	la racine dans "r". Retourne 1 (nombre de racines).
//--
{
  UpdateDegIfDirty();
  if (deg != 1) THROW(sizeMismatchErr);

  if (Element(1) == 0) return 0;
  r = -Element(0)/Element(1);
  return 1;
}

//++
int Poly::Root2(double& r1, double& r2) const
//
//	Seulement si le polynôme est de degre 2: retourne
//	les racines dans "r1" et "r2". Retourne 0, 1 ou 2
//	(nombre de racines).
//--
{
  UpdateDegIfDirty();
  if (deg != 2) THROW(sizeMismatchErr);

  double delta = Element(1)*Element(1) - 4*Element(0)*Element(2);
  if (delta < 0) return 0;
  if (delta == 0) {
    r1 = r2 = -Element(1)/2/Element(0);
    return 1;
  }
  r1 = (-Element(1) - sqrt(delta))/2/Element(0);
  r2 = (-Element(1) + sqrt(delta))/2/Element(0);
  return 2;
}

//++
Poly& Poly::operator = (Poly const& a)
//
//	Opérateur d'affectation.
//--
{
  if (this == &a) return *this;
  Vector::operator=(a);

  UpdateDeg();
  return *this;
}

//++
// Titres	Opérations sur polynômes
//--

//++
Poly& Poly::operator += (Poly const& b)
//
//--
{
  UpdateDegIfDirty();
  b.UpdateDegIfDirty();

  if (b.deg > deg) Realloc(b.deg);

  int n = (deg > b.deg) ? deg : b.deg;
  for (int i=0; i<=n; i++) Element(i) += b.Element(i);

  UpdateDeg();
  return *this;
}

//++
Poly& Poly::operator -= (Poly const& b)
//
//--
{
  UpdateDegIfDirty();
  b.UpdateDegIfDirty();

  if (b.deg > deg) Realloc(b.deg);

  int n = (deg > b.deg) ? deg : b.deg;
  for (int i=0; i<=n; i++) Element(i) -= b.Element(i);

  UpdateDeg();
  return *this;
}

//++
Poly& Poly::operator *= (double a)
//
//--
{
  UpdateDegIfDirty();
  for (int i=0; i<=deg; i++) Element(i) *= a;
  return *this;
}

//++
Poly Poly::Mult(Poly const& b) const
//
//--
{
Poly c(deg + b.deg);
  UpdateDegIfDirty();
  b.UpdateDegIfDirty();

  c.deg = deg + b.deg;

  for (int i=0; i<=c.deg; i++) {
    c[i] = 0;
    int kmin = (i <= deg) ? 0 : i - deg;
    int kmax = (i <= deg) ? i : deg;
    for (int k=kmin; k<=kmax; k++)
      c[i] += (*this)[k] * b[i-k];
  }
return c;
}

//++
void Poly::Print(ostream& s) const
//
//	Impresssion.
//--
{
  UpdateDegIfDirty();
  int nz=0;
  for (int i = deg; i>=0; i--) {
    if ((*this)[i] != 0) {
      nz = 1;
      if (i < deg && (*this)[i] > 0) s << "+";
      s << (*this)[i];
      if (i == 1) s << "*X ";
      if (i > 1) s << "*X^" << i << " ";
    }
  }
  if (!nz) s << " 0 ";
}

//++
double Poly::Fit(Vector const& x, Vector const& y, int degre)
//
//	Ajustement polynomial par moindre carrés. Un polynôme de
//	degré "degre" est ajusté sur les données "x" et "y", et stocké dans
//	l'objet courant. Retourne le chi2.
//--
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);

  Realloc(degre);

  Matrix a(degre+1, n);

  for (int c=0; c<n; c++) {
    double xpow = 1.0;
    for (int l=0; l<=degre; l++) {
      a(l,c) = xpow;
      xpow *= x(c);
    }
  }

  LinFitter lf;
  double rc = lf.LinFit(a,y,(Vector&)*this);
  UpdateDeg();
  return rc;
}

//++
double Poly::Fit(Vector const& x, Vector const& y, Vector const& erry2, int degre,
                 Vector& errCoef)
//
//	Ajustement polynomial par moindre carrés. Un polynôme de
//	degré est ajusté sur les données x et y, et stocké dans
//	l'objet courant. erry2 contient le carre des erreurs sur y. 
//	Retourne le chi2.
//--
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  if (n != erry2.NElts()) THROW(sizeMismatchErr);

  Realloc(degre);
  errCoef.Realloc(degre+1);

  Matrix a(degre+1, n);

  for (int c=0; c<n; c++) {
    double xpow = 1.0;
    for (int l=0; l<=degre; l++) {
      a(l,c) = xpow;
      xpow *= x(c);
    }
  }

  LinFitter lf;
  double rc = lf.LinFit(a,y,erry2,(Vector&)*this,errCoef);
  UpdateDeg();
  return rc;
}


//++
// Poly Poly::power(int n) const
//
//	Retourne le polynôme à la puissance n
//--

Poly Poly::power(int n) const // a accelerer !!!
{
  if (n < 0) THROW(rangeCheckErr);
  if (n == 0) { Poly r(0); r[0] = 1; return r;}
  if (n == 1) { return *this; }
  return *this * power(n-1);
}

//++
Poly Poly::operator() (Poly const& b) const
//
//	Substitution d'un polynôme dans un autre.
//--
{
  Poly c(b.Degre()*Degre());
  for (int i=0; i<= Degre(); i++)
    c += (*this)[i] * b.power(i);
  return c;
}


//////////////////////////////////////////////////////////////////////////
void ObjFileIO<Poly>::ReadSelf(PInPersist& is)
{
if(dobj==NULL) dobj=new Poly;
int_4 dg;
is >> dg;
dobj->Realloc(dg,true);
is >> *((Vector *) dobj);
dobj->UpdateDeg();
}

void ObjFileIO<Poly>::WriteSelf(POutPersist& os) const
{
if(dobj == NULL) return;
dobj->UpdateDegIfDirty();
dobj->Realloc(dobj->deg,true);
os << dobj->deg;
os << *((Vector *) dobj);
}

//////////////////////////////////////////////////////////////////////////
int binomial(int n, int p)
{
  int c = 1;
  for (int i=n-p+1; i<=n; i++) c *= i;
  for (int j=2; j<=p; j++) c /= j;
  return c;
}

//////////////////////////////////////////////////////////////////////////
// ******************* POLY 2 VARIABLES ******************
//++
// Class	Poly2
// Lib  	Outils++ 
// include	poly.h
//
//	Classe de calcul sur polynômes à deux variables.
//--

//++
// Links	Parents
// Vector
//--

//++
// Titre	Constructeurs
//--

//++
Poly2::Poly2(int degreX, int degreY)
//
//	Crée un polynôme de degrés partiels degreX et degreY.
//--
:Vector((degreX+1)*(degreY+1)), dirty(0),
 maxDegX(degreX), maxDegY(degreY), degX(0), degY(0), deg(0)
{
  END_CONSTRUCTOR
}

//++
Poly2::Poly2(Poly const& polX, Poly const& polY)
//
//	Crée un polynôme à deux variables comme produit
//	de deux polynômes à une variable, p2(x,y)=px(x)py(y)
//--
:Vector((polX.Degre()+1)*(polY.Degre()+1)), dirty(0),
 maxDegX(polX.Degre()), maxDegY(polY.Degre()),
 degX(polX.Degre()), degY(polY.Degre()), deg(degX+degY)
{
  for (int i=0; i<=degX; i++)
    for (int j=0; j<=degY; j++)
      Coef(i,j) = polX[i]*polY[j];
  END_CONSTRUCTOR
}

//++
Poly2::Poly2(Poly2 const& a)
//
//	Constructeur par copie.
//--
:Vector(a), dirty(a.dirty),
 maxDegX(a.maxDegX), maxDegY(a.maxDegY),
 degX(a.degX), degY(a.degY), deg(a.deg)
{
  END_CONSTRUCTOR
}


//++
// Titre	Méthodes
//--

//++
Poly2& Poly2::operator = (Poly2 const& a)
//
//	Opérateur d'affectation.
//--
{
  if (this == &a) return *this;
  if (maxDegX < a.DegX() || maxDegY < a.DegY())
    Realloc(a.DegX(), a.DegY());


  for (int i=0; i<= maxDegX; i++)
    for (int j=0; j<= maxDegY; j++)
      Coef(i,j) = a.Coef(i,j);

  UpdateDeg();
  return *this;
}

//++
void Poly2::Realloc(int degreX, int degreY)
//
//	Redimensionne le polynôme comme etant un 
//	polynôme de degrés partiels degreX et degreY.
//--
{
  UpdateDegIfDirty();
  Poly2 tmp(*this);
  Vector::Realloc((degreX+1)*(degreY+1));
  Reset();
  maxDegX = degreX;
  maxDegY = degreY;

// Attention - Reza 30/09/99
// il faut prendre le min en degre du polynome de depart et le nouveau
  int cdegx = (tmp.degX < degreX) ? tmp.degX : degreX;
  int cdegy = (tmp.degY < degreY) ? tmp.degY : degreY;
  for (int i=0; i<= cdegx; i++)
    for (int j=0; j<= cdegy; j++)
      Coef(i,j) = tmp.Coef(i,j);
}


void Poly2::UpdateDeg() const
{
  (int&)degX=(int&)degY=(int&)deg=0;

  for (int dx=0; dx<=maxDegX; dx++)
    for (int dy=0; dy<=maxDegY; dy++)
      if (Coef(dx,dy) != 0) {
        if (dx > degX) (int&)degX = dx;
        if (dy > degY) (int&)degY = dy;
        if (dx+dy > deg) (int&)deg = dx+dy;
      }

  (int&)dirty = 0;
}

//++
// int Poly2::DegX() const
//	Degré partiel en X.
// int Poly2::DegY() const
//	Degré partiel en Y
// int Poly2::MaxDegX() const
//	Degré partiel maximum (alloué) en X
// int Poly2::MaxDegY() const
//	Degré partiel maximum (alloué) en Y
// int Poly2::Deg()  const
//	Degré total.
//--

//++
// double& Poly2::Coef(int dx, int dy)
//	Retourne le coefficient de x^dx y^dy
//	(avec aussi version const).
//--

//++
double Poly2::operator()(double x, double y) const
//
//	Retourne la valeur en (x,y).
//--
{
  UpdateDegIfDirty();
  double res = 0;
  double xPow = 1;
  for (int dx=0; dx<=maxDegX; dx++) {
    double yPow = 1;
    for (int dy=0; dy<=maxDegY; dy++) {
      res += Coef(dx,dy) * xPow * yPow;
      yPow *= y;
    }
    xPow *= x;
  }
  return res;
}

//++
double Poly2::Fit(Vector const& x, Vector const& y, Vector const& z,
                  int degreX, int degreY)
//
//	Ajustement par moindre carrés z = P(x,y), degrés partiels imposés.
//--
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  if (n != z.NElts()) THROW(sizeMismatchErr);

  Realloc(degreX, degreY);

  Matrix a((degreX+1)*(degreY+1), n);

  for (int c=0; c<n; c++) {
    double xPow = 1.0;
    for (int dx = 0; dx <= degreX; dx++) {
      double yPow = 1.0;
      for (int dy = 0; dy <= degreY; dy++) {
        a(IndCoef(dx,dy), c) = xPow*yPow;
        yPow *= y(c);
      }
      xPow *= x(c);
    }
  }

  LinFitter lf;
  double rc = lf.LinFit(a,z,(Vector&)*this);
  UpdateDeg();
  return rc;
}


//++
double Poly2::Fit(Vector const& x, Vector const& y, Vector const& z,
                  Vector const& errz2, int degreX, int degreY,
                  Vector& errCoef)
//
//	Ajustement par moindre carrés z = P(x,y), degrés partiels imposés,
//	et erreurs^2 sur z dans errz2.
//--
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  if (n != z.NElts()) THROW(sizeMismatchErr);
  if (n != errz2.NElts()) THROW(sizeMismatchErr);

  Realloc(degreX, degreY);
  errCoef.Realloc((degreX+1)*(degreY+1));

  Matrix a((degreX+1)*(degreY+1), n);

  for (int c=0; c<n; c++) {
    double xPow = 1.0;
    for (int dx = 0; dx <= degreX; dx++) {
      double yPow = 1.0;
      for (int dy = 0; dy <= degreY; dy++) {
        a(IndCoef(dx,dy), c) = xPow*yPow;
        yPow *= y(c);
      }
      xPow *= x(c);
    }
  }

  LinFitter lf;
  double rc = lf.LinFit(a,z,errz2,(Vector&)*this,errCoef);
  UpdateDeg();
  return rc;
}

//++
double Poly2::Fit(Vector const& x, Vector const& y, Vector const& z,
                  int degre)
//
//	Ajustement par moindre carrés z = P(x,y), degré total imposé.
//--
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  if (n != z.NElts()) THROW(sizeMismatchErr);

  Realloc(degre, degre);   // certains vaudront 0, impose.

  Matrix a((degre+1)*(degre+2)/2, n);
  Vector cf((degre+1)*(degre+2)/2);
#define C_INDEX(i,j) ((i) + (j)*(2*degre+3-(j))/2)

  for (int c=0; c<n; c++) {
    double xPow = 1.0;
    for (int dx = 0; dx <= degre; dx++) {
      double yPow = 1.0;
      for (int dy = 0; dy <= degre; dy++) {
        if (dy+dx <= degre)
          a(C_INDEX(dx,dy), c) = xPow*yPow;
        yPow *= y(c);
      }
      xPow *= x(c);
    }
  }

  LinFitter lf;
  double rc = lf.LinFit(a,z,cf);

  for (int dx = 0; dx <= degre; dx++)
    for (int dy = 0; dy <= degre; dy++)
      if (dx+dy <= degre)
        Coef(dx,dy) = cf(C_INDEX(dx,dy));
      else
        Coef(dx,dy) = 0;

  UpdateDeg();
  return rc;
}


//++
double Poly2::Fit(Vector const& x, Vector const& y, Vector const& z,
                  Vector const& errz2, int degre,
                  Vector& errCoef)
//
//	Ajustement par moindre carrés z = P(x,y), degré total imposé,
//	et erreurs^2 sur z dans errz2.
//--
{
  int n = x.NElts();
  if (n != y.NElts()) THROW(sizeMismatchErr);
  if (n != z.NElts()) THROW(sizeMismatchErr);
  if (n != errz2.NElts()) THROW(sizeMismatchErr);

  Realloc(degre, degre);
  errCoef.Realloc((degre+1)*(degre+1));
#define C_INDEX(i,j) ((i) + (j)*(2*degre+3-(j))/2)

  Matrix a((degre+1)*(degre+2)/2, n);
  Vector cf((degre+1)*(degre+2)/2);
  Vector ecf((degre+1)*(degre+2)/2);

  for (int c=0; c<n; c++) {
    double xPow = 1.0;
    for (int dx = 0; dx <= degre; dx++) {
      double yPow = 1.0;
      for (int dy = 0; dy <= degre; dy++) {
        if (dy+dx <= degre)
          a(C_INDEX(dx,dy), c) = xPow*yPow;
        yPow *= y(c);
      }
      xPow *= x(c);
    }
  }

  LinFitter lf;
  double rc = lf.LinFit(a,z,errz2,cf,ecf);


  for (int dx = 0; dx <= degre; dx++)
    for (int dy = 0; dy <= degre; dy++)
      if (dx+dy <= degre) {
        Coef(dx,dy) = cf(C_INDEX(dx,dy));
        errCoef(IndCoef(dx,dy)) = ecf(C_INDEX(dx,dy));
      } else {
        Coef(dx,dy) = 0;
        errCoef(IndCoef(dx,dy)) = 0;
      }
  UpdateDeg();
  return rc;
}

//++
void Poly2::Print(ostream& s) const
//
//	Impression sur stream s.
//--
{
  UpdateDegIfDirty();
  int nz=0;
  int notfirst=0;
  for (int dx = degX; dx>=0; dx--)
    for (int dy= degY; dy>=0; dy--) {
      double c = Coef(dx,dy);
      if (c != 0) {
        nz = 1;
        if (notfirst && c > 0) {
          s << "+";
          notfirst = 1;
        }
        s << c << " ";
        if (dx == 1) s << "* X ";
        if (dx > 1) s << "* X^" << dx << " ";
        if (dy == 1) s << "* Y ";
        if (dy > 1) s << "* Y^" << dy << " ";
        s << endl;
    }
  }
  if (!nz) s << " 0 ";
}

//++
// Titre	Opérations
//--

//++
Poly2& Poly2::operator += (Poly2 const& b)
//
//--
{
  if (maxDegX < b.DegX() || maxDegY < b.DegY())
    Realloc(b.DegX(),b.DegY());

  UpdateDegIfDirty();

  int mx = b.DegX();
  int my = b.DegY();
  for (int i=0; i<= mx; i++)
    for (int j=0; j<= my; j++)
      Coef(i,j) += b.Coef(i,j);

  UpdateDeg();
  return *this;
}

//++
Poly2& Poly2::operator -= (Poly2 const& b)
//
//--
{
  if (maxDegX < b.DegX() || maxDegY < b.DegY())
    Realloc(b.DegX(),b.DegY());

  UpdateDegIfDirty();

  int mx = b.DegX();
  int my = b.DegY();
  for (int i=0; i<= mx; i++)
    for (int j=0; j<= my; j++)
      Coef(i,j) -= b.Coef(i,j);

  UpdateDeg();
  return *this;
}

//++
Poly2& Poly2::operator *= (double a)
//
//--
{
  for (int i=0; i<NElts(); i++)
    Element(i) *= a;

  return *this;
}

//++
Poly2 Poly2::Mult(Poly2 const& b) const
//
//--
{
  Poly2 c(DegX() + b.DegX(), DegY() + b.DegY());
  UpdateDegIfDirty();
  b.UpdateDegIfDirty();

  for (int i=0; i<=DegX(); i++)
    for (int j=0; j<=DegY(); j++)
      for (int k=0; k<=b.DegX(); k++)
        for (int l=0; l<=b.DegY(); l++)
          c.Coef(i+k,j+l) += Coef(i,j)*b.Coef(k,l);
  return c;
}

//++
Poly2 Poly2::power(int n) const
//
//	Calcule le polynôme P(x,y)^n
//--
{
  if (n < 0) THROW(rangeCheckErr);
  if (n == 0) { Poly2 r(0); r.Coef(0,0) = 1; return r;}
  if (n == 1) { return *this; }
  return *this * power(n-1);
}


//++
Poly2 Poly2::operator() (Poly const& a, Poly const& b) const
//
//	Substitution de deux polynômes en X et Y,
//	P2(pa(x), pb(y)).
//--
{
  UpdateDegIfDirty();
  Poly2 c(maxDegX*a.Degre(), maxDegY*b.Degre());

  for (int i=0; i<= degX; i++)
    for (int j=0; j<= degY; j++) {
      Poly2 d(a.power(i), b.power(j));
      c += Coef(i,j) * d;
    }

  return c;
}

//++
Poly2 Poly::operator() (Poly2 const& a) const
//
//	Substitution d'un polynôme à deux variables dans
//	un polynôme à une variable, P(P2(x,y)).
//--
{
  Poly2 c(a.MaxDegX()*Degre(), a.MaxDegY()*Degre());

  for (int i=0; i<= Degre(); i++)
    c += (*this)[i] * a.power(i);
  return c;
}

//////////////////////////////////////////////////////////////////////////
void ObjFileIO<Poly2>::ReadSelf(PInPersist& is)
{
if(dobj==NULL) dobj=new Poly2;
int_4 dgx, dgy;
is >> dgx >> dgy;
dobj->Realloc(dgx,dgy);
is >> *((Vector *) dobj);
dobj->UpdateDeg();
}

void ObjFileIO<Poly2>::WriteSelf(POutPersist& os) const
{
if(dobj == NULL) return;
os << dobj->maxDegX << dobj->maxDegY;
os << *((Vector *) dobj);
}


//////////////////////////////////////////////////////////////////////////
#ifdef __CXX_PRAGMA_TEMPLATES__
#pragma define_template ObjFileIO<Poly>
#pragma define_template ObjFileIO<Poly2>
#endif

#if defined(ANSI_TEMPLATES) || defined(GNU_TEMPLATES)
template class ObjFileIO<Poly>;
template class ObjFileIO<Poly2>;
#endif
