/*This is an extract from the main program (lens_distortion_estimation.c), dealing with the 
* inversion of the radial distortion model
*/ 

/**
* \fn int ami_inverse_lens_distortion(double x,double y,double x0,double y0,
double *xt,double *yt,double *a,int Na)
* \brief function to inverse the lens distortion transformation
* \Author Luis Alvarez
* \return return 0 if the function finishes properly
*/
int ami_inverse_lens_distortion(
  double x,double y        /** POINT TO INVERSE (INPUT)*/,
  double x0,double y0      /** CENTER OF THE IMAGE (INPUT)*/,
  double *xt,double *yt    /** INVERVE POINT TRANSFORMED (OUTPUT) */,
  double *a                /** LENS DISTORTION MODEL POLYNOM */,
  int Na                   /** DEGREE OF THE LENS DISTORTION MODEL POLYNOM */
  )
{
  int i,Nr;
  double paso,d,*b,*b2,*rx,*ry,root;

  /* WE ALLOCATE MEMORY */
  b=(double*)malloc( sizeof(double)*(Na+2) );
  b2=(double*)malloc( sizeof(double)*(Na+2) );
  rx=(double*)malloc( sizeof(double)*(Na+2) );
  ry=(double*)malloc( sizeof(double)*(Na+2) );

  /* WE DEFINE THE POLYNOM WE NEED TO COMPUTE ROOTS */
  d=sqrt((x-x0)*(x-x0)+(y-y0)*(y-y0));
  b[0]=-1; b[1]=1.;
  paso=d;
  for(i=2;i<(Na+2);i++){
    b[i]=a[i-1]*paso;
    paso*=d;
   }
  if(Na==2){
    Nr=ami_RootCubicPolynomial(b,3,rx);
    for(i=0;i<Na;i++) ry[i]=0;
  }
  else{
    for(i=0;i<(Na+2);i++) b2[i]=b[Na+1-i];
    Nr=rpoly(b2,Na+1,rx,ry);
  }
  /* WE SELECT THE REAL ROOT NEAR TO 1 */
  root=10e5;
  for(i=0;i<Nr;i++){
    if(fabs(ry[i])<0.00000000001 && fabs(root-1)>fabs(rx[i]-1)) root=rx[i];
  }
  /* WE TRANSFORM THE POINT COORDINATES */
  *xt=x0+(x-x0)*root;
  *yt=y0+(y-y0)*root;
  free(b); free(rx); free(ry); free(b2);
  return(0);
}