/*flutter_optmiser_gaussian.cpp*/
/*
* Copyright 2014 IPOL Image Processing On Line http://www.ipol.im/
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
/**
 * @file flutter_optimiser_gaussian.cpp
 * @brief Routines for flutter shutter code optimization,
 * gaussian  model. The functions in this file
 * implement equation (17).
 * @author Yohann Tendero <tendero@math.ucla.edu>
 */



#include <math.h>
#include "flutter_optimiser_gaussian.h"
/**
 * epsilon definition.
 */
#define eps  0.001//epsilon definition

#ifndef ABS
/**
 * absolute value definition.
 */
#define ABS(x)    (((x) > 0) ? (x) : (-(x)))
#endif


#ifndef M_PI
/**
 * M_PI is a POSIX definition for Pi
 */
#define M_PI 3.14159265358979323846
#endif



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// gaussian_optimiser ///////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn gaussian_optimiser(double* code,double s,int code_length,double deltat)
 * @brief Computes the a_k coefficients of the code; gaussian motion model.
 * @param double* code : array of doubles where code[k] contains the \alpha_k
 * This function implements equation (17):
 * \alpha_k= \int_{0}^{4\pi\sigma_{\mathcal N} \Delta t }
 * \frac{\sqrt[4]{\int_{\varepsilon}^{4\sigma_{\mathcal N}}
 * \exp\left({\frac{-v^2}{2\sigma_{\mathcal N}^2}}\right)
 * \frac{\mathbb{1}_{[-|v|\pi,|v| \pi]}
 * \left(\frac{\xi}{\Delta t} \right)}{|v|} dv} \cos\left(k\xi\right)}
 * {\sqrt{\mathrm{sinc}\left(\frac{\xi}{2 \pi}\right)}}d\xi.
 * (Recall that: sinc(x):=\frac{\sin(\pi x)}{\pi x}.)
 * @param double sigma : motion model parameter (sdt-dev here);
 * @param int code_length : length of the code L;
 * @param double deltat : double deltat : the temporal sampling of the
 * flutter shutter function;
 * @return void.
 */
void gaussian_optimiser(double* code,double sigma,int code_length,double deltat)
{
    /// This function calls "gaussian_integrand" with the approriate
    /// parameters : k, deltat, velocity_{max}, etc.

    double sum_code=0; // for the normalisation so that \int  \alpha (t) dt=1.
    // recall that \int \alpha(t)dt=\Deltat \sum \alpha_k.

    ///Main loop for the computation of a_k (code coefficient)
    for ( int k=0; k<code_length; k++)
    {

        ///Initalization for integral evaluation
        double a=0;
        double b=M_PI*(4*sigma)*deltat;
        if (b>M_PI) b=M_PI;
        // Recall that for the Gaussian model v_{max}=4sigma.
        double h=(b-a)/2;
        double s1=gaussian_integrand(sigma,deltat,a,k-round(code_length/2))+
                  gaussian_integrand(sigma,deltat,b,k-round(code_length/2));
        double s2=0;
        double s4=gaussian_integrand(sigma,deltat,a+h,
                                     k-round(code_length/2));
        double tn=h*(s1+4*s4)/3;
        double ta=tn+2*eps*tn;
        ///init so the first iteration of the main loop is always done.
        int zh=2;
        int j;

        ///Main loop for the integral evaluation
        while (ABS(ta-tn)>eps*ABS(tn))
        {
            ta=tn;
            zh=2*zh;
            h=h/2;
            s2=s2+s4;
            s4=0;
            j=1;
            while (j<=zh)
            {
                s4=s4+gaussian_integrand(sigma,deltat,a+j*h,
                                         k-round(code_length/2+.5));
                j=j+2;
            }
            tn = h*(s1+2*s2+4*s4)/3;
        }//End of the loop, integral evaluated

        code[k]=(double)tn; //storing the code.
        sum_code=sum_code+code[k];
    }

    for ( int k=0; k<code_length; k++)
        code[k]=code[k]/(deltat*sum_code);

}




////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// gaussian_integrand  /////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn gaussian_integrand(double sigma,double deltat, double xi, int k)
 * @brief Function to integrate in order to compute the a_k coefficient of
 * the code; gaussian motion model. This is the integrand in equation (17):
 * \frac{\sqrt[4]{\int_{\varepsilon}^{4\sigma_{\mathcal N}}
 * \exp\left({\frac{-v^2}{2\sigma_{\mathcal N}^2}}\right)
 * \frac{\mathbb{1}_{[-|v|\pi,|v| \pi]}
 * \left(\frac{\xi}{\Delta t} \right)}{|v|} dv} \cos\left(k\xi\right) }
 * {\sqrt{\mathrm{sinc}\left(\frac{\xi}{2 \pi}\right)}}
 * (Recall that: sinc(x):=\frac{\sin(\pi x)}{\pi x}.)
 * @param double s : motion model parameter (sdt-dev here);
 * @param double deltat : the temporal sampling of the
 * flutter shutter function;
 * @param double xi : frequency;
 * @param int k : the k-th coefficient of the code;
 * @return double.
 */
double gaussian_integrand(double sigma,double deltat,
                          double xi, int k)
{

    if (xi!=0)
    {
        return( pow(xi/(2*sin(xi/2)),0.5)*
                //This is \frac 1 {\sqrt \sinc (\frac  \xi {2\pi})}
                pow(gaussian_w(sigma,xi/deltat),0.25)*cos(k*xi));
    }
    else
    {
        return ( pow(gaussian_w(sigma,0),0.25));
    }


}








////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// Gaussian_w ///////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn gaussian_w(double sigma, double xi)
 * @brief Computes the W(\xi) function; gaussian motion model.
 * This is equation (15):
 * W(\xi):=\int_{\varepsilon}^{4\sigma_{\mathcal N}}
 * \exp\left({\frac{-v^2}{2\sigma_{\mathcal N}^2}}\right)
 * \frac{\mathbb{1}_{[-|v|\pi,|v| \pi]}(\xi)}{|v|} dv
 * @param double sigma : motion model parameter
 * (sdt-dev here);
 * @param double xi : frequency;
 * @return double W(\xi).
 */
double gaussian_w(double sigma, double xi)
{
    double velocity_max=4*sigma;
    if (ABS(xi)>velocity_max*M_PI) return(0);
    // to speed up some, as if ABS(xi)>velocity_max*M_PI*deltat by construction
    //the integrand is zero.
    double a=sigma/1000;//Avoids singularity, OK since the
    //function is in L^1 see alsopage 7 and Algorithm 1 step 3.
    double b=velocity_max;
    double h=(b-a)/2;
    double s1=gaussian_w_integrand(a,sigma,xi)+
              gaussian_w_integrand(b,sigma,xi);
    double s2=0;
    double s4=gaussian_w_integrand(a+h,sigma,xi);
    double tn=h*(s1+4*s4)/3;
    double ta=tn+2*eps*tn;
    ///init so the first iteration of the main loop is always done.
    int zh=2;
    int j;

    while (ABS(ta-tn)>eps*ABS(tn))
    {
        ta=tn;
        zh=2*zh;
        h=h/2;
        s2=s2+s4;
        s4=0;
        j=1;
        while (j<=zh)
        {
            s4=s4+gaussian_w_integrand(a+j*h,sigma,xi);
            j=j+2;
        }
        tn = h*(s1+2*s2+4*s4)/3;
    }
    return(tn);

}




////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// gaussian_w_integrand /////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn gaussian_w_integrand(double velocity, double sigma,
                            double xi)
 * @brief Function to integrate in velocity to get the W function;
 * gaussian motion model. This is the integrand in equation (13):
 * \exp\left({\frac{-v^2}{2\sigma_{\mathcal N}^2}}\right)
 * \frac{\mathbb{1}_{[-|v|\pi,|v| \pi]}(\xi)}{|v|}
 * @param double velocity : velocity;
 * @param double s : motion model parameter (sdt-dev here);
 * @param double xi : frequency;
 * @return double \frac{exp(-velocity*velocity/(2*s*s))
 * \mathbb{1]{[-M_PI*velocity,M_PI*velocity]}(velocity) }{|velocity s|}.
 */
double gaussian_w_integrand(double velocity, double sigma,
                            double xi)
{
    ///Notice that the normalization is
///not required since the code is defined up to a constant multiplication.
/// See also the integrand of (13) and remark 2, page 8.
    if (ABS(xi)<ABS(M_PI*velocity))
    {
        return(exp(-velocity*velocity/(2*sigma*sigma))/ABS(velocity));
    }
    else
    {
        return(0);
    }
}