#include "mex.h"
#include<stdio.h>
#include<math.h>
#define epsilon 1.e-10

void ini(double *z,double *x,double *y, int nbpt,int np){
double xd,xf,yd,yf,aire;
int ic,i,ip,j,test;
ic=(int)(nbpt/2);
/* moyenne sur 5 points */
xd=0;xf=0;yd=0;yf=0;
for(i=0;i<5;i++){xd+=x[0+i];xf+=x[nbpt-1-i];yd+=y[0+i];yf+=y[nbpt-1-i];}
xd=xd/5;xf=xf/5;yd=yd/5;yf=yf/5;
z[1]=(yf-yd)/(xf-xd);
z[0]=yf-z[1]*(xf-x[ic]);
for (j=0;j<np;j++){
    z[2+3*j]=0;
    test=0;
    z[3+3*j]=3;
    for (i=0;i<nbpt;i++){
        yf=y[i]-(z[0]+z[1]*(x[i]-x[ic]));
        if(j!=0){yf=yf-z[2]*z[3]*z[3]/((x[i]-z[4])*(x[i]-z[4])+z[3]*z[3]+epsilon);}
        if(j==2){yf=yf-z[5]*z[6]*z[6]/((x[i]-z[7])*(x[i]-z[7])+z[6]*z[6]+epsilon);}
        if(yf>z[2+3*j]){z[2+3*j]=yf;z[4+3*j]=x[i];ip=i;}}
        z[2+3*j]=z[2+3*j]*0.7;
    for (i=ip;i<nbpt;i++){
        yf=y[i]-(z[0]+z[1]*(x[i]-x[ic]));
        if(j!=0){yf=yf-z[2]*z[3]*z[3]/((x[i]-z[4])*(x[i]-z[4])+z[3]*z[3]+epsilon);}
        if(j==2){yf=yf-z[5]*z[6]*z[6]/((x[i]-z[7])*(x[i]-z[7])+z[6]*z[6]+epsilon);}
        if((yf<z[2+3*j]/2)&(test==0)){z[3+3*j]=x[i]-z[4+3*j];test=1;}}
    test=0;
        if(z[3+3*j]>20){    
        for (i=ip;i>0;i--){
        yf=y[i]-(z[0]+z[1]*(x[i]-x[ic]));
        if(j!=0){yf=yf-z[2]*z[3]*z[3]/((x[i]-z[4])*(x[i]-z[4])+z[3]*z[3]+epsilon);}
        if(j==2){yf=yf-z[5]*z[6]*z[6]/((x[i]-z[7])*(x[i]-z[7])+z[6]*z[6]+epsilon);}
        if((yf<z[2+3*j]/2)&(test==0)){z[3+3*j]=z[4+3*j]-x[i];test=1;}}}
    if(z[3+3*j]>20){z[3+3*j]=20;}
    if(z[3+3*j]<2){z[3+3*j]=2;}}
 return;}

void erreur1(double *err, double *z,double *x, double *y, int nbpt, int nbpic){
int i,ic,j;
double yf;
err[0]=0;
ic=(int)(nbpt/2);
for(i=0;i<nbpt;i++){
    yf=y[i]-(z[0]+z[1]*(x[i]-x[ic])+z[2]*z[3]*z[3]/((x[i]-z[4])*(x[i]-z[4])+z[3]*z[3]+epsilon));
    if(nbpic>1){yf=yf-z[5]*z[6]*z[6]/((x[i]-z[7])*(x[i]-z[7])+z[6]*z[6]+epsilon);}
    if(nbpic>2){yf=yf-z[8]*z[9]*z[9]/((x[i]-z[10])*(x[i]-z[10])+z[9]*z[9]+epsilon);}    
    err[0]+=yf*yf;}
err[0]=sqrt(err[0]);}

void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]){
int mrows,ncols,nbpic,i,j,nbparam,ic,test,cptsens,cpt;
double *x,*y,*z,*a,*yf,paramtest;
double coef[12];
double err,errm,errp,var,pas,mult,cte,sens,z0,z2,z5,z8;
if ((nrhs!=3)) {
    mexErrMsgTxt("\nFournir 3 arguments [param,yf]=fit(x,y,nb pic)\n");}
if (nlhs!=2)  {mexErrMsgTxt("1 argument demandé en retour [param,yf]=fit(x,y,nbpic)\n");}
x = mxGetPr(prhs[0]);
y = mxGetPr(prhs[1]);
a = mxGetPr(prhs[2]);
nbpic=(int)(a[0]);
mrows = mxGetM(prhs[0]);
ncols = mxGetN(prhs[0]);
/* Create matrix for the return argument. */
plhs[0] = mxCreateDoubleMatrix(1,3+3*nbpic, mxREAL);
z = mxGetPr(plhs[0]); 
plhs[1] = mxCreateDoubleMatrix(mrows,ncols, mxREAL);
yf = mxGetPr(plhs[1]); 
ini(z,x,y,ncols*mrows,nbpic);

/*on minimise les paramètres les uns après les autres */
nbparam=2+3*nbpic;
var=1;
erreur1(&err,z,x,y,ncols*mrows,nbpic);
for(i=0;i<2+3*nbpic;i++){
    coef[i]=1;
    if(i==1){coef[i]=0.01;}
    if(((i-2)/3)==0){coef[i]=10;}}
for(j=0;j<100;j++){
    for(i=1;i<nbparam;i++){
        paramtest=z[i];
        z[i]=z[i]+var*coef[i];
        erreur1(&errm,z,x,y,ncols*mrows,nbpic);
        test=1;
        mult=1;
        sens=1;
        cte=0;
        cptsens=0;       
        while(test==1){
            if (errm<err){
            	err=errm;
            	cte=cte+sens*mult;
                mult=mult*2.0;
                }
            else{
                cptsens++;
        	    if(cptsens==2){  	    
        	        if(mult<1.5){
        	            test=0;}
        	        else{
        	            mult=mult*0.5;}
        	        cptsens=0;}
        	    else{
        	        sens=-sens;}
        	    }
            z[i]=paramtest+var*coef[i]*(cte+sens*mult);      
            erreur1(&errm,z,x,y,ncols*mrows,nbpic);
       }

        z[i]=paramtest+var*cte*coef[i];        
        if(fabs(cte)<3){coef[i]=coef[i]/2;}
        if(fabs(cte)>50){coef[i]=coef[i]*2;}
    }
   /*evolution des hauteurs ensemble 0,2,5,8*/
        z0=z[0];
        z2=z[2];
        if(nbpic>1){z5=z[5];}
        if(nbpic==3){z8=z[8];}   
        z[0]=z[0]+var*coef[0];
        z[2]=z[2]-var*coef[0];
        if(nbpic>1){z[5]=z[5]-var*coef[0];}
        if(nbpic==3){z[8]=z[8]-var*coef[0];} 
        erreur1(&errm,z,x,y,ncols*mrows,nbpic);
        test=1;
        mult=1;
        sens=1;
        cte=0;
        cptsens=0;       
        while(test==1){
            if (errm<err){
            	err=errm;
            	cte=cte+sens*mult;
                mult=mult*2.0;
                }
            else{
                cptsens++;
        	    if(cptsens==2){  	    
        	        if(mult<1.5){
        	            test=0;}
        	        else{
        	            mult=mult*0.5;}
        	        cptsens=0;}
        	    else{
        	        sens=-sens;}
        	    }
        	    
        	z[0]=z0+var*coef[0]*(cte+sens*mult);
            z[2]=z2-var*coef[0]*(cte+sens*mult);
            if(nbpic>1){z[5]=z5-var*coef[0]*(cte+sens*mult);}
            if(nbpic==3){z[8]=z8-var*coef[0]*(cte+sens*mult);} 
            erreur1(&errm,z,x,y,ncols*mrows,nbpic);
       }

        z[0]=z0+var*cte*coef[0];        
        z[2]=z2-var*coef[0]*cte;
        if(nbpic>1){z[5]=z5-var*coef[0]*cte;}
        if(nbpic==3){z[8]=z8-var*coef[0]*cte;}  
        var=var*2/3;
}












ic=(int)(ncols*mrows/2);
for(i=0;i<ncols*mrows;i++){
    yf[i]=(z[0]+z[1]*(x[i]-x[ic])+z[2]*z[3]*z[3]/((x[i]-z[4])*(x[i]-z[4])+z[3]*z[3]));
    if(nbpic>1){yf[i]+=z[5]*z[6]*z[6]/((x[i]-z[7])*(x[i]-z[7])+z[6]*z[6]);}
    if(nbpic>2){yf[i]+=z[8]*z[9]*z[9]/((x[i]-z[10])*(x[i]-z[10])+z[9]*z[9]);}}
 z[3+3*nbpic-1]=err;
}