/*optimal_snapshot.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 optimal_snapshot.cpp
 * @brief Routines for optimal snapshot computation.
 * It finds the optimal exposure time for the standard camera
 * assuming a motion model by minimizing :
 * Equation (20).
 * This corresponds to Algorithm 1 step 1.
 * @author Yohann Tendero <tendero@math.ucla.edu>
 */



#include <math.h>
#include "optimal_snapshot.h"


#define eps   0.001//epsilon definition, for integral evaluation.


#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






////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// Optimal snapshot computation /////////////////////
///////////////////////////////	Dummy minimization of E(T) /////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn optimal_snapshot(int flag_motion_model,
                             double motion_model_parameter)
 * @brief Given a motion model compute the optimal exposure time for a snaphot.
 * The minimization is done by scanning on T the energy
 * defined by equation (20):
 * E(T)=\int_0^{|v_{max}|} \int_{0}^{\pi}
 *\frac{1 }{T\mathrm{sinc}^2\left(\frac{\xi v T}{2 \pi}\right)}\rho(v)d\xi dv
 * Arguments: Motion model and parameters.
 * @param int flag_motion_model : motion model selection
 * -0 for truncated Gaussian of std-dev motion_model_parameter;
 * -1 For uniform of range motion_model_parameter;
 * @param double motion_model_parameter : motion model parameter.
 * @return T^* : the optimal exposure of the snapshot i.e., the argmin of E(T)
 * for the given motion model (Algorithm 1 step 1).
 */

double optimal_snapshot(int flag_motion_model,
                        double motion_model_parameter)
{
    ///This function minimizes (20) by scanning the $T$ values.
    //initialization
    double T_star=0; //$T^*$ in the paper.
    double optimal_Energy=optimal_snapshot_energy(0, flag_motion_model,
                          motion_model_parameter);
    double current_Energy=optimal_Energy;
    double velocity_max=motion_model_parameter;
    ///The following is for the (truncated) Gaussian at 4*motion_model_parameter
    if (flag_motion_model==0) velocity_max=4*motion_model_parameter;


    // main loop for enegy minimization : scanning on \Deltat values
    // see algo 1, step 1. (Note that velocity_max is positive.)
    for (double T=(2/velocity_max)/100; //start : \frac{0.02}{v_{max}}
            T<1.999/velocity_max;  //stop \frac{1.999}{v_{max}}
            T=T+(2/velocity_max)/100)// step \frac{0.02}{v_{max}}
    {
        current_Energy=optimal_snapshot_energy(T,
                                               flag_motion_model,
                                               motion_model_parameter);
        if (current_Energy<optimal_Energy)
        {
            optimal_Energy=current_Energy;
            T_star=T;
        }
    }
    return(T_star);

}




////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// Computation of E(T) //////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn double optimal_snapshot_energy(double T, int flag_motion_model,
                               double motion_model_parameter)
 * @brief Given a motion model computes the energy of Equation (20) for
 * a given Tt.
 * @param double T : exposure time of the
 * @param int flag_motion_model : motion model selection
 * -0 for truncated Gaussian of std-dev motion_model_parameter;
 * -1 For uniform of range motion_model_parameter;
 * @return E(T) (see Equation (20)).
 */
double optimal_snapshot_energy(double T, int flag_motion_model,
                               double motion_model_parameter)
{
    /// Computes E(\tilde \Deltat) depending on \rho (the motion model)

    double velocity_max=motion_model_parameter;
    if (flag_motion_model==0) velocity_max=4*motion_model_parameter;
    // flag_motion_model=0 if the motion model is (truncated) Gaussian.

    /// The following evaluate the integrals using Algorithm 3
    /// with a=0, b=velocity_max, eps=0.001 (see line 34 of this file)

    ///Initalization.
    double a=0;// Using parity.
    double b=velocity_max;
    double h=(b-a)/2;
    double s1=optimal_snapshot_integrand(a,T)*
              proba_motion_model(a,motion_model_parameter,
                                 flag_motion_model)+
              optimal_snapshot_integrand(b,T)*
              proba_motion_model(b,motion_model_parameter,
                                 flag_motion_model);
    double s2=0;
    double s4=optimal_snapshot_integrand(a+h,T)*
              proba_motion_model(a+h,motion_model_parameter,
                                 flag_motion_model);
    double tn=h*(s1+4*s4)/3;
    double ta=tn+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+optimal_snapshot_integrand(a+j*h,T)*
               proba_motion_model(a+j*h,motion_model_parameter,
                                  flag_motion_model);
            j=j+2;
        }
        tn = h*(s1+2*s2+4*s4)/3;
    }
    return(tn);

}


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// optimal snpashot integrand ///////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn optimal_snapshot_integrand(double velocity, double T)
 * @brief Given a velocity v and T this function returns
 * the integral in \xi of equation (20), i.e.,
 * \int_0^\pi \frac{1}{T \mathrm{sinc}^2\left(\frac{\xi v T}{2\pi}\right)}d\xi
 * (Recall that: sinc(x):=\frac{\sin(\pi x)}{\pi x}.)
 * @param double T : exposure time
 * @param double velocity : velocity.
 * @return \int_0^\pi \frac{1}
 * {T \mathrm{sinc}^2\left(\frac{\xi v T}{2\pi}\right)}d\xi
 */
double optimal_snapshot_integrand(double velocity, double T)
{

    /// The following evaluate the integrals using Algorithm 3
    /// with a=0, b=\pi, eps=0.001 (see line 34 of this file)
    double a=0;
    double b=M_PI;
    double h=(b-a)/2;
    double s1=optimal_snapshot_integrand_part2(a,velocity,T)+
              optimal_snapshot_integrand_part2(b,velocity,T);
    double s2=0;
    double s4=optimal_snapshot_integrand_part2(a+h,velocity,T);
    double tn=h*(s1+4*s4)/3;
    double ta=tn+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+optimal_snapshot_integrand_part2(a+j*h,velocity,T);
            j=j+2;
        }
        tn = h*(s1+2*s2+4*s4)/3;
    }
    return(2*tn);
}



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////optimal snpashot integrand part2  //////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn odouble optimal_snapshot_integrand_part2(double xi, double velocity,
                                  double T)
 * @brief This function implements a part of the integrand in (20).
 * @param double xi : frequency
 * @param double velocity : velocity
 * @param double T : exposure time
 * @return $ \frac{1 }{T \mathrm{sinc}^2\left( \frac{\xi v T}{2 \pi}\right) }$
 * (Recall that: sinc(x):=\frac{\sin(\pi x)}{\pi x}.)
 */
double optimal_snapshot_integrand_part2(double xi, double velocity,
                                        double T)
{
    double xivT_2=xi*velocity*T/2;
    double sinc=1;
    if (xi*velocity!=0)
    {
        sinc=sin(xivT_2)/xivT_2;
    }
    if (T!=0 && sinc!=0)
    {
        return(1/(T*sinc*sinc));
    }
    else
    {
        return(99999); //Dummy case, called if deltat=0 (the energy is infinite)
    }

}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///////////////////////////// Probability of a velocity velocity \rho(v) ///////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/**
 * @fn proba_motion_model(double velocity, double motion_model_parameter,
                         int flag_motion_model)
 * @brief Given a motion model compute the probability of the
 * velocity velocity (\rho(velocity))
 * @param double velocity : velocity
 * @param int flag_motion_model : motion model selection
 * -0 for truncated Gaussian of std-dev motion_model_parameter;
 * -1 For uniform of range motion_model_parameter;
 * @param double motion_model_parameter : motion model parameter
 * @return \rho(v).
 */
double proba_motion_model(double velocity, double motion_model_parameter,
                          int flag_motion_model)
{

    if (flag_motion_model==0)
        return(exp(-velocity*velocity/
                   (2*motion_model_parameter*motion_model_parameter)));
    ///Normalization is unnecessary; it'll lead to a constant multiplication
    /// (See also, Algorithm 1 step1). ř
    else /// This means that (flag_motion_model==1)  is true
        return(1/(2*motion_model_parameter));
}