/*@@@**************************************************************************
 * \file  dynProg4Freespace.cpp
 * \author Hernan Badino
 * \date  Wed March 7 16:51 CET 2007
 * \notes 
 *******************************************************************************
 ******************************************************************************/

/* INCLUDES */
#include <math.h>
#include <stdlib.h>

#include "dynProgOp.h"
#include "logger.h"

/* C FUNCTION PROTOTYPE */
#ifdef __cplusplus
extern "C" {
#endif
    int median_filter(int arr[], const int n);
#ifdef __cplusplus
}
#endif

using namespace VIC;

/* *************************** METHOD ************************************** */
/**
 * Standardconstructor.
 *
 *
 * \brief          Standardconstructor.
 * \author         Hernan Badino
 * \date           07.03.2007
 *
 * \note           Set the initial variables and allocate the object.
 * \remarks        -
 * \sa             -
 *
 * \param           (IN)   - .
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
CDynamicProgrammingOp::CDynamicProgrammingOp( const int f_width_i,
                                              const int f_height_i )
        : m_nodesImg (                                              ),
          m_width_i (                                     f_width_i ),
          m_height_i (                                   f_height_i ),
          m_distCost_f (                                        5.f ),
          m_predCost_f (                                       0.5f ),
          m_distTh_f (                                         10.f ),
          m_predTh_f (                                         10.f ),
          m_initialCost_f (                                     0.f ),
          m_minCostValue_f (                                   0.0f ),
          m_maxCostValue_f (                                  1.e9f ),
          m_expectedGradient_p (                               NULL ), 
          m_distCostVector_p (                                 NULL ),
          m_applyMedianFilter_b  (                             true ),
          m_medFiltHKSize_i (                                     9 ),
          m_pathTol_i (                                           5 )
{
    reinitialize();
}

CDynamicProgrammingOp::CDynamicProgrammingOp( )
        : m_nodesImg (                                              ),
          m_width_i (                                             8 ),
          m_height_i (                                            8 ),
          m_distCost_f (                                        5.f ),
          m_predCost_f (                                       0.5f ),
          m_distTh_f (                                         10.f ),
          m_predTh_f (                                         10.f ),
          m_initialCost_f (                                     0.f ),
          m_minCostValue_f (                                   0.0f ),
          m_maxCostValue_f (                                  1.e9f ),
          m_expectedGradient_p (                               NULL ), 
          m_distCostVector_p (                                 NULL ),
          m_applyMedianFilter_b  (                             true ),
          m_medFiltHKSize_i (                                     9 ),
          m_pathTol_i (                                           5 )
{
    reinitialize();
}
/* *************************** METHOD ************************************** */
/**
 * (Re)initialize the node structure.
 *
 *
 * \brief          (Re)initialize the node structure.
 * \author         Hernan Badino
 * \date           07.03.2007
 *
 * \remarks        -
 * \sa             -
 *
 * \param          - (IN)
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CDynamicProgrammingOp::reinitialize()
{
    m_nodesImg.freeMemory();
    m_nodesImg.setSize ( m_width_i, 
                         m_height_i );
    m_nodesImg.ensureAllocation();
    
    m_nodesImg.clear();
}


/* *************************** METHOD ************************************** */
/**
 * Destructor.
 *
 *
 * \brief          Destructor.
 * \author         Hernan Badino
 * \date           07.03.2007
 *
 * \note           -
 * \remarks        -
 * \sa             -
 *
 *************************************************************************** */
CDynamicProgrammingOp::~CDynamicProgrammingOp( )
{
    m_nodesImg.freeMemory();
}

/******************************/
/*    SET GET METHODS         */
/*****************************/
/* *************************** METHOD ************************************** */
/**
 * Sets the cost image size.
 *
 *
 * \brief          Sets the cost image size.
 * \author         Hernan Badino
 * \date           08.03.2007
 *
 * \note           - if current size is equal to new one no change 
 *                   is performed.
 * \sa             -
 *
 * \param          - f_width_i (IN) : new width of image.
 * \param          - f_height_i (IN): new height of image.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CDynamicProgrammingOp::setCostImageSize( const int f_width_i, 
                                              const int f_height_i )
{
    if ( f_width_i  != m_width_i || 
         f_height_i != m_height_i )
    {
        m_width_i  = f_width_i;
        m_height_i = f_height_i;
        reinitialize();
    }
}



/* *************************** METHOD ************************************** */
/**
 * Sets the expected gradient vector.
 *
 * \brief          Sets the expected gradient vector.
 * \author         Hernan Badino
 * \date           11.05.2009
 *
 * \note           This vector allows to define a gradient of the expected 
 *                 solution.
 * \sa             -
 *
 * \param[in]      -  new vector.
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CDynamicProgrammingOp::setExpectedGradient ( const float * const f_vec_p )
{ 
    m_expectedGradient_p = f_vec_p; 
}


/* *************************** METHOD ************************************** */
/**
 * Sets the distance cost vector.
 *
 * \brief          Sets the distance cost vector.
 * \author         Hernan Badino
 * \date           08.03.2007
 *
 * \note           This vector allows to define a different distance cost for 
 *                 every row. The element store in the position i specifies the 
 *                 cost of a jump from the row i to row i+1.
 * \sa             -
 *
 * \param[in]      -  new vector.
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CDynamicProgrammingOp::setDistCostVector ( const std::vector <float> * const f_vec_p )
{ 
    if ((int)f_vec_p->size() < m_width_i)
    {
        printf("Input vector length is too small: %i must be at least %i.",
               (int)f_vec_p -> size(),
               m_width_i );
        
        return;
    }
    
    
    m_distCostVector_p = f_vec_p; 
}

/******************************/
/*    COMPUTE METHODS         */
/******************************/

/* *************************** METHOD ************************************** */
/**
 * Computes dynammic programming with the input image and return 
 * as output the optimal path from left to right.
 *
 *
 * \brief          Computes dynammic programming with the input cost image.
 *
 * \author         Hernan Badino
 * \date           08.03.2007
 *
 * \note           -
 * \remarks        -
 * \sa             -
 *
 * \param          f_costImg_p (IN)    - Cost image.
 * \param          f_vecRes   (IN/OUT) - Prediction and output path.
 * \return         bool                - Success of computation.
 *
 *************************************************************************** */

bool 
CDynamicProgrammingOp::compute (  const CFloatImage      & f_costImg,
                                  std::vector<int>       & fr_vecRes,
                                  const std::vector<int> & f_followPath,
                                  const std::vector<int> & f_pathTolVector )
{
    if ( (int) fr_vecRes.size()      < m_height_i ||
         (int) f_costImg.getWidth()  < m_width_i || 
         (int) f_costImg.getHeight() < m_height_i )
    {
        printf("Size of input image or vector is too small.");
        SHOWVAL( fr_vecRes.size() );
        SHOWVAL( f_costImg.getWidth()  );
        SHOWVAL( f_costImg.getHeight() );
        SHOWVAL( m_width_i );
        SHOWVAL( m_height_i );
        return false;
    }

    if ( ( f_followPath.size() != 0 &&
           (int) f_followPath.size() < m_height_i ) ||
         ( f_pathTolVector.size() != 0 && 
           f_followPath.size() == 0 ) )
    {
        printf("Size of follow path vector is too small.");
        SHOWVAL(f_followPath.size () );
        SHOWVAL( f_costImg.getHeight() );
        return false;
    }

    if ( f_pathTolVector.size() != 0 &&
         (int) f_pathTolVector.size() < m_height_i ) 
    {
        printf("Size of follow path tolerance vector is too small.");
        SHOWVAL( f_pathTolVector.size () );
        SHOWVAL( f_costImg.getHeight() );
        return false;
    }

    if ( m_auxVector.size() < fr_vecRes.size() )
    {
        m_auxVector.resize( fr_vecRes.size() );
    }

    int      i, j, k;
    float    newCost_f;
    float    minAccCost_f = 0;
    float    node2nodeDist_f;
    int      startCol_i = 0, endCol_i = m_width_i - 1;
    int      startColPrevRow_i = 0, endColPrevRow_i = m_width_i - 1;
    int      startColThisRow_i, endColThisRow_i;
    int      tolerance_i = m_pathTol_i;

    /// Set the current expected gradient of the solution to 0.
    float    currGrad_f = 0.;

    /// Set the jump cost equal to the default distance cost.
    float    jumpCost_f = m_distCost_f;
    const std::vector<float> jumpCostVec = m_distCostVector_p?*m_distCostVector_p:std::vector<float>(0);

    // Lets start with the first row.
    i = 0;

    // Set first limits to startCol and endCol if required.
    if ( f_followPath.size() )
    {
        if (f_pathTolVector.size())
            tolerance_i = f_pathTolVector[i];

        startCol_i = f_followPath[i] - tolerance_i;
        if (startCol_i < 0 || startCol_i >= m_width_i ) startCol_i = 0;

        endCol_i = f_followPath[i] + tolerance_i;
        if (endCol_i < 0 || endCol_i >= m_width_i) endCol_i = m_width_i - 1;

    }

    // Let set a flag indicating that the first row has not stil been found.
    startColThisRow_i= -1;
    endColThisRow_i  = -1;

    //printf("Starting in col %i ending in col %i (init)\n", 
    //           startCol_i, endCol_i );
    for (j = startCol_i; j <= endCol_i; ++j)
    {
        // Check first if this cost can be considered as valid.
        if ( f_costImg.getScanline(i)[j] > m_minCostValue_f )
        {
            // Check first if this cost can be considered as valid.
            if ( f_costImg.getScanline(i)[j] <= m_maxCostValue_f )
                m_nodesImg.getScanline(i)[j].m_nodCost_f = f_costImg.getScanline(i)[j];
            else
                m_nodesImg.getScanline(i)[j].m_nodCost_f = m_maxCostValue_f;
        }
        else
            m_nodesImg.getScanline(i)[j].m_nodCost_f = m_maxCostValue_f;

        // Up to now the last row is this row.
        endColThisRow_i = j;

        if ( startColThisRow_i < 0 ) 
        {
            // For the first column we reduce some cost in order to be robust against noise
            // in the same row.
            m_nodesImg.getScanline(i)[j].m_nodCost_f -= m_initialCost_f;

            startColThisRow_i = j;
        }        
        
        // Add prediction cost with saturation.
        if ( fr_vecRes[i] >= 0 && m_predCost_f )
        {
            node2nodeDist_f = fabs(fr_vecRes[i] - j);
            
            m_nodesImg.getScanline(i)[j].m_nodCost_f += m_predCost_f * std::min(node2nodeDist_f, m_predTh_f);
        }

        // Update accumulated cost = current cost.
        m_nodesImg.getScanline(i)[j].m_accCost_f = m_nodesImg.getScanline(i)[j].m_nodCost_f;

        // Parent node is the base node.
        m_nodesImg.getScanline(i)[j].m_parNode_i = -1;
    }

    if (startColThisRow_i == -1)
    {
        m_nodesImg.getScanline(i)[startCol_i].m_parNode_i = -1;
        m_nodesImg.getScanline(i)[startCol_i].m_nodCost_f = 0;
        m_nodesImg.getScanline(i)[startCol_i].m_accCost_f = 0;

        startColThisRow_i = endColThisRow_i = startCol_i;
        //return false;
    }

    for (++i; i < m_height_i; ++i)
    {
        if (m_expectedGradient_p)
            currGrad_f = m_expectedGradient_p[i];

        // Store the first valid node of the previous row in an auxiliar variable.
        // This will be use to set the default parent node in the current row.
        startColPrevRow_i = startColThisRow_i;
        endColPrevRow_i   = endColThisRow_i;
        
        // Set firstlimits to startCol and endCol if required.
        if ( f_followPath.size() )
        {
            if (f_pathTolVector.size())
                tolerance_i = f_pathTolVector[i];

            startCol_i = f_followPath[i] - tolerance_i;
            if (startCol_i < 0 || startCol_i >= m_width_i ) startCol_i = 0;
            
            endCol_i = f_followPath[i] + tolerance_i;
            if (endCol_i < 0 || endCol_i >= m_width_i) endCol_i = m_width_i - 1;
        }
        
        //printf("Starting in col %i ending in col %i (prev start: %i prev end: %i)\n", 
        //       startCol_i, endCol_i, startColThisRow_i, endColThisRow_i );

        // Let set a flag indicating that the first row has not stil been found.
        startColThisRow_i = -1;
        endColThisRow_i   = -1;

        for (j = startCol_i; j <= endCol_i; ++j)
        {
            // Check first if this cost can be considered as valid.
            if ( f_costImg.getScanline(i)[j] < m_minCostValue_f ||
                 f_costImg.getScanline(i)[j] > m_maxCostValue_f )
                continue;

            // Check first if this cost can be considered as valid.
            if ( f_costImg.getScanline(i)[j] > m_minCostValue_f )
            {
                // Check first if this cost can be considered as valid.
                if ( f_costImg.getScanline(i)[j] <= m_maxCostValue_f )
                    m_nodesImg.getScanline(i)[j].m_nodCost_f = f_costImg.getScanline(i)[j];
                else
                    m_nodesImg.getScanline(i)[j].m_nodCost_f = m_maxCostValue_f;
            }
            else
                m_nodesImg.getScanline(i)[j].m_nodCost_f = m_maxCostValue_f;
            
            // Up to now the last row is this row.
            endColThisRow_i = j;

            if ( startColThisRow_i < 0 ) 
            {
                // For the first column we reduce some cost in order to be robust against noise
                // in the same row.
                m_nodesImg.getScanline(i)[j].m_nodCost_f -= m_initialCost_f;
                
                startColThisRow_i = j;
            }

            // Add prediction cost with saturation.
            if ( fr_vecRes[i] >= 0 && m_predCost_f )
            {
                node2nodeDist_f = fabs(fr_vecRes[i] - j);
                m_nodesImg.getScanline(i)[j].m_nodCost_f += m_predCost_f * std::min(node2nodeDist_f, m_predTh_f);
                /*
                printf("%i, %i Adding prediction cost %f for dist %f and pred %i\n",
                       i, j, 
                       m_predCost_f * std::min(node2nodeDist_f, m_predTh_f),
                       node2nodeDist_f,
                       fr_vecRes[i] );
                */
            }                    

            // Per default the parent node is the first one, i.e.
            // the node corresponding to first valid cell in the 
            // previous row.
            m_nodesImg.getScanline(i)[j].m_parNode_i  = startColPrevRow_i;

            // Initial cost (of the current parent node).

            if (jumpCostVec.size())
                jumpCost_f = jumpCostVec[i-1];

            node2nodeDist_f = fabs(j - startColPrevRow_i - currGrad_f);

            newCost_f = ( // Accumulated Cost.
                    m_nodesImg.getScanline(i-1)[startColPrevRow_i].m_accCost_f + 
                    // Local Cost.
                    m_nodesImg.getScanline(i  )[j].m_nodCost_f +  
                    // Smoothness cost with saturation.
                    jumpCost_f * std::min(node2nodeDist_f, m_distTh_f) ); 

            m_nodesImg.getScanline(i)[j].m_accCost_f = newCost_f;

            for (k = startColPrevRow_i+1; k <= endColPrevRow_i; ++k)
            {
                if (jumpCostVec.size())
                    jumpCost_f = jumpCostVec[i-1];
                //printf("%i, %i %i jumpCost_f = %f (size vec: %i)\n", i,j,k, jumpCost_f, jumpCostVec.size());
                
                // then compute new cost...
                node2nodeDist_f = fabs(j - k - currGrad_f);
                newCost_f = ( 
                        // Accumulated Cost. 
                        m_nodesImg.getScanline(i-1)[k].m_accCost_f +
                        // Local Cost.
                        m_nodesImg.getScanline(i  )[j].m_nodCost_f +
                        // Smoothness cost with saturation.
                        jumpCost_f * std::min(node2nodeDist_f, m_distTh_f)  );

                // and check if it is better than the current one.
                if ( newCost_f < m_nodesImg.getScanline(i)[j].m_accCost_f )
                {
                    m_nodesImg.getScanline(i)[j].m_accCost_f = newCost_f;
                    m_nodesImg.getScanline(i)[j].m_parNode_i = k;
                    
                }
            }
        }

        if (startColThisRow_i == -1)
        {
            printf("Warning:\n");
            SHOWVAL(startCol_i);
            SHOWVAL(endCol_i);

            m_nodesImg.getScanline(i)[startCol_i].m_parNode_i = startColPrevRow_i;
            m_nodesImg.getScanline(i)[startCol_i].m_nodCost_f = m_nodesImg.getScanline(i-1)[startColPrevRow_i].m_nodCost_f;
            m_nodesImg.getScanline(i)[startCol_i].m_accCost_f = m_nodesImg.getScanline(i-1)[startColPrevRow_i].m_nodCost_f;

            startColThisRow_i = endColThisRow_i = startCol_i;
            //return false;
        }   
    }

    // Lets search now the best path (lowest cost). This could have been done
    // inside the previous cycle by checking the smallest cost if i == m_height_i - 1.
    // However, the computation time difference should be very very small and the 
    // code is more readable.
    --i; //i = m_height_i - 1;

    // We can reuse the startColThisRow and endColThisRow, since they are still valid.
    int      f_bestNode_i = -1;
    bool     f_uninit_b = true;
    
    for (j = startColThisRow_i; j <= endColThisRow_i; ++j)
    {
        // is this node valid?
            // Check first if this cost can be considered as valid.
        //if ( f_costImg.getScanline(i)[j] >= m_minCostValue_f && 
        //     f_costImg.getScanline(i)[j] <= m_maxCostValue_f )
        //if (f_costImg.getScanline(i)[j] != 0)
        {
            if ( m_nodesImg.getScanline(i)[j].m_accCost_f < minAccCost_f || 
                 f_uninit_b )
            {
                f_uninit_b = false;
                minAccCost_f  = m_nodesImg.getScanline(i)[j].m_accCost_f;
                f_bestNode_i = j;
            }
        }        
    }

    // This means no valid cell in the last row.
    if (f_bestNode_i == -1)
    {
        printf("No valid cell in last row.");
        return false;
    }
    
    // Get the best path from bottom to top.

    for (; i >=0; --i)
    {        
        m_auxVector[i] = f_bestNode_i;
        f_bestNode_i = m_nodesImg.getScanline(i)[f_bestNode_i].m_parNode_i;

        if (i > 0 && (f_bestNode_i < 0 || f_bestNode_i >= m_width_i))
        {
            printf("This shouldn't happen.");
            printf("error i = %i f_bestNode_i = %i, width = %i\n", i,
                   f_bestNode_i, m_width_i );
            //exit(1);
        }
    }

    if (m_applyMedianFilter_b)
    {
        int f_array_a[DP4F_MAX_MEDFILT_KERNEL_SIZE];
        int f_filtSize_i = m_medFiltHKSize_i * 2 + 1;
        int f_lastElem_i = m_height_i-m_medFiltHKSize_i;
        int f_min_i, f_max_i;
        f_min_i = 0;
        f_max_i = f_filtSize_i-1;

        // Copy left part of vector.
        for (i = 0; i < m_medFiltHKSize_i; ++i)
            fr_vecRes[i] = m_auxVector[i];
       
        while (i < f_lastElem_i)
        {   
            for (j = f_min_i; j <= f_max_i; ++j)
                f_array_a[j-f_min_i] = m_auxVector[j];

            fr_vecRes[i] = median_filter(f_array_a, f_filtSize_i);
            
            ++i;
            ++f_min_i;
            ++f_max_i;
        }

        // Copy right part of vector.
        for (; i < m_height_i; ++i)
            fr_vecRes[i] = m_auxVector[i];
    }
    else
    {
        for (unsigned int i = 0; i < m_auxVector.size(); ++i)
        {
            fr_vecRes[i] = m_auxVector[i];
        }
    }

    return true;
}



/******************************/
/*          DISPLAY           */
/******************************/

/* *************************** METHOD ************************************** */
/**
 * Show the cost of each node as a color encoded image.
 *
 *
 * \brief          Show the cost of each node as a color encoded image.
 * \author         Hernan Badino
 * \date           02.05.2007
 *
 * \note           -
 * \remarks        -
 * \sa             -
 *
 * \param          f_disp_y (IN)     - display where to show the image.
 * \param          f_sY_i  (IN)    - start row of the display.
 * \param          f_sX_i  (IN)    - start colum n of the display.
 * \param          f_level_i       - level to show the overlay.  
 * \param          f_maxVal_f (IN) - max value for color scale.
 * \param          f_encode_red (IN) - encode red?
 * \return         void
 *
 *************************************************************************** */
#if 0
void 
CDynamicProgrammingOp::showCostImageOvl ( const CDrawingList & f_drawList ) const
{
    int i, j;
    int f_Brightness_i;

    const float f_scaleX_f = f_disp_p->GetWidth()  / (float)m_width_i ;
    const float f_scaleY_f = f_disp_p->GetHeight() / (float)m_height_i ;

    float f_maxValAcc_f  = -1.f;
    float f_maxValNod_f  = -1.f;

    // Search for maximum.
    for (i = 0; i < m_height_i; ++i)
    {
        for (j = 0; j < m_width_i; ++j)
        {
            if ( m_nodesImg.getScanline(i)[j].m_nodCost_f > 0)
            {
                if (f_maxValAcc_f < 1.f/m_nodesImg.getScanline(i)[j].m_accCost_f ||
                    f_maxValAcc_f < 0 )
                    f_maxValAcc_f = 1./m_nodesImg.getScanline(i)[j].m_accCost_f;

                if ( f_maxValNod_f < 1.f/m_nodesImg.getScanline(i)[j].m_nodCost_f ||
                     f_maxValNod_f < 0 )
                    f_maxValNod_f = 1./m_nodesImg.getScanline(i)[j].m_nodCost_f;
            }
        }
    }
    
    if (f_maxValAcc_f <= 0.f ||
        f_maxValNod_f <= 0.f )
    {
        printf("Maximum cost value is 0.");
        return;
    }

    if (f_clear_b)
    {
        f_disp_p -> ClearOvl( f_sY_i, 
                              f_sX_i,
                              f_level_i );

        f_disp_p->ClearScreen ( f_sY_i, 
                                f_sX_i,
                                0 );
    }

    for (i = 0; i < m_height_i; ++i)
    {
        for (j = 0; j < m_width_i; ++j)
        {
            //if ( m_nodesImg.getScanline(i)[j].m_nodCost_f <= 0 ) continue;

            f_Brightness_i = (int) ( 192 * ( 1.f/m_nodesImg.getScanline(i)[j].m_nodCost_f / 
                                             f_maxValNod_f ) ) + 63;
            f_Brightness_i = std::min(std::max(f_Brightness_i, 0), 255);

            if (m_auxVector[i] == j)
                f_disp_p -> SetColor ( f_Brightness_i, 0, 0  );
            else
                f_disp_p -> SetColor ( 0, f_Brightness_i, 0  );

            f_disp_p -> FilledRectOvl ( f_sY_i,
                                        f_sX_i,
                                        (j-.5f) * f_scaleX_f,
                                        (i-.5f) * f_scaleY_f,
                                        f_scaleX_f / 2,
                                        f_scaleY_f,
                                        f_level_i );

            f_Brightness_i = (int) ( 192 * ( 1.f/m_nodesImg.getScanline(i)[j].m_accCost_f / 
                                             f_maxValAcc_f ) ) + 63;
            f_Brightness_i = std::min(std::max(f_Brightness_i, 0), 255);

            f_disp_p -> SetColor ( 0, 0, f_Brightness_i );

            f_disp_p -> FilledRectOvl ( f_sY_i,
                                        f_sX_i,
                                        j * f_scaleX_f,
                                        (i-.5f) * f_scaleY_f - .5f,
                                        f_scaleX_f / 2,
                                        f_scaleY_f,
                                        f_level_i );
        }
    }
}

#endif
