/*flutter_optimiser_uniform.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_uniform.cpp
 * @brief Routines for flutter shutter code optimization,
 * uniform motion model. The functions in this file
 * implement equation (18).
 * @author Yohann Tendero <tendero@math.ucla.edu>
 */


#include <math.h>
#include "flutter_optimiser_uniform.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


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// Uniform_optimiser ////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn uniform_optimiser(double* code double velocity_max,
 *                       int code_length, double deltat)
 * @brief Computes the a_k coefficients of the code; uniform motion model.
 * @param double* code : array of doubles where code[k] contains the \alpha_k
 * This function implements equation (18):
 *  \alpha_k= \int_{0}^{\pi |v_{max}| \Delta t }
 * \frac{\sqrt[4]{\int_{\varepsilon}^{|v_{max}|}
 * \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
 * @param double velocity_max : motion model parameter;
 * @param int code_length : length of the code L;
 * @param double deltat : the temporal sampling of the
 * flutter shutter function;
 * @return void.
 */
void uniform_optimiser(double* code, double velocity_max,
                       int code_length, double deltat)
{
    /// This function just calls "uniform_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 :
    for ( int k=0; k<code_length; k++)
    {
        ///Initalization for integral evaluation
        double a=0;
        double b=M_PI*velocity_max*deltat; // \pi v_{max} \Deltat
        if (b>M_PI) b=M_PI;
        double h=(b-a)/2;
        double s1=uniform_integrand(velocity_max,deltat,a,
                                    k-round(code_length/2))+
                  uniform_integrand(velocity_max,deltat,b,
                                    k-round(code_length/2));
        double s2=0;
        double s4=uniform_integrand(velocity_max,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+uniform_integrand(velocity_max,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];
    }
    // loop for code normalization.
    for ( int k=0; k<code_length; k++)
        code[k]=code[k]/(deltat*sum_code);
}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// Uniform_integrand ////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn uniform_integrand(double velocity_max,double deltat, double xi,
                         int k)
 * @brief Function to integrate (in \xi) in order to compute the a_k
 * coefficient of the code; uniform motion model.
 * This is the integrand of 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 velocity_max : maximum velocity;
 * @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 , see the integrand of (17)
 */
double uniform_integrand(double velocity_max,double deltat, double xi,
                         int k)
{
///Construct the function to be integrated in order to compute the a_k:


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

        return ( pow(uniform_w(velocity_max,0),0.25)) ;
    }

}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// uniform_w ////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn uniform_w(double velocity_max,double deltat, double xi)
 * @brief Computes the w(\xi) function; uniform motion model.
 * This is equation (16):
 * W(\xi)=\int_{\varepsilon}^{|v_{max}|}
 * \frac{\mathbb{1}_{[-|v|\pi,|v| \pi]}(\xi)}{|v|} dv
 * @param double velocity_max : range of the uniform
 * (maximum velocity);
 * @param double xi : frequency;
 * @return double w(\xi), see equation (16), and algorithm 1 step 3.
 */
double uniform_w(double velocity_max, double xi)
/// Compute the weight function \W(\xi) for a uniform velocity.
{
    /// Construct the \W function for an uniform motion model,

///Initalization.
    if (ABS(xi)>velocity_max*M_PI) return(0);
    // to speed up some: indeed, the function w defined in equation (16)
    // is supported on $[-\pi |v_{max}, \pi |v_{max}|]$.

    //(ELSE) in the support of $\rho$

    /// The following evaluate the integrals using Algorithm 3
    /// with a=velocity_max/1000,
    /// b=velocity_max (see the above comment on the support of \rho)
    /// and eps=0.001 (see line 32 of this file)

    double a=velocity_max/1000;//Avoids singularity, OK since
    //the function is in L^1 see also remark 1 page 8 and Algorithm 1 step 3.
    double b=velocity_max;

    double h=(b-a)/2;
    double s1=uniform_w_integrand(a,xi)+
              uniform_w_integrand(b,xi);
    double s2=0;
    double s4=uniform_w_integrand(a+h,xi);
    double tn=h*(s1+4*s4)/3;
    double ta=tn+2*eps*tn;
    int zh=2;
    int j;

///Main loop for evaluation of \int
    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+uniform_w_integrand(a+j*h,xi);
            j=j+2;
        }
        tn = h*(s1+2*s2+4*s4)/3;
    }
    return(tn);

}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// uniform_w_integrand //////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn uniform_w_integrand(double velocity, double xi)
 * @brief Function to integrate in velocity to get the w function;
 * uniform motion model. This is the integrand in equation (16):
 * \frac{\mathbb{1}_{[-|v|\pi,|v| \pi]}(\xi)}{|v|}
 * @param double velocity : velocity;
 * @param double xi : frequency;
 * @return $\frac{\mathbb{1}_{[-|v|\pi,|v| \pi]}(\xi)}{|v|}$
 */
double uniform_w_integrand(double velocity, double xi)
///Unormalized density of probility of velocity.
///Notice that the normalization is
///not required since the code is defined up to a constant multiplication.
/// See also the integrand of (16) and remark 2, page 8.
{

    if (ABS(xi)<ABS(M_PI*velocity))
    {
        return(1/(ABS(velocity)));
    }
    else return(0);
}