#include "globdef.h"
#include "uidef.h"
#include "llsqdef.h"


void parabolic_fit(float *amp, float *pos, float yy1, float yy2, float yy3)
{
float t3,t4;
// ***********************************
// Fit a parabola to the max point and it's nearest neighbours.
// As input we have 3 data points y(x)
// Find parameters a,b and c to place these points on the curve:
//  y(x)=a+b*(x-c)**2=a+b*x**2-2*x*b*c+b*c**2
// The equations to solve are:
// y(-1)=a + b + 2*b*c + b*c**2
// y( 0)=a     +         b*c**2
// y( 1)=a + b - 2*b*c + b*c**2
//  b has to be negative (for a positive peak)
//  abs(c) has to be below 0.5 since y(0)is the largest value.
// t4=y(-1)-y(1)=4*b*c
// t3=2*(y(-1)+y(1)-2*y(0))=4*b
// t4/t3=c=peak offset
// ***********************************

t4=yy1-yy3;
t3=2*(yy1+yy3-2*yy2);
if(t3 < 0)
  {
  amp[0]=yy2-0.5*t4*t4/t3;
  t4=t4/t3;
  if(fabs(t4) > 1)t4/=fabs(t4);
  pos[0]=t4;
  }
else
  {
  if(yy1 > yy3)
    {
    amp[0]=yy1;
    pos[0]=-1;
    }
  else
    {
    amp[0]=yy3;
    pos[0]=1;
    }
  }
}    






int llsq1(void)
{
float aux[2*LLSQ_MAXPAR];
int ipiv[2*LLSQ_MAXPAR];
int kpiv;
int i,j,k;
float t1,sig,piv,beta;
if(llsq_npar > LLSQ_MAXPAR)
  {
  lirerr(1051);
  return -1;
  }
kpiv=piv=0;
for(k=0; k<llsq_npar; k++)
  {
  ipiv[k]=k;
  t1=0;
  for(i=0; i<llsq_neq; i++)
    {
    t1+=pow(llsq_derivatives[k*llsq_neq+i],2.0);
    }
  aux[k]=t1;
  if(t1 > piv)
    {
    piv=t1;
    kpiv=k;
    }
  }
if(piv == 0)return -1;
sig=sqrt(piv);
for(k=0; k<llsq_npar; k++)
  {
  if(kpiv>k)
    {
    t1=aux[k];
    aux[k]=aux[kpiv];
    aux[kpiv]=t1;
    for(i=k; i<llsq_neq; i++)
      {
      t1=llsq_derivatives[k*llsq_neq+i];
      llsq_derivatives[k*llsq_neq+i]=llsq_derivatives[kpiv*llsq_neq+i];
      llsq_derivatives[kpiv*llsq_neq+i]=t1;
      }
    }
  if(k > 0)
    {
    sig=0;
    for(i=k; i<llsq_neq; i++)
      {
      sig+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[k*llsq_neq+i];
      }
    sig=sqrt(sig);
    }
  t1=llsq_derivatives[k*llsq_neq+k];
  if(t1 < 0 )sig=-sig;
  ipiv[kpiv]=ipiv[k];
  ipiv[k]=kpiv;
  beta=t1+sig;
  llsq_derivatives[k*llsq_neq+k]=beta;
  beta=1/(sig*beta);
  j=llsq_npar+k;
  aux[j]=-sig;
  if(k<llsq_npar-1)
    {
    piv=0;
    kpiv=k+1;
    for(j=k+1; j<llsq_npar; j++)
      {
      t1=0;
      for(i=k; i<llsq_neq; i++)
        {
        t1+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[j*llsq_neq+i];
        }
      t1=beta*t1;
      for(i=k; i<llsq_neq; i++)
        {
        llsq_derivatives[j*llsq_neq+i]-=llsq_derivatives[k*llsq_neq+i]*t1;
        }
      t1=aux[j]-llsq_derivatives[j*llsq_neq+k]*llsq_derivatives[j*llsq_neq+k];
      aux[j]=t1;
      if(t1 > piv)
        {
        piv=t1;
        kpiv=j;
        }
      }
    }
  t1=0;
  for(i=k; i<llsq_neq; i++)
    {
    t1+=llsq_derivatives[k*llsq_neq+i]*llsq_errors[i];
    }
  t1*=beta;
  for(i=k; i<llsq_neq; i++)
    {
    llsq_errors[i]-=llsq_derivatives[k*llsq_neq+i]*t1;
    }
  }
llsq_steps[llsq_npar-1]=llsq_errors[llsq_npar-1]/aux[2*llsq_npar-1];
if(llsq_npar == 1)return 0;
for(k=llsq_npar-2; k>=0; k--)
  {
  t1=llsq_errors[k  ];
  for(i=k+1; i<llsq_npar; i++)
    {
    t1-=llsq_derivatives[i*llsq_neq+k]*llsq_steps[i];
    }
  t1/=aux[llsq_npar+k];
  llsq_steps[k]=llsq_steps[ipiv[k]];
  llsq_steps[ipiv[k]  ]=t1;
  }
return 0;
}

int llsq(void)
{
float aux[2*LLSQ_MAXPAR];
int ipiv[2*LLSQ_MAXPAR];
int kpiv;
int i,j,k;
float t1,t2,sig,piv,beta;
if(llsq_npar > LLSQ_MAXPAR)
  {
  lirerr(1051);
  return -1;
  }
kpiv=piv=0;
for(k=0; k<llsq_npar; k++)
  {
  ipiv[k]=k;
  t1=0;
  for(i=0; i<llsq_neq; i++)
    {
    t1+=pow(llsq_derivatives[k*llsq_neq+i],2.0);
    }
  aux[k]=t1;
  if(t1 > piv)
    {
    piv=t1;
    kpiv=k;
    }
  }
if(piv == 0)return -1;
sig=sqrt(piv);
for(k=0; k<llsq_npar; k++)
  {
  if(kpiv>k)
    {
    t1=aux[k];
    aux[k]=aux[kpiv];
    aux[kpiv]=t1;
    for(i=k; i<llsq_neq; i++)
      {
      t1=llsq_derivatives[k*llsq_neq+i];
      llsq_derivatives[k*llsq_neq+i]=llsq_derivatives[kpiv*llsq_neq+i];
      llsq_derivatives[kpiv*llsq_neq+i]=t1;
      }
    }
  if(k > 0)
    {
    sig=0;
    for(i=k; i<llsq_neq; i++)
      {
      sig+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[k*llsq_neq+i];
      }
    sig=sqrt(sig);
    }
  t1=llsq_derivatives[k*llsq_neq+k];
  if(t1 < 0 )sig=-sig;
  ipiv[kpiv]=ipiv[k];
  ipiv[k]=kpiv;
  beta=t1+sig;
  llsq_derivatives[k*llsq_neq+k]=beta;
  beta=1/(sig*beta);
  j=llsq_npar+k;
  aux[j]=-sig;
  if(k<llsq_npar-1)
    {
    piv=0;
    kpiv=k+1;
    for(j=k+1; j<llsq_npar; j++)
      {
      t1=0;
      for(i=k; i<llsq_neq; i++)
        {
        t1+=llsq_derivatives[k*llsq_neq+i]*llsq_derivatives[j*llsq_neq+i];
        }
      t1=beta*t1;
      for(i=k; i<llsq_neq; i++)
        {
        llsq_derivatives[j*llsq_neq+i]-=llsq_derivatives[k*llsq_neq+i]*t1;
        }
      t1=aux[j]-llsq_derivatives[j*llsq_neq+k]*llsq_derivatives[j*llsq_neq+k];
      aux[j]=t1;
      if(t1 > piv)
        {
        piv=t1;
        kpiv=j;
        }
      }
    }
  t1=0;
  t2=0;
  for(i=k; i<llsq_neq; i++)
    {
    t1+=llsq_derivatives[k*llsq_neq+i]*llsq_errors[2*i  ];
    t2+=llsq_derivatives[k*llsq_neq+i]*llsq_errors[2*i+1];
    }
  t1*=beta;
  t2*=beta;
  for(i=k; i<llsq_neq; i++)
    {
    llsq_errors[2*i  ]-=llsq_derivatives[k*llsq_neq+i]*t1;
    llsq_errors[2*i+1]-=llsq_derivatives[k*llsq_neq+i]*t2;
    }
  }
llsq_steps[2*(llsq_npar-1)  ]=
                        llsq_errors[2*(llsq_npar-1)  ]/aux[2*llsq_npar-1];
llsq_steps[2*(llsq_npar-1)+1]=
                        llsq_errors[2*(llsq_npar-1)+1]/aux[2*llsq_npar-1];
if(llsq_npar == 1)return 0;
for(k=llsq_npar-2; k>=0; k--)
  {
  t1=llsq_errors[2*k  ];
  t2=llsq_errors[2*k+1];
  for(i=k+1; i<llsq_npar; i++)
    {
    t1-=llsq_derivatives[i*llsq_neq+k]*llsq_steps[2*i  ];
    t2-=llsq_derivatives[i*llsq_neq+k]*llsq_steps[2*i+1];
    }
  t1/=aux[llsq_npar+k];
  t2/=aux[llsq_npar+k];
  llsq_steps[2*k  ]=llsq_steps[2*ipiv[k]  ];
  llsq_steps[2*k+1]=llsq_steps[2*ipiv[k]+1];
  llsq_steps[2*ipiv[k]  ]=t1;
  llsq_steps[2*ipiv[k]+1]=t2;
  }
return 0;
}