PolynomialSolve.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#include "Base/Common/W32Compat.hh"
#include <bias_config.h>
#include "PolynomialSolve.hh"
#include "Minpack.hh"

#ifdef BIAS_HAVE_GSL
#include <gsl/gsl_poly.h>
#include <gsl/gsl_errno.h>
#endif

#include <Base/Common/BIASpragma.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <MathAlgo/Laguerre.hh>
#include <MathAlgo/SVD.hh>
#include <iostream>
#include <algorithm>

using namespace BIAS;
using namespace std;

#define DOUBLE_MIN 1e-6
#define PS_DOUBLE_EPSILON DBL_EPSILON

int PolynomialSolve::Solve(const vector<POLYNOMIALSOLVE_TYPE>& ocoeff,
                           vector<POLYNOMIALSOLVE_TYPE>& sol)
{
 
  BIASASSERT(!ocoeff.empty());

  vector<POLYNOMIALSOLVE_TYPE> coeff = ocoeff;
  CheckCoefficients(coeff);

  int deg=(int)coeff.size()-1;
  int num=deg;


  sol.clear();
  switch (deg){
    // zero order 
  case -1: sol.push_back(0);
    return 1;
  case 0: return 0; // cannot have solution, 0=0 is not possible here
  case 1:
    num = Linear(coeff, sol);
    break;
  case 2:
    num = Quadratic(coeff, sol);
    break;
  case 3:
    num = Cubic(coeff, sol);
    break;
  case 4:
    num = Quartic(coeff, sol);
    break;
  default:
    return Numeric(ocoeff, sol);
    break;
  }
  NonLinearRefine(coeff,sol);

  return num;
}

int PolynomialSolve::Solve(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                           vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
{
  int deg=(int)coeff.size()-1;
  int num=deg;
  BIASASSERT(deg>=0);
  switch (deg){
  case 0:
    sol.clear();
    num=0;
    break;
  case 1:
    num = Linear(coeff, sol);
    break;
  case 2:
    num = Quadratic(coeff, sol);
    break;
  case 3:
    num = Cubic(coeff, sol);
    break;
  default:
    num = Numeric(coeff, sol);
    break;
  }
  return num;
}
 

int PolynomialSolve::Numeric(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                             vector<POLYNOMIALSOLVE_TYPE>& sol)
{
  vector<complex<POLYNOMIALSOLVE_TYPE> > csol;
  int res=Numeric(coeff, csol), num=0;
  sol.clear();
  sol.reserve(coeff.size()-1);
  if (res>0){
    for (int i=0; i<res; i++){
      if (csol[i].imag()==0.0){
        sol.push_back(csol[i].real());
      }
    }
    num=sol.size();
  } else {
    num=res;
  }
  return num;
}

int PolynomialSolve::Numeric(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                             vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
{
  int deg=(int)coeff.size()-1;
#ifdef BIAD_DEBUG
  if (coeff[deg]==0.0){
    BIASERR("leading coefficient is zero, aborting"););
  }
#endif
  BIASASSERT(coeff[deg]!=0.0);
  BIASASSERT(deg>=0);

  int num=0;
#ifdef BIAS_HAVE_GSL
  
   
  if (deg>0){
#ifdef BIAS_DEBUG
    for (unsigned i=0; i<coeff.size(); i++){
      BIASCDOUT(D_PS_GSL, setw(3)<<i<<"th coeff: "<<coeff[i]<<endl);
    }
#endif
    size_t n=deg+1;
    double *a=new double[n];
    double *z=new double[2*deg];
    gsl_poly_complex_workspace *w=gsl_poly_complex_workspace_alloc(n);

    for (unsigned i=0; i<n; i++) a[i]=coeff[i];
    int res = gsl_poly_complex_solve(a, n, w, z);
    gsl_poly_complex_workspace_free(w);
    if (res==GSL_SUCCESS){
      sol.reserve(n-1);
      sol.clear();
      n--;
      for (unsigned i=0; i<n; i++){
        sol.push_back(complex<double>(z[i<<1], z[(i<<1)+1]));
        BIASCDOUT(D_PS_GSL, setw(3)<<i<<"th solution : "<< *sol.rbegin() 
                  <<"\tpoly("<<z[i<<1]<<") = "
                  <<gsl_poly_eval(a, n+1, z[i<<1]) << endl);
      }
      delete a;
      delete z;
      num=n;
    } else {
      sol.clear();
      num=-1;
    }
  } else {
    sol.clear();
    num=0;
  }
#else
  
//   deg=0;
//   sol.clear();
//   BIASERR("gsl needed for PolynomialSolve::Numeric");
//   num=-1;
  if(deg>0)
  {
    LaguerreSolver l;
    l.Solve(coeff,sol);
    //find number of real solutions
    num = 0;
 //   cout<<endl<<"xxxxxxxxxxxxxx size of sol -> "<<sol.size()<<endl;
    for (unsigned int i=0; i<sol.size(); i++)
    {
     // cout<<sol[i]<<endl;
      if (Equal(sol[i].imag(),0.0,10e-11))
      {
        sol[i]=std::complex< double>(sol[i].real(),0.0);
        num++; 
      }
    }
  }
  else
  {
   
    sol.clear();
    num=0;
  }
  
#endif
  return num;
}





int PolynomialSolve::Analytic(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                              vector<POLYNOMIALSOLVE_TYPE>& sol)
{
  int deg=(int)coeff.size()-1;
  BIASASSERT(deg<=4);
  int num=0;
  switch (deg){
  case 0:
    sol.clear();
    num=0;
    break;
  case 1:
    num = Linear(coeff, sol);
    break;
  case 2:
    num = Quadratic(coeff, sol);
    break;
  case 3:
    num = Cubic(coeff, sol);
    break;
  case 4:
    num = Quartic(coeff, sol);
    break;
  default:
    BIASERR("something strange happened");
    BIASABORT;
    break;
  }
  return num;
}

int PolynomialSolve::Analytic(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                              vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
{
  int deg=(int)coeff.size()-1;
  BIASASSERT(deg<=3);
  int num=0;
  switch (deg){
  case 0:
    sol.clear();
    num=0;
    break;
  case 1:
    num = Linear(coeff, sol);
    break;
  case 2:
    num = Quadratic(coeff, sol);
    break;
  case 3:
    num = Cubic(coeff, sol);
    break;
  default:
    BIASERR("something strange happened");
    BIASABORT;
    break;
  }
  return num;
}







int PolynomialSolve::Linear(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                            vector<POLYNOMIALSOLVE_TYPE>& sol)
{
  int deg=(int)coeff.size()-1;
#ifdef BIAS_DEBUG
  if(deg!=1 || coeff[deg]==0.0){
    BIASERR("coeff is not a polynom of first order");
  }
#endif
  BIASASSERT(deg==1);
  BIASASSERT(coeff[deg]!=0.0);

  sol.resize(deg);
  // degree 1 has one solution
  sol[0] = -coeff[0] / coeff[1];

  return 1; 
}

int PolynomialSolve::Linear(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                            vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
{
  vector<POLYNOMIALSOLVE_TYPE> rsol;
  int deg=Linear(coeff, rsol);
  sol.resize(deg);
  if (deg!=0)
    sol[0] = complex<POLYNOMIALSOLVE_TYPE>(rsol[0], 0.0);
  return 1; 
}




int PolynomialSolve::Quadratic(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                               vector<POLYNOMIALSOLVE_TYPE>& sol)
{

  int deg=(int)coeff.size()-1;
#ifdef BIAS_DEBUG
  if(deg!=2 || coeff[deg]==0.0){
    BIASERR("coeff is not a polynom of second order");
  }
#endif
  BIASASSERT(deg==2);
  BIASASSERT(coeff[deg]!=0.0);
  int num=0;
  
  sol.clear();
  sol.reserve(deg);
#ifdef BIAS_HAVE_GSL
  POLYNOMIALSOLVE_TYPE s[2];
  num=gsl_poly_solve_quadratic(coeff[2], coeff[1],coeff[0],&(s[0]),&(s[1]));
  if (num==2 && s[0]==s[1]) num=1;
  for (int i=0; i<num; i++) sol.push_back(s[i]);
#else
  // compute normal coefficients
  POLYNOMIALSOLVE_TYPE q1 = coeff[1] / (coeff[2] * 2.0);
  POLYNOMIALSOLVE_TYPE q0 = coeff[0] /  coeff[2];

  // get discriminant and accuracy
  POLYNOMIALSOLVE_TYPE discriminant = q1*q1 - q0;
  POLYNOMIALSOLVE_TYPE epsilon_d = 
      (fabs(discriminant)+7.0*q1*q1+3.0*fabs(q0)) * DBL_EPSILON;

  // check whether there is no solution
  if (discriminant < -epsilon_d) {
    num = 0;
  } else if (discriminant <= epsilon_d) { // we have solutions, check how many
    // within our accuracy, we have two identical solutions !
    sol.push_back(-q1);
    num = 1;
  } else {
    // we have two distinct roots in IR
    // compute first one according to sign of q1:
    POLYNOMIALSOLVE_TYPE FirstRoot = 
      ((q1>0.0)?-sqrt(discriminant):sqrt(discriminant)) - q1;
    sol.push_back(FirstRoot);
    
    // avoid cancellation when calculating second root:
    sol.push_back(q0/FirstRoot);
    num=2; 
  }
#endif
  return num;
}

int PolynomialSolve::Quadratic(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                               vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
{
  int deg=(int)coeff.size()-1;
#ifdef BIAS_DEBUG
  if(deg!=2 || coeff[deg]==0.0){
    BIASERR("coeff is not a polynom of second order");
  }
#endif
  BIASASSERT(deg==2);
  BIASASSERT(coeff[deg]!=0.0);
  int num=0;
  
  sol.resize(deg);
#ifdef BIAS_HAVE_GSL
  double z[4];
  num = gsl_poly_complex_solve_quadratic(coeff[2], coeff[1], coeff[0],
                                         (gsl_complex*)&(z[0]),
                                         (gsl_complex*)&(z[2]));
  sol[0]=complex<POLYNOMIALSOLVE_TYPE>(z[0], z[1]);
  sol[1]=complex<POLYNOMIALSOLVE_TYPE>(z[2], z[3]);
#else
  BIASERR("Quadratic(vector<double>, vbector<complex<double>>) only "
          <<"implemented with gsl");
  num = -1;
#endif
  return num;
}





int PolynomialSolve::Cubic(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                           std::vector<POLYNOMIALSOLVE_TYPE>& sol)
{
#ifdef BIAS_DEBUG
  int deg=(int)coeff.size()-1;
  if(deg!=3 || coeff[deg]==0.0){
    BIASERR("coeff is not a polynom of third order");
  }
#endif
  BIASASSERT(deg==3);
  BIASASSERT(coeff[deg]!=0.0);
  int num=0; 
  sol.reserve(3);

#ifdef BIAS_HAVE_GSL
  POLYNOMIALSOLVE_TYPE msol[3];
  num = gsl_poly_solve_cubic(coeff[2]/coeff[3], coeff[1]/coeff[3],
                             coeff[0]/coeff[3], &(msol[0]), 
                             &(msol[1]), &(msol[2]));
  for (int i=0; i<num; i++)
    sol.push_back(msol[i]);
#else
  // normalize to first coefficient, i.e. a3=1
  POLYNOMIALSOLVE_TYPE a0 = coeff[0] / coeff[3];
  POLYNOMIALSOLVE_TYPE a1 = coeff[1] / coeff[3];
    
  // substitute x=z-a2/3
  // quadratic term goes away
  POLYNOMIALSOLVE_TYPE a2 = coeff[2] / (coeff[3] * 3.0);

  POLYNOMIALSOLVE_TYPE q = a1 / 3.0 - a2*a2;
  POLYNOMIALSOLVE_TYPE r = ( a1*a2 - a0 ) * 0.5  - a2*a2*a2 ;

  // see if we can factorize to z==0
  if (fabs(r)<PS_DOUBLE_EPSILON)    {
    // yes, its the easy way
    // rest is z^2 + 3q == 0
    if (q>-PS_DOUBLE_EPSILON) {
        // no other solutions apart from z==0
        // (or z==0 is a triple zero crossing)
        // back-substitute to x
        sol.push_back(-a2);
        num = 1;
    } else {
        POLYNOMIALSOLVE_TYPE t = sqrt( -3.0 * q );
        sol.resize(3);
        sol[0] = -a2 - t;
        sol[1] = -a2;
        sol[2] = -a2 + t;
        num = 3;
    }
  } else {
    
    // now we have z^3 + 3qz + r = 0
    POLYNOMIALSOLVE_TYPE d = q*q*q + r*r;

    // can we directly factorize the polynomial into linear and quadratic:
    if (fabs(d) < PS_DOUBLE_EPSILON) {
      r = cbrt(r);
      sol.resize(2);
      sol[0] = 2.0*r - a2;
      sol[1] = -r - a2;
      num = 2;
    } else {
   
      // no, we got an offset    
      if (d > 0.0) {
        // quadratic has two imaginary sols
        // only one real solution
        POLYNOMIALSOLVE_TYPE s1,s2;

        d = sqrt(d);

        s1 = cbrt(r + d);
        s2 = cbrt(r - d);

        sol.push_back(s1 + s2 - a2);
        return 1;
      }
    
      // the general case: d<0
      // need to think in complex plane
      d = sqrt(-d);
      q = atan2(d,r) / 3.0;
      r = cbrt(hypot(d,r));
   
      POLYNOMIALSOLVE_TYPE s = 2.0 * r * cos(q);
      POLYNOMIALSOLVE_TYPE t = fabs(2.0 * r * sin(q)) * sin(60.0*M_PI/180.0);

      sol.resize(3);
      sol[0] =  s + -a2 ;
      sol[1] = -0.5*s -a2 - t;
      sol[2] = -0.5*s -a2 + t;
      num=3;
    }
  }
#endif
  return num;
}

int PolynomialSolve::Cubic(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                           vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
{
#ifdef BIAS_DEBUG
  int deg=(int)coeff.size()-1;
  if(deg!=3 || coeff[deg]==0.0){
    BIASERR("coeff is not a polynom of third order");
  }
#endif
  BIASASSERT(deg==3);
  BIASASSERT(coeff[deg]!=0.0);
  int num=0; 

#ifdef BIAS_HAVE_GSL
  double z[6];
  num = gsl_poly_complex_solve_cubic(coeff[2]/coeff[3], coeff[1]/coeff[3],
                                     coeff[0]/coeff[3], (gsl_complex*)&(z[0]), 
                                     (gsl_complex*)&(z[2]), 
                                     (gsl_complex*)&(z[4]));
  for (int i=0; i<num; i++){
    sol.push_back(complex<POLYNOMIALSOLVE_TYPE>(z[2*i], z[2*i+1]));
  }
//   sol[0]=complex<POLYNOMIALSOLVE_TYPE>(z[0], z[1]);
//   sol[1]=complex<POLYNOMIALSOLVE_TYPE>(z[2], z[3]);
//   sol[2]=complex<POLYNOMIALSOLVE_TYPE>(z[4], z[5]);
#else
  sol.clear();
  BIASERR("gsl needed for PolynomialSolve::Cubic(vector, vector<complex>)");
  num=-1;
#endif
  return num;
}


int PolynomialSolve::Quartic(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                             vector<POLYNOMIALSOLVE_TYPE>& sol)
{
    //BUG: Unknown bug, and unclear who reported the bug!
  BIASWARN("ATTENTION! there seems to be a bug in Quartic()!");
  int deg=(int)coeff.size()-1;
#ifdef BIAS_DEBUG
  if(deg!=4 || coeff[deg]==0.0){
    BIASERR("coeff is not a polynom of fourth order");
  }
#endif
  BIASASSERT(coeff[deg]!=0.0);
  BIASASSERT(deg==4);
  int num=0;
  
  sol.clear();
  sol.reserve(deg);

#ifdef BIAS_HAVE_GSL
  //  BIASERR("no gsl implementation of quartic polynomial available");
#endif

  vector<POLYNOMIALSOLVE_TYPE> CubicCoeffs, CubicSolutions;
  CubicCoeffs.resize(4);
  
  // normalize to highest order coeficient a4==1
  POLYNOMIALSOLVE_TYPE a0 = coeff[0] / coeff[4];
  POLYNOMIALSOLVE_TYPE a1 = coeff[1] / coeff[4];
  POLYNOMIALSOLVE_TYPE a2 = coeff[2] / coeff[4];
  POLYNOMIALSOLVE_TYPE a3 = coeff[3] / coeff[4];
    
  CubicCoeffs[0] = -( a1*a1 + a0 * a3 * a3  - 4.0 * a0 * a2 );
  CubicCoeffs[1] = a1*a3 - 4.0 * a0;
  CubicCoeffs[2] = -a2;
  CubicCoeffs[3] = 1.0;

  // unsigned int s = 
  Cubic(CubicCoeffs, CubicSolutions);
  // cout <<"solveCubic returned "<<s<<" solutions."<<endl;

  for(unsigned int i = 0 ; i < CubicSolutions.size() ; i++ ) {
    POLYNOMIALSOLVE_TYPE u,s,t;
    u = CubicSolutions[i];
    
    s = a3*a3 / 4.0 + u - a2;
    t = u * u / 4.0 - a0;
    
    if ((s>-DOUBLE_MIN) && (t>-DOUBLE_MIN)) {
      if (s<DOUBLE_MIN) s=0;
      if (t<DOUBLE_MIN) t=0;

      vector<POLYNOMIALSOLVE_TYPE> QuadraticCoeffs, QuadraticSolutions;
        QuadraticCoeffs.resize(3);
        
        s = sqrt(s);
        t = sqrt(t);
        
        // check sign of t
        if ((s*t>0.0)^(a3*u/2.0-a1>0.0)) t = -t;
                
        QuadraticCoeffs[0] = u / 2.0 + t;
        QuadraticCoeffs[1] = a3 / 2.0 + s;
        QuadraticCoeffs[2] = 1.0;
        Quadratic(QuadraticCoeffs, QuadraticSolutions);
        
        QuadraticCoeffs[0] = u / 2.0 - t;
        QuadraticCoeffs[1] = a3 / 2.0 - s;
        Quadratic(QuadraticCoeffs, sol);
      
      for (unsigned int j=0; j<QuadraticSolutions.size(); j++)
        sol.push_back(QuadraticSolutions[j]);
        // cout << "SolveQuartic has "<<Solutions.size()<<" solutions: ";
      // for (int g=0; g<Solutions.size(); g++) cout << Solutions[g]<<" ";
      // cout <<endl;
      num = sol.size();
    } else {
      // cout <<"s="<<s<<" t="<<t<<endl;
    }
  }
  return num;
}

void PolynomialSolve::CheckCoefficients(vector<POLYNOMIALSOLVE_TYPE>& coeff,
                                        double eps)
{
  // find largest polynomial coefficient
  POLYNOMIALSOLVE_TYPE largestcoeff=0;
  for (unsigned int i=0; i<coeff.size(); i++) {
    if (fabs(coeff[i])>largestcoeff) largestcoeff = fabs(coeff[i]);
  }
  if (largestcoeff>0.0) {
    // normalize by next power of two of largest coefficient, so largest
    // coefficient is close to 1, but we dont get any loss in precision.
    largestcoeff = exp2(-1.0*rint(log2(largestcoeff)));
    for (unsigned int i=0; i<coeff.size(); i++) {
      coeff[i] *= largestcoeff;
    }
  } else {
    coeff.clear();
    return;
  }
  // this is now relative checking
  while (fabs(*coeff.rbegin())<=eps && coeff.size()>0){
    BIASCDOUT(D_PS_COEFF, "removing leading zero: "<<*coeff.rbegin()<<endl);
    coeff.pop_back();
  }
}

void PolynomialSolve::CalCoefficients(const vector<POLYNOMIALSOLVE_TYPE>& sol,
                                      vector<POLYNOMIALSOLVE_TYPE>& coeff)
{
  int deg=(int)sol.size();
  int nc=deg+1;
  coeff.resize(nc);
  for (int c=0; c<nc; c++){
    BIASCDOUT(D_PS_COEFF, "\ncoff["<<c<<"] = ");
    coeff[c]=_GetCoeff(c, 0, sol);
    BIASCDOUT(D_PS_COEFF, " = "<<coeff[c]<<endl);
  }
}

POLYNOMIALSOLVE_TYPE 
PolynomialSolve::_GetCoeff(int order, int begin,
                           const vector<POLYNOMIALSOLVE_TYPE>& sol)
{
  POLYNOMIALSOLVE_TYPE sum=0.0;
  int size=(int)sol.size();
  if (order==size){
    sum=1.0;
    BIASCDOUT(D_PS_COEFF, sum);
  } else if (order==0){
    sum=-sol[0];
    BIASCDOUT(D_PS_COEFF, -sol[0]);
    for (int i=1; i<size; i++) {
      sum*=-sol[i];
      BIASCDOUT(D_PS_COEFF, "*"<<-sol[i]);
    }
  } else { 
    sum=0.0;
    vector<POLYNOMIALSOLVE_TYPE> msol=sol;
    for (int i=begin; i<size; i++){
      msol=sol;
      // remove (i)th element from msol
      msol.erase(msol.begin()+i);
#ifdef BIAS_DEBUG
      if (order-1==0) { BIASCDOUT(D_PS_COEFF, " + "); }
#endif
      sum+=_GetCoeff(order-1, i, msol);
    }
  }
  return sum;
}

int PolynomialSolve::GetNumberOfRealSolutions(const vector<POLYNOMIALSOLVE_TYPE>& coeff)
{
  vector<POLYNOMIALSOLVE_TYPE> sol;
  //  Poor implementation of GetNumberOfRealsol().
  //  Actually computes all solutions and throws them away.
  return (Solve(coeff, sol));
}


int PowellFuncPolyEval(void *p,int n, const double *x, double *fvec, int iflag)
{
#ifdef BIAS_HAVE_GSL
  //cast pointer to object to access members, make it threadsafe!
  PolynomialSolve* ptr = NULL;
  //ptr = dynamic_cast<PolynomialSolve*>(p);
  ptr = (PolynomialSolve*)(p);
  if(ptr != NULL && ptr->PowellCoeff_!=NULL){
    *fvec=gsl_poly_eval(ptr->PowellCoeff_, ptr->PowellCoeffNum_, *x);
    return 0;
  }
  else{
    BIASERR("The Coefficients are NULL, something is wrong!");
    return -2;
  }  
#else
  BIASERR("GSL (Gnu Scientific Library) needed.");
  return -1;
#endif
}

int PolynomialSolve::NonLinearRefine(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                                     vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
{
  PowellCoeffNum_=coeff.size();
  if (PowellCoeff_!=NULL){ delete[] PowellCoeff_; PowellCoeff_=NULL; };
  PowellCoeff_=new double[PowellCoeffNum_];
  for (int i=0; i<PowellCoeffNum_; i++) PowellCoeff_[i]=coeff[i];
  for (unsigned i=0; i<sol.size(); i++){
    if (imag(sol[i])==0.0){
      Vector<double> ig(1), re(1);
      ig[0]=real(sol[i]);
      if (Powell(PowellFuncPolyEval, (void*)(this), ig, re, 1e-10)==0){
#ifdef BIAS_HAVE_GSL
        GetDebugStream().precision(25);
        BIASCDOUT(D_PS_NLREFINE, "before refine: "<<ig[0]<<"\tpoly: "
                  <<gsl_poly_eval(PowellCoeff_, PowellCoeffNum_, ig[0])<<endl);
#endif
        sol[i]=(complex<double>(re[0], 0.0));
#ifdef BIAS_HAVE_GSL
        BIASCDOUT(D_PS_NLREFINE, "after refine: "<<re[0]<<"\tpoly: "
                  <<gsl_poly_eval(PowellCoeff_, PowellCoeffNum_, re[0])
                  <<"\tsol: "<<sol[i]<<endl);
#endif
      } else {
        BIASERR("nonlinear optimization failed for "<<i);
      }
    } else {
      BIASCDOUT(D_PS_NLREFINE, "not refining "<<sol[i]
                <<", because solution is imaginary  "<<endl);
    }
  }
  delete[] PowellCoeff_; PowellCoeff_=NULL;
  return 0;
}

int PolynomialSolve::NonLinearRefine(const vector<POLYNOMIALSOLVE_TYPE>& coeff,
                                     vector<POLYNOMIALSOLVE_TYPE>& sol)
{
  PowellCoeffNum_=coeff.size();
  if (PowellCoeff_!=NULL){ delete[] PowellCoeff_; PowellCoeff_=NULL; };
  PowellCoeff_=new double[PowellCoeffNum_];
  for (int i=0; i<PowellCoeffNum_; i++) PowellCoeff_[i]=coeff[i];
  for (unsigned i=0; i<sol.size(); i++){
    Vector<double> initialvalue(1), resultvalue(1);
    initialvalue[0] = sol[i];
    if (Powell(PowellFuncPolyEval,(void*)(this),initialvalue, resultvalue, 1e-15)==0){
      BIASCDOUT(D_PS_NLREFINE, "residual before refine: "
                <<PolynomialSolve::EvaluatePolynomial(sol[i], coeff)
                <<"\troot: "<<setprecision(19)<<sol[i]<<endl);
      sol[i]=resultvalue[0];
      BIASCDOUT(D_PS_NLREFINE, "residual after refine: "
                <<PolynomialSolve::EvaluatePolynomial(sol[i], coeff)
                <<"\troot: "<<setprecision(19)<<sol[i]<<endl);

    } else {
      // BIASERR("nonlinear optimization failed for "<<i);
    }
  }
  delete[] PowellCoeff_; PowellCoeff_=NULL;
  return 0;
}


vector<double> values;
vector<double> locations;

int TheErrorFunction(void *p,int m, int n, const double *x,
                      double *fvec, int iflag) {
  vector<double> params;
  for (int i=0; i<n; i++)  params.push_back(x[i]);
 
  PolynomialSolve PS;
  for (int j=0; j<m; j++)
    fvec[j] = values[j] - PS.EvaluatePolynomial(locations[j], params);
  return 0;
}


int PolynomialSolve::FitPolynomial(const unsigned int degree,
                                   const std::vector<double>& x,
                                   const std::vector<double>& y,
                                   std::vector<double> &coefficients) {
  BIASASSERT(x.size()==y.size());
  coefficients.clear();
  coefficients.resize(degree+1, 0.0);
  Matrix<double> A(x.size(), degree+1);
  Vector<double> B(y.size()), X(degree+1);
  for (unsigned int row=0; row<x.size(); row++) {
    const double & xc = x[row];
    for (unsigned int col=0; col<degree+1; col++) {
      A[row][col] = 1;
      for (unsigned int colpower=0; colpower<col; colpower++) 
        A[row][col] *= xc;
    }
    B[row] = y[row];
  }
  
 
  SVD mySVD;
  int result = mySVD.Solve(A, B, X);
  if (result!=0) {
    BIASERR("linear solution failed");
    return -1;
  }
  Vector<double> resultcoeff(degree+1);

  // optimize numerically unstable linear result
  values = y;
  locations = x;

  if ((result = LevenbergMarquardt(&TheErrorFunction, (void*)(this), x.size(), degree+1,
                                   X,resultcoeff, 
                                   MINPACK_DEF_TOLERANCE))!=0) {
     BIASERR("Error in Levenberg-Marquardt-Optimization of Polynomial: "<<result);
     // use linear solution
     for (unsigned int col=0; col<degree+1; col++) coefficients[col] = X[col];
  } else {
    // use these coefficients
    for (unsigned int col=0; col<degree+1; col++) {
      coefficients[col] = resultcoeff[col];
    }
  }
  // optimization falied, but linear ok:
  if (result!=0) return 1;
  return 0;
}