#include "wabor_t_theta_functions.h"


pac_type_real wabor_t_Theta_bspline(    pac_type_real f UNUSED, pac_type_real* data_f0_k_ax UNUSED){
    if(data_f0_k_ax==NULL){
        post("Error wabor_t_Theta_inv_bspline");
        return 0;
    }
    //thetadata[4+WABOR_T_PADDING_LEFT+sizew+WABOR_T_PADDING_LEFT]=data_theta_of_f
    pac_type_real* data_theta_of_w=data_f0_k_ax+4+WABOR_T_PADDING_LEFT;
    int sizew=data_theta_of_w[WABOR_T_LENGTH_BIN];   
    pac_type_real* data_theta_of_f=data_f0_k_ax+4+WABOR_T_PADDING_LEFT+sizew+WABOR_T_PADDING_LEFT;
    int sizef=data_theta_of_f[WABOR_T_LENGTH_BIN];
    int startf=data_theta_of_f[WABOR_T_PLACE_HOLDER];
    
    if(sizef==0) {
        return NAN;
    }
    
    pac_type_real dSign=f<0?-1:1;
    f=fabsPAC(f);
    f-=startf;

    pac_type_real val=dSign*bspline_inter(f,data_theta_of_f,sizef,2);
    return val;   
}

pac_type_real wabor_t_Theta_inv_bspline(    pac_type_real w, pac_type_real* data_f0_k_ax UNUSED){
    if(data_f0_k_ax==NULL){
        post("Error wabor_t_Theta_inv_bspline");
        return 0;
    }
    //thetadata[4+WABOR_T_PADDING_LEFT]=data_theta_of_w
    pac_type_real* data_theta_of_w=data_f0_k_ax+4+WABOR_T_PADDING_LEFT;
    int sizew=data_theta_of_w[WABOR_T_LENGTH_BIN];
    int startw=data_theta_of_w[WABOR_T_PLACE_HOLDER];
    
    if(sizew==0) {
        return NAN;
    }
    
    pac_type_real dSign=w<0?-1:1;
    w=fabsPAC(w);
    w-=startw;

    pac_type_real val=dSign*bspline_inter(w,data_theta_of_w,sizew,2);
    return val;
}
pac_type_real wabor_t_d_Theta_inf_bspline(    pac_type_real f UNUSED, pac_type_real* data_f0_k_ax){
    if(data_f0_k_ax==NULL){
        post("Error wabor_t_Theta_inv_bspline");
        return 0;
    }
    //thetadata[4+WABOR_T_PADDING_LEFT]=data_theta_of_w
    pac_type_real* data_theta_of_w=data_f0_k_ax+4+WABOR_T_PADDING_LEFT;
    int sizew=data_theta_of_w[WABOR_T_LENGTH_BIN];
    //int startw=data_theta_of_w[WABOR_T_PLACE_HOLDER];
    
    if(sizew==0) {
        return NAN;
    }
    
    pac_type_real diff, maxdiff=0;
    for(int i=1; i<sizew;++i){
        diff= data_theta_of_w[i]-data_theta_of_w[i-1];
        if(diff<0) {
            post("Warping map not monotone increasing at datapoint %i",i);
        }
        if(maxdiff<diff) {
            maxdiff=diff;
        }
    }

    return 1/maxdiff;

}



//c1_nyqu:
//C1, Linear zwischen 0 und fin, Linear von fout nach wnyquist (wny)


pac_type_real wabor_t_Theta_exp_c1_nyqu(    pac_type_real f, pac_type_real* data_f0_k_fny_wny){
    //x-Axis: f, y-Axis: w
    //pac_type_real fout=fqsupminus1;
    pac_type_real f0=((pac_type_real*)data_f0_k_fny_wny)[0];
    pac_type_real k=((pac_type_real*)data_f0_k_fny_wny)[1];
    pac_type_real fny=((pac_type_real*)data_f0_k_fny_wny)[2];
    pac_type_real wny=((pac_type_real*)data_f0_k_fny_wny)[3];
    pac_type_real fin=k/logPAC(2);
    pac_type_real fout,wout;
    if(fny>wny) {
        fout= INFINITY;
        wout=INFINITY;
    } else {
        pac_type_real val=LambertW1(-(wny/2*powPAC(2,1 - fny/k)/(E*f0)));
        fout= (k+fny*logPAC(2)+k*val)/logPAC(2);
        wout=(f0*powPAC(2.0L,fout/k));          
    }
    pac_type_real dSign=(f<0?-1:1);
    f=fabsPAC(f);
    if(f<=fin) return dSign * (f*E*f0*logPAC(2)/k);
    if(f<=fout) return dSign*(f0*powPAC(2.0L,f/k));
    
    pac_type_real kout=(wny-wout)/(fny-fout);
    if(kout<0) {
        error("wabor_t: Theta called with bad parameters.");
    }    
    return dSign*((f-fout)*kout+wout);
}
pac_type_real wabor_t_Theta_inv_exp_c1_nyqu(    pac_type_real w, pac_type_real* data_f0_k_fny_wny){
    //pac_type_real fout=fqsupminus1;
    pac_type_real f0=((pac_type_real*)data_f0_k_fny_wny)[0];
    pac_type_real k=((pac_type_real*)data_f0_k_fny_wny)[1];
    pac_type_real fny=((pac_type_real*)data_f0_k_fny_wny)[2];
    pac_type_real wny=((pac_type_real*)data_f0_k_fny_wny)[3];
    pac_type_real win=E*f0;
    pac_type_real wout, fout;
    if(fny>wny) {
        fout= INFINITY;
        wout=INFINITY;
    } else {
        pac_type_real val=LambertW1(-wny/2*powPAC(2,1 - fny/k)/(E*f0));
        fout= (k+fny*logPAC(2)+k*val)/logPAC(2);
        wout=(f0*powPAC(2.0L,fout/k));        
    }
    pac_type_real dSign=(w<0?-1:1);
    w=fabsPAC(w);
    if(w<=win) { return dSign*(w*k/(E*f0*logPAC(2))); }
    if(w<=wout) { return dSign*(logPAC(w/f0)*k/logPAC(2.0L)); }
    pac_type_real kout=(wny-wout)/(fny-fout);
    if(kout<0) {
        error("wabor_t: Theta called with bad parameters.");
    }
    return dSign*((w-wout)/kout+fout);
}
pac_type_real wabor_t_d_Theta_inf_exp_c1_nyqu( pac_type_real f UNUSED , pac_type_real* data_f0_k_fny_wny){
    pac_type_real f0=((pac_type_real*)data_f0_k_fny_wny)[0];
    pac_type_real k=((pac_type_real*)data_f0_k_fny_wny)[1];
    //pac_type_real fny=((pac_type_real*)data_f0_k_fny_wny)[2];
    //pac_type_real wny=((pac_type_real*)data_f0_k_fny_wny)[3];
    pac_type_real kin=E*f0*logPAC(2)/(k);
    return kin;
}

pac_type_real wabor_t_Theta_id(           pac_type_real f, pac_type_real* data_f0_k UNUSED){
    return f; 
} 
pac_type_real wabor_t_Theta_inv_id(       pac_type_real w, pac_type_real* data_f0_k UNUSED){
    return w; 
}
pac_type_real wabor_t_d_Theta_inf_id(	pac_type_real f UNUSED, pac_type_real* data_f0_k UNUSED){
    return 1.0L;
}

pac_type_real wabor_t_Theta_lin(           pac_type_real f, pac_type_real* data_f0_k){
    //pac_type_real f0=((pac_type_real*)data_f0_d)[0];
    pac_type_real k=((pac_type_real*)data_f0_k)[1];
    return k*f; 
} 
pac_type_real wabor_t_Theta_inv_lin(       pac_type_real w, pac_type_real* data_f0_k){
    //pac_type_real f0=((pac_type_real*)data_f0_k)[0];
    pac_type_real k=((pac_type_real*)data_f0_k)[1];
    return w/k; 
} 
pac_type_real wabor_t_d_Theta_inf_lin(  pac_type_real f UNUSED, pac_type_real* data_f0_k){
    //pac_type_real f0=((pac_type_real*)data_f0_d)[0];
    pac_type_real k=((pac_type_real*)data_f0_k)[1];
    return k;
}

pac_type_real wabor_t_Theta_square(        pac_type_real f, pac_type_real* data_f0_k){
    pac_type_real dSign=(f<0?-1.0:1.0); 
    pac_type_real f0=((pac_type_real*)data_f0_k)[0];
    pac_type_real k= ((pac_type_real*)data_f0_k)[1];
    f=fabsPAC(f);
    return dSign*(f*(f + k*f0))/k; 
} 
pac_type_real wabor_t_Theta_inv_square(    pac_type_real w, pac_type_real* data_f0_k){
    pac_type_real dSign=(w<0?-1.0:1.0);
    pac_type_real f0=((pac_type_real*)data_f0_k)[0];
    pac_type_real k= ((pac_type_real*)data_f0_k)[1];
    w=fabsPAC(w);
    return dSign*(-(k*f0) + sqrtPAC(k)*sqrtPAC(k*f0*f0 + 4*w))/2.; 
}
pac_type_real wabor_t_d_Theta_inf_square(  pac_type_real f UNUSED, pac_type_real* data_f0_k){
    pac_type_real f0=((pac_type_real*)data_f0_k)[0];
    return f0;
}


//Inverse Function
pac_type_real wabor_t_Theta_inv( pac_type_real (*func)(pac_type_real x, pac_type_real* _data ),  pac_type_real val, pac_type_real* data, 
        pac_type_real left, pac_type_real right, 
        pac_type_real accuracy ){
    //Loest die Gleichung f(y)=val nach y auf. 
    //Startet zu suchen zwischen left und right
    if(right==left){
        right+=1;
        left-=1;
    }
    if(right<left){
        pac_type_real swapvar=right;
        right=left;
        left=swapvar;        
    }
    while(func(left,data)<val && func(right,data)<val || func(left,data)>val && func(right,data)>val) {
        left*=0.75;
        right*=1.5;
        if(left==-INFINITY||left==INFINITY||left==NAN||right==-INFINITY||right==INFINITY||right==NAN){
            return NAN;
        }
    }
    
    
    pac_type_real half,h,l;
    int n=0;
#define MAXITER (100)
    while(++n,
            half=(left+right)/2.0,
            h=func(half,data),l=func(left,data),
            fabsPAC(h-val)>accuracy){
        if(h<val && l<val || h>val && l>val ){
            left=half;
        } else {
            right=half;
        }
        half=(left+right)/2.0;
        if(n>=MAXITER) {
            post("wabor_t_inverse: no convergence");
            return NAN;
        }
    }
    return half;
}


//BSplineBasis
pac_type_real wabor_t_bspline_basis(int n, pac_type_real x){
    //Returns zero-centered n-th B-Spline with support -(n-1)/2 ... (n+1)/2
    x=fabsPAC(x);
    switch(n){
        case 0:
            if(x<1.0/2.0) return 1.0;
            return 0;
            break;
            
        case 1:
            if(x<1) return 1-x;
            return 0;
            break;
            
        case 2:
            if(x<0.5) return 1.0/4.0*(3.0 - 4*x*x);
            if(x<1.5) return 9.0/8.0 + (-3.0/2.0 + x/2.0)* x;
            return 0;
            break;
            
        case 3:
            if(x<1.0) return 0.6666666666666666 + (-1 + x/2.)*x*x;
            if(x<2.0) return 1.3333333333333333 + x*(-2 + (1 - x/6.)*x);
            return 0;
            break;
            
        case 4:
            if(x<0.5) return 0.5989583333333334 + x*x*(-0.625 + x*x/4.);
            if(x<1.5) return 0.5729166666666666 + x*(0.20833333333333334 + x*(-1.25 + (0.8333333333333334 - x/6.)*x));
            if(x<2.5) return 1.6276041666666667 + x*(-2.6041666666666665 + x*(1.5625 + (-0.4166666666666667 + x/24.)*x));
            return 0;
            break;
            
        case 5:
            if(x<1.0) return 0.55 + x*x*(-0.5 + (0.25 - x/12.)*x*x);
            if(x<2.0) return 0.425 + x*(0.625 + x*(-1.75 + x*(1.25 + (-0.375 + x/24.)*x)));
            if(x<3.0) return 2.025 + x*(-3.375 + x*(2.25 + x*(-0.75 + (0.125 - x/120.)*x)));
            return 0;
            break;
            
        case 6:
            if(x<0.5) return 0.5110243055555556 + x*x*(-0.4010416666666667 + x*x*(0.14583333333333334 - x*x/36.));
            if(x<1.5) return 0.5117838541666667 + x*(-0.009114583333333334 + x*(-0.35546875 + x*(-0.12152777777777778 + x*(0.328125 + (-0.14583333333333334 + x/48.)*x))));
            if(x<2.5) return 0.17955729166666667 + x*(1.3197916666666667 + x*(-2.5703125 + x*(1.8472222222222223 + x*(-0.65625 + (0.11666666666666667 - x/120.)*x))));
            if(x<3.5) return 2.553146701388889 + x*(-4.376822916666667 + x*(3.1263020833333335 + x*(-1.1909722222222223 + x*(0.2552083333333333 + (-0.029166666666666667 + x/720.)*x))));
            return 0;
            break;
                  
        default:
            post("Error. Non implemented B-Spline chosen");
            return NAN;
    }
    
    return 0;
}

//Bspline Interpolation
pac_type_real bspline_inter(pac_type_real t, pac_type_real *py, int length, int order ){
    //returns x and y value as complex number
    if(py==NULL) return NAN;
    
    pac_type_real val=0;
    pac_type_real y, weight;
    int lowlimit=(int)(t-order/2.0);
    int uplimit=(int)(t+(order+1)/2.0);
    
    for(int a=lowlimit; a<=uplimit;++a){
        if(a<0) {
            y=py[0]; //constant extrapolation
        } else if (a>=length) { 
            //x=px[length-1]; y=py[length-1]; //constant extrapolation
            pac_type_real deltay=py[length-1]-py[length-2];            
            y=py[length-1]+deltay*(a-(length-1)); 
        } else {
            y=py[a];
        }
        weight=wabor_t_bspline_basis(order,t-a);
        val+=weight*y;
    }
    return val;
    
}


pac_type_real bspline_inter_inv( pac_type_real (*func)(pac_type_real _t, pac_type_real *_px, pac_type_real *_py, int _length, int _n),
        pac_type_real val, pac_type_real *px, pac_type_real *py, int length, int n,
        pac_type_real left, pac_type_real right, pac_type_real accuracy ){
    
    //Loest die Gleichung f(y)=val nach y auf. 
    //Startet zu suchen zwischen left und right
    if(right==left){
        right+=1;
        left-=1;
    }
    if(right<left){
        pac_type_real swapvar=right;
        right=left;
        left=swapvar;        
    }
    if(fabsPAC(func(left,px,py,length,n)-val)<accuracy) return left;
    if(fabsPAC(func(right,px,py,length,n)-val)<accuracy) return right;
    
    pac_type_real half,h,l,r;
    while(l=func(left,px,py,length,n),
          r=func(right,px,py,length,n),
            l<val && r<val || l>val && r>val) {
        left-=1;
        if(left<0){
            left*1.5;
        } else {
            left*0.75;
        }
        right+=1;
        if(right>0){
            right*1.5;
        } else {
            right*0.75;
        }
        if(left==-INFINITY||left==INFINITY||left==NAN||right==-INFINITY||right==INFINITY||right==NAN){
            return NAN;
        }
    }
    
    int iter=0;
#define MAXITER (100)
    l=func(left,px,py,length,n);
    while(++iter,
            half=(left+right)/2.0,
            h=func(half,px,py,length,n),
            fabsPAC(h-val)>accuracy){
        if(h<val && l<val || h>val && l>val ){
            left=half;
            l=h;
        } else {
            right=half;
        }
        half=(left+right)/2.0;
        if(iter>=MAXITER) {
            post("bspline_inter_inv: no convergence");
            return NAN;
        }
    }
    return half;    
}

pac_type bspline_inter2(pac_type_real t, pac_type_real *px, pac_type_real *py, int length, int order ){
    //returns x and y value as complex number
    if(px==NULL && py==NULL) return NAN;
    
    pac_type val=0+0*I;
    pac_type_real x,y, weight;
    int lowlimit=(int)(t-order/2.0);
    int uplimit=(int)(t+(order+1)/2.0);
    if(t-order<0){ --lowlimit;}
    
    for(int a=lowlimit; a<=uplimit;++a){
        if(a<0) {
            pac_type_real deltax=px[1]-px[0];
            pac_type_real deltay=py[1]-py[0]; 
            
            x=px[0]+deltax*a; //linear extrapolation
            y=py[0]+deltay*a; 

        } else if (a>=length) { 
            //x=px[length-1]; y=py[length-1]; //constant extrapolation
            pac_type_real deltax=px[length-1]-px[length-2];
            pac_type_real deltay=py[length-1]-py[length-2];            
            x=px[length-1]+deltax*(a-(length-1)); //linear extrapolation
            y=py[length-1]+deltay*(a-(length-1)); 
        } else {
            x=px[a]; y=py[a];
        }
        weight=wabor_t_bspline_basis(order,t-a);
        val+=weight*x+I*weight*y;
    }
    return val;
    
}

pac_type_real bspline_inter2_first(pac_type_real t, pac_type_real *px, pac_type_real *py, int length, int n ){
    return crealPAC(bspline_inter2(t, px, py, length, n));
}
pac_type_real bspline_inter2_second(pac_type_real t, pac_type_real *px, pac_type_real *py, int length, int n){
    return cimagPAC(bspline_inter2(t, px, py, length, n));
}


/* Lambert W function, -1 branch. 

   Keith Briggs 2012-01-16. Wojtek Slominski pointed out bug in case if (fabs(p)<1e-4); p is not set for z >= -1e-6.  Now fixed.
   K M Briggs 2001 Apr 09
   Keith dot Briggs at bt dot com
 * 
   Returned value W(z) satisfies W(z)*exp(W(z))=z
   Valid input range: -1/e <= z    < 0
   Output range:      -1   >= W(z) > -infinity

   See LambertW.c for principal branch.
   Together these form the only purely real branches.
*/

double LambertW1(const pac_type_real z) {
   static pac_type_real  last_z=-0.367879441171442; // -1/e
   static pac_type_real  last_ret=-1;
   if(z==last_z) {
       return last_ret;       
   }
   last_z=z;
    
  int i; 
  const pac_type_real eps=4.0e-16, em1=0.3678794411714423215955237701614608; 
  pac_type_real p=1.0,e,t,w,l1,l2;
  if (z<-em1 || z>=0.0 || isinf(z) || isnan(z)) { 
    post("LambertW1: bad argument %g, return 0",z); 
    last_ret=z;
    return last_ret;
    
  }
  /* initial approx for iteration... */
  if (z<-1e-6) { /* series about -1/e */
    p=-sqrt(2.0*(2.7182818284590452353602874713526625*z+1.0));
    w=-1.0+p*(1.0+p*(-0.333333333333333333333+p*0.152777777777777777777777)); 
  } else { /* asymptotic near zero */
    l1=log(-z);
    l2=log(-l1);
    w=l1-l2+l2/l1;
  }
  if (fabs(p)<1e-4) {
      last_ret=w;
      return last_ret;
  }
  for (i=0; i<10; i++) { /* Halley iteration */
    e=exp(w); 
    t=w*e-z;
    p=w+1.0;
    t/=e*p-0.5*(p+1.0)*t/p; 
    w-=t;
    if (fabs(t)<eps*(1.0+fabs(w))) { 
        last_ret=w;
        return last_ret; /* rel-abs error */
    }
  }
  /* should never get here */
  post("LambertW1: No convergence at z=%g, return 0",z); 
  last_ret=0;
  return last_ret;
  //exit(1);
}
