#include <stdio.h>
#include <math.h>
#include "matrice.h"
#include "lbl_array.h"
#include "lsq_fix.h"

#define DRES 1.0e-8

/*--------------------------------------------------------------------------*
 | lsq_nbeacon()                                                            |
 | Jerome Vaganay -                                                         |
 *--------------------------------------------------------------------------*/

void lsq_nbeacon(double n0,double e0,double depth,lbl_array *lbl,
		 double dnb[MAX_BEACON_NUMBER], double deb[MAX_BEACON_NUMBER],
		 double *n,double *e,double *residual,matrice covar)
{
  int i,ind,nb_ranges;
  double tol=1.0e-20;

  /*--- count how many ranges are available */
  nb_ranges = 0;
  for (i=1; i<=lbl->number_of_beacon; i++)
    if (lbl->sr[i] != 0.0)
      nb_ranges++;

  /*--- load x and y */
  ind = 0;
  for (i=1; i<=lbl->number_of_beacon; i++) {
    if (lbl->sr[i] != 0.0) {
      e(y,ind,0) = lbl->sr[i];
      e(x,ind,0) = lbl->nb[i]-dnb[i];
      e(x,ind,1) = lbl->eb[i]-deb[i];
      e(x,ind,2) = lbl->db[i];
      e(x,ind,3) = depth;
      ind++;
    }
  }

  /*--- initial position */
  e(a,0,0) = n0;
  e(a,1,0) = e0;

  /*--- load noise matrix */
  for (i=0; i<nb_ranges; i++)
    e(noise,i,i) = sqrt(lbl->tof_var*lbl->sos*lbl->sos);

  if (!Levenberg_Marquardt(x,y,a,residual,covar,&tol,noise,nb_ranges)) {
    printf("Levenberg_Marquardt returns 0 in lsq_nbeacon\n");
  }
  else {
    *n = e(a,0,0);
    *e = e(a,1,0);
  }
}

/*----------------------------------------------------------------------*
|  Algorithme de Levenberg Marquardt - O. STRAUSS  / modif J. VAGANAY   |
|  reduction de l'erreur quadratique sur la fonction y=f(x,a)           |
|  x et y sont les donnees et a les parametres à ajuster                |
|                                                                       |
|  x: matrice d'entree de la fonction                                   |
|  y: vecteur de sortie de la fonction                                  |
|  a: parametre a ajuster                                               |
|  chi2: valeur du critère minimisé                                     |
|  tolerance: tolérance de l'algorithme                                 |
*-----------------------------------------------------------------------*/

int Levenberg_Marquardt(matrice x,matrice y,matrice a,double *chi2,
			matrice Variance, double *tolerance,
			matrice noise,int nb_ranges)
{
  int convergence,i;
  double chi2_ref;
  double lambda,facteur,lambda_max = 1.0e32;

  /*--- initialisation de lambda */
  lambda = 0.001 ;

  Moindres_Carres(x,y,a,Hessien,Derivee,chi2,noise,nb_ranges);

  chi2_ref = (*chi2);
  mat_copy_S(a,essai);

  convergence = 0;
  while (!convergence) {

    mat_copy_S(Hessien,Covariance);

    /*--- augmentation des éléments diagonaux et recopie de Derivee dans sol */
    facteur = 1.0+lambda;
    for (i=0; i<2; i++)
      e(Covariance,i,i) *= facteur;
    mat_copy_S(Derivee,sol);
    solve_syst(Covariance.valeur,sol.valeur) ;
    mat_copy_S(sol,da);

    /*--- c'est la fin on a convergé */
    if (( Fabs(lambda)<PRECISION ) || ( Fabs(lambda)>lambda_max )) {
      if (inv22(Hessien.valeur) > PRECISION) {
	mat_copy_S(Hessien,Variance);
	return(1);
      }
      else {
	return(0);
      }
    }

    mat_plus_S(a,da,essai);

    Moindres_Carres(x,y,essai,Covariance,sol,chi2,noise,nb_ranges);

    /*--- nouvelle solution acceptée */
    if ( (*chi2)<chi2_ref ) {
      lambda *= 0.1;
      convergence = ( Fabs( chi2_ref - (*chi2) ) < DRES );
      chi2_ref = (*chi2);

      mat_copy_S(Covariance,Hessien);
      mat_copy_S(sol,Derivee);
      mat_copy_S(essai,a);

      convergence = convergence || ( chi2_ref < (*tolerance) );
      convergence = convergence || ( Fabs(Norme(da.valeur,2)) < (*tolerance) );
    }

    /*--- echec de la recherche, on augmente lambda */
    else {
      lambda *= 10.0 ;
      (*chi2) = chi2_ref ;
    }
  } /* fin du while */

  if (inv22(Hessien.valeur) > PRECISION) {
    mat_copy_S(Hessien,Variance);
    return(1) ;
  }
  else {
    return(0);
  }
}

/*------------------------------------------------------------------------------*
 | 0. STRAUSS. Adaptation: J. VAGANAY                                           |
 *------------------------------------------------------------------------------*/

int Moindres_Carres(matrice x,matrice y,matrice a,matrice Hessien,matrice Derivee,
		    double *chi2,matrice noise,int nb_ranges)
{
  int i;

  /*--- remplissage de Y et W */
  for (i=0; i<nb_ranges; i++) {
    e(Y,i,0) = sqrt(pow(e(a,0,0)-e(x,i,0),2.0)+pow(e(a,1,0)-e(x,i,1),2.0)+pow(e(x,i,3)-e(x,i,2),2.0));
    e(W,i,0) = (e(a,0,0)-e(x,i,0))/e(Y,i,0);
    e(W,i,1) = (e(a,1,0)-e(x,i,1))/e(Y,i,0);
  }

  /*--- calcul de Ninv = inv(noise) */
  for (i=0; i<nb_ranges; i++)
    e(Ninv,i,i) = 1.0/e(noise,i,i);

  /*--- calcul de Hessien = Wt*Ninv*W */
  mat_transpose_S(W,Wt);
  mat_prod_S(Wt,Ninv,WtNinv);
  mat_prod_S(WtNinv,W,Hessien);

  /*--- calcul de Derivee = Wt*Ninv*(y-Y) */
  mat_moins_S(y,Y,dy);
  mat_transpose_S(W,Wt);
  mat_prod_S(Wt,Ninv,temp);
  mat_prod_S(temp,dy,Derivee);

  /*--- calcul du chi2 */
  mat_transpose_S(dy,dyt);
  mat_prod_S(dyt,Ninv,dytNinv);
  mat_prod_S(dytNinv,dy,res);
  *chi2 = *(res.valeur);

  return(1);
}