/* 
* 
* Copyright (C) 2013 UMPC
* 
* This file is part of CLaRiNet.
*  
* CLaRiNet 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.
* 
* CLaRiNet 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 CLaRiNet.  If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <stddef.h>
#include "GeoMorph.h"
#include "Erreur.h"
#include "FloatRaster.h"
#include "Hausdorff.h"
#include "esriheader.h"
#include <limits>
#include <map>
#include <set>

using namespace std;

/*******************************************************************************
 * 	                Fonction "EuclideanDistanceMap" :                      *
 *                                                                             *
 * Calcul d'une carte de distance euclidienne a un ensemble de pixels definis  *
 * par Indic > seuil. Algorithme tire de :                                     *
 * Felzenszwalb, P. F., and D. P. Huttenlocher (2004), Distance transforms of  *
 * sampled functions, tech. note, Cornell Computing and Information Science    *
 *******************************************************************************/

int EuclideanDistanceMap ( int HandleIndic , float thresh_indic , int HandleEDM )
{
	double s,infinity,d;
	int q,k,nr,nc,i_infinity,s1,s2,ncols,nrows;
        FloatRaster * Indic , * EDM ;
        int ** imap ;
        int * vcol , * vrow , * transfocol , * transforow ;
        double * zcol , * zrow ;

        if ( !(Indic = GetFloatRaster(HandleIndic)) )
        {
            SetErreur("EuclideanDistanceMap : raster # %d (indicator) does not exist\n",HandleIndic);
            return(-1);
        }

        if ( !(EDM = GetFloatRaster(HandleEDM)) )
        {
            SetErreur("EuclideanDistanceMap : raster # %d (distance map) does not exist\n",HandleEDM);
            return(-1);
        }

        if ( CheckSameHeader( &(Indic->Header) , &(EDM->Header) ) )
        {
            SetErreur("EuclideanDistanceMap : rasters # %d (indicator) et # %d (distance map to be filled) do not have identical referencing headers\n",HandleIndic, HandleEDM);
            return(-1);
        }

        ncols = (Indic->Header).ncols ;
        nrows = (Indic->Header).nrows ;

	infinity = numeric_limits<double>::infinity();
	i_infinity = nrows*nrows + ncols*ncols;

        imap = (int **) malloc ( nrows * sizeof(int *) );

        for (nr=0;nr<nrows;nr++)
        {
            imap[nr] = (int *) malloc ( ncols * sizeof(int) );
        }

        // Allocation des tableaux de travail

        vcol = (int *) malloc ( nrows * sizeof(int));
        vrow = (int *) malloc ( ncols * sizeof(int));
        zcol = (double *) malloc ( (nrows+1) * sizeof(double));
        zrow = (double *) malloc ( (ncols+1) * sizeof(double));
        transfocol = (int *) malloc ( nrows * sizeof(int));
        transforow = (int *) malloc ( ncols * sizeof(int));

	// Initialisation de la carte imap Ã  l'aide de l'indicatrice -----------
        // Cout nul si indic > thresh_indic , infini sinon

	for(nr=0;nr<nrows;nr++)
        {   for(nc=0;nc<ncols;nc++)
            {
                if ( (Indic->Data)[nr][nc] > thresh_indic  )
                {
                    imap[nr][nc] = 0 ;
                }
                else
                {
                    imap[nr][nc] = i_infinity ;
                }
            }
        }

	// TransformÃ©e selon les colonnes --------------------------------------

	for(nc=0;nc<ncols;nc++)
	{
		k=0;
		vcol[0]=0;
		zcol[0]=-1.0*infinity;
		zcol[1]=infinity;

		// Calcul de l'enveloppe infÃ©rieure

		for(q=1;q<nrows;q++)
		{
test_s_col:
			if(imap[q][nc]<i_infinity)
			{
				s1=imap[q][nc]-imap[vcol[k]][nc];                                
				s2=q*q-vcol[k]*vcol[k];
				s=((double)(s1+s2))/((double)(2*q-2*vcol[k]));
			}
			else
				s=infinity;

			if(s<zcol[k])
			{
				k--;
				goto test_s_col;
			}
			else
			{
				k++;
				vcol[k]=q;
				zcol[k]=s;
				zcol[k+1]=infinity;
			}
		}

		// Remplissage des distances

		k=0;
		for(q=0;q<nrows;q++)
		{
			// Ecriture de la colonne prÃ©cÃ©dente dans imap

			if(nc>0)
				imap[q][nc-1]=transfocol[q];

			// Calcul de la colonne courante

			while(zcol[k+1]<q)
				k++;

			if ( imap[vcol[k]][nc] < i_infinity )
				transfocol[q]=((q-vcol[k])*(q-vcol[k]))+imap[vcol[k]][nc];
			else
				transfocol[q]=i_infinity;
		}
	}

	for(q=0;q<nrows;q++)
	{
		imap[q][ncols-1]=transfocol[q];
	}

	// TransformÃ©e selon les lignes ----------------------------------------

	for(nr=0;nr<nrows;nr++)
	{
		k=0;
		vrow[0]=0;
		zrow[0]=-1.0*infinity;
		zrow[1]=infinity;

		// Calcul de l'enveloppe infÃ©rieure

		for(q=1;q<ncols;q++)
		{
test_s_row:
			if( imap[nr][q] < i_infinity )
			{
				s1=imap[nr][q]-imap[nr][vrow[k]];
				s2=q*q-vrow[k]*vrow[k];
				s=((double)(s1+s2))/((double)(2*q-2*vrow[k]));
			}
			else
				s=infinity;

			if(s<zrow[k])
			{
				k--;
				goto test_s_row;
			}
			else
			{
				k++;
				vrow[k]=q;
				zrow[k]=s;
				zrow[k+1]=infinity;
			}
		}

		// Remplissage des distances

		k=0;
		for(q=0;q<ncols;q++)
		{
			if(nr>0)
				imap[nr-1][q]=transforow[q];

			while(zrow[k+1]<q)
				k++;

			if( imap[nr][vrow[k]] < i_infinity )
				transforow[q]=((q-vrow[k])*(q-vrow[k]))+imap[nr][vrow[k]];
			else
				transforow[q]=i_infinity;
		}
	}

	for(q=0;q<ncols;q++)
	{
		imap[nrows-1][q]=transforow[q];
	}

        // Recopie des valeurs en flottants ------------------------------------

      	for(nr=0;nr<nrows;nr++)
        {   for(nc=0;nc<ncols;nc++)
            {
                d = (double) ( imap[nr][nc] ) ;
                d = (EDM->Header.cellsize) * sqrt(d) ;
                (EDM->Data)[nr][nc] = (float) d ;
            }
        }

        // DÃ©sallocation des tableaux de travail -------------------------------

        free(vcol);
        free(vrow);
        free(zcol);
        free(zrow);
        free(transfocol);
        free(transforow);

        for (nr=0;nr<nrows;nr++)
            free ( imap[nr] );

        free ( imap );

        return(0);
}

/**********************************************************************************
 * 	                Fonction ""                                                   *
 *  Identifies paths in a graph (i.e. sequences of vertices having a degree of 2) *
 **********************************************************************************/

int BuildPaths ( int * ID_START , int * ID_END , int n , int *** PATH_ , int ** LENGTH_  , int * npaths_ , int ** DEGREE_ , int * nnodes_ )
{
	int i,d,npaths,nnodes,pivot,currentpathlength,neighbor,prevtoken;
	map <int,set<int> > Graph ;
	map <int,set<int> > ::iterator imisi ;
    map<int,int> Degree ;	
	map<int,int>::iterator imii ;
	set<int> voidset ;
	set<int>::iterator isi ;
	
	voidset.clear();
	
	// Initialization of the graph
		
	for (i=0;i<n;i++)
	{
		imisi = Graph.find(ID_START[i]);
		if (imisi==Graph.end())
			Graph.insert ( make_pair (ID_START[i], voidset ) );

		Graph[ID_START[i]].insert ( ID_END[i] ) ;
		
		imisi = Graph.find(ID_END[i]);
		if (imisi==Graph.end())
			Graph.insert ( make_pair (ID_END[i], voidset ) );

		Graph[ID_END[i]].insert ( ID_START[i] ) ;
	}
	
	// Compute degree of each vertex
	
	nnodes = Graph.size();
	(*nnodes_) = nnodes ;
	(*DEGREE_) = (int *) malloc ( 2 * nnodes * sizeof(int)) ;
	
	i=0;
	
	for ( imisi=Graph.begin();imisi!=Graph.end();imisi++ )
	{
		d = (*imisi).second.size();
		Degree.insert ( make_pair ( (*imisi).first , d ) ) ;
		
		(*DEGREE_)[i] = (*imisi).first;
		(*DEGREE_)[nnodes+i] = d;
		
		i++;		
	}
		
	// Build paths
	
	npaths = 0;
	(*PATH_) = NULL ;
	(*LENGTH_) = NULL ;
	
	imii = Degree.begin();
	
	while ( imii != Degree.end() )
	{
		// Search for a starting vertex i.e. with degree different from 2
	
		if ( (*imii).second != 2 )
		{
				pivot = (*imii).first ;
						
				// On construit tous les chemins qui partent du pivot
				// (éventuellement des boucles)
				
				while ( Graph[pivot].size()>0 )	// Tant que l'on n'a pas construit tous les chemins partant du pivot
				{
					npaths++ ; 
					
					(*LENGTH_) = (int * ) realloc( (*LENGTH_) , npaths * sizeof(int *) );
					currentpathlength = 1;
					
					(*PATH_) = (int **) realloc ( (*PATH_) , npaths * sizeof(int *) ) ;
					(*PATH_)[npaths-1] = (int *) malloc ( 1 * sizeof(int *) ) ;
					(*PATH_)[npaths-1][0] = pivot ;
					
					prevtoken = pivot ;
					
					isi = Graph[prevtoken].begin() ;
					neighbor = (*isi);
					d = 2 ;
					
					while ( d==2) // Tant que le chemin courant n'est pas terminé ( rencontre d'un noeud de valence > ou < 2 )
					{
						d = Degree[neighbor] ;
						
						currentpathlength++;
						(*PATH_)[npaths-1] = (int *) realloc ( (*PATH_)[npaths-1] , currentpathlength * sizeof(int *) ) ;
						(*PATH_)[npaths-1][currentpathlength-1] = neighbor ;
												
						// Suppression des aretes "prevtoken-neighbor" et "neighbor-prevtoken" dans le graphe
					
					    Graph[neighbor].erase(prevtoken) ;
						Graph[prevtoken].erase(neighbor) ;
				
						prevtoken = neighbor ;
						if (Graph[prevtoken].size()>0)
						{
							isi = Graph[prevtoken].begin() ;
							neighbor = (*isi);
						}
						
					}
					
					(*LENGTH_)[npaths-1] = currentpathlength ;
				
				} // fin de la boucle d'exploration des voisins du pivot

		}
		
		imii++ ;
	}

	Graph.clear();
	Degree.clear();
	(*npaths_) = npaths ;
	
	return(0);
		
}


