/* 
* 
* 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 "FloatRaster.h"
#include "FlowDirGrid.h"
#include "Topology.h"
#include "Mesh.h"
#include "MeshTree.h"
#include "esriheader.h"
#include "Erreur.h"
#include <set>
#include <map>
#include <algorithm>

using namespace std;

int GetCatchmentSum_ ( FlowDirGrid * F , FloatRaster * R , int * _outlet, set<int> & Nodes, int ** _BB , float * _Sum , set<int> & UpNodes , int * _nr , int * _nc );
int GetUpstreamNodes_ ( FlowDirGrid * F , int * _outlet, set<int> & Nodes, set<int> & UpNodes );

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int MakeMeshTree ( FlowDirGrid * F , FloatRaster * CW , FloatRaster * PK , int nnodes , int * NODES , int * CNT , MeshTree ** _MTree , double * _meanweight )
{

    int i,j,ichild,nr,nc,nchildren ;
    double meanweight , weight ;
    float f;
    set<int> UpNodes , Nodes ;
    map<int,int> MapNodes;
    set<int>::iterator isi ;
    map<int,int>::iterator imii;


    (*_MTree) = (MeshTree *) malloc ( nnodes * sizeof ( MeshTree ) );

    Nodes.clear();

    for(i=0;i<nnodes;i++)
    {
        Nodes.insert(NODES[i]) ;
        MapNodes.insert ( make_pair ( NODES[i] , i ) ) ;
        (*_MTree)[i].RootCell = NODES[i] ;
        (*_MTree)[i].Parent = NULL ;
        (*_MTree)[i].SSE = 0 ;
        (*_MTree)[i].Weight = 0 ;
        (*_MTree)[i].cnt = CNT[i] ;
        (*_MTree)[i].PositionRange = NULL ;
        (*_MTree)[i].CWeight = NULL ;
        (*_MTree)[i].RangeLength = 0 ;
        (*_MTree)[i].CurrentPosition = -1 ;

    }

    // Construction de la topologie

    meanweight = 0 ;

    for(i=0;i<nnodes;i++)
    {
        UpNodes.clear();

        if ( GetUpstreamNodes_ ( F , NODES + i , Nodes, UpNodes ) )
        {
            SetErreur("MakeMeshTree : could not compute catchment sum at node # %d\n", NODES[i] );
            return(-1);
        }

        nchildren = UpNodes.size();
        (*_MTree)[i].nchildren = nchildren ;
        (*_MTree)[i].Child = (MeshTree **) malloc ( nchildren * sizeof(MeshTree *) );

        isi = UpNodes.begin();

        (void) rec2RC ( &(CW->Header) , & ((*_MTree)[i].RootCell) , & nr , & nc );
        f = CW->Data[nr][nc];
        weight = (double) f ;

        for(j=0;j<nchildren;j++)
        {
            imii = MapNodes.find ( *isi );
            ichild = (*imii).second ;
            ((*_MTree)[i].Child)[j] =  (*_MTree) + ichild ;
            (*_MTree)[ichild].Parent = (*_MTree) + i ;

//            SetErreur("Construction du lien père : %d -> fils : %d (%d)\n",(*_MTree)[i].RootCell,((*_MTree)[i].Child)[j]->RootCell,(*_MTree)[ichild].RootCell);
//            SetErreur("Construction du lien fils : %d <- père : %d (%d)\n",(*_MTree)[ichild].RootCell,((*_MTree)[ichild]).Parent->RootCell,(*_MTree)[i].RootCell);

            (void) rec2RC ( &(CW->Header) , & (((*_MTree)[i].Child)[j]->RootCell ) , & nr , & nc );
            f = CW->Data[nr][nc];

            weight = weight - (double) f ;

            isi++;
        }

        (*_MTree)[i].Weight = (float) weight ;
        meanweight += weight ;
    }

    meanweight = meanweight / nnodes ;

    // Mise a jour de la variance totale ajustable a l'amont de chaque noeud
   
    for(i=0;i<nnodes;i++)
    {
        if ( ! (*_MTree)[i].Parent )   // Si l'element n'a pas de parent
        {
            if ( MakeMeshTree_ ( (*_MTree) + i , & meanweight ) )
            {
                SetErreur("MakeMeshTree : initial recursive call to MakeMeshTree_ failed\n" );
                return(-1);
            }

            if ( BuildPositionRange_ ( F , CW , PK , (*_MTree) + i ) )
            {
                SetErreur("MakeMeshTree :  initial recursive call to BuildPositionRange_ failed\n" );
                return(-1);
            }
        }
    }

    (*_meanweight) = meanweight ;

    Nodes.clear();
    UpNodes.clear();
    MapNodes.clear();

    return(0);

}

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int MakeMeshTree_ ( MeshTree * MTree , double * _meanweight )
{
    int i ;
    double eps ;

   eps = MTree->Weight - (*_meanweight) ;
   MTree->SSE = eps * eps ;

   MTree->TreeWeight = MTree->Weight ;
   MTree->ndescendants = MTree->nchildren ;

    for(i=0;i<MTree->nchildren;i++)
    {
        if ( MakeMeshTree_ ( MTree->Child[i] , _meanweight ) )
        {
            SetErreur("MakeMeshTree_ : recursive call failed\n" );
            return(-1);
        }

        if ( ! MTree->Child[i]->cnt )       // Si le fils Child[i] de la maille d'appel n'est pas un pt de contrainte, on propage la variance ajustable vers l'aval
        {
            MTree->SSE += MTree->Child[i]->SSE ;
            MTree->ndescendants += MTree->Child[i]->ndescendants ;
            MTree->TreeWeight += MTree->Child[i]->TreeWeight ;
        }
    }

    return(0);
}

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int OptimMeshVariance_ ( MeshTree * MTree , double * _meanweight , double * _globaltarget, double * _damper )
{
    double W,SW,Winf,Wsup,dW,localtarget,globaltarget_i ;
    int i,p,pinf,psup;

    W = MTree->TreeWeight ;
    SW = 0;

    if (MTree->ndescendants>0)
    {
//        globaltarget_i = W/(1+MTree->ndescendants);
//        globaltarget_i = (1.0-(*_damper)) * globaltarget_i + (*_damper) * (*_globaltarget) ;

        globaltarget_i = (*_meanweight);

        // Optimisation des arbres amont pour se rapprocher de la cible (*_globaltarget_i)

        for (i=0;i<MTree->nchildren;i++)
        {
            if ( ! (MTree->Child[i]->cnt) )
            {
                if ( OptimMeshVariance_ ( MTree->Child[i] , _meanweight , & globaltarget_i , _damper ) )
                {
                    SetErreur("OptimMeshVariance_ : recursive call failed\n" );
                    return(-1);
                }

                SW += MTree->Child[i]->TreeWeight ;
            }
        }

        W -= SW ;
        localtarget = SW / (MTree->ndescendants) ;
    }
    else
    {
        localtarget = (*_globaltarget);
    }

    if ( MTree->Parent )
    {
        localtarget = (1.0-(*_damper)) * localtarget + (*_damper) * (MTree->Parent)->Weight ;
    }

    
    if (localtarget < 0)
    {
        SetErreur("OptimMeshVariance_ : target weight is negative\n" );
        return(-1);
    }

    /*******************************************************************
    * Deplacement de MTree->RootCell en fonction de l'Ã©cart Ã  la cible *
    *******************************************************************/

    if ( MTree->cnt )
    {
        MTree->Weight = W ;
        return(0);
    }

    if ( W == localtarget )
        return(0);

    //--------------------------------------------------------------------------
    // si le poids de la maille racine est plus petit que la cible
    
    if ( W < localtarget )
    {
        // On cherche la meilleure position vers l'aval,
        // entre MTree->CurrentPosition et MTree->RangeLength-1

        pinf = MTree->CurrentPosition ;
        psup = MTree->RangeLength - 1 ;
        Winf = W ;

        dW = (MTree->CWeight)[psup] - (MTree->CWeight)[pinf] ;
        Wsup = Winf + dW ;

        if ( Wsup < localtarget )
        {
            pinf = psup;
            Winf = Wsup;
        }

        while ( psup - pinf > 1 )
        {
            p = ( pinf + psup ) / 2 ;

            dW = (MTree->CWeight)[p] - (MTree->CWeight)[pinf] ;
            W = Winf + dW ;

            if ( W > localtarget )
            {
                psup = p ;
                Wsup = W ;
            }
            else
            {
                pinf = p ;
                Winf = W ;
            }
        }

        MTree->Weight = Winf ;
        MTree->CurrentPosition = pinf;
        MTree->TreeWeight = Winf + SW ;
        MTree->RootCell = (MTree->PositionRange)[pinf];
    }
    else
    //--------------------------------------------------------------------------
    // si le poids de la maille racine est plus grand que la cible
    {
        // On cherche la meilleure position vers l'amont,
        // entre 0 et MTree->CurrentPosition

        pinf = 0 ;
        psup =  MTree->CurrentPosition ;
        Wsup = W ;

        dW = (MTree->CWeight)[psup] - (MTree->CWeight)[pinf] ;
        Winf = Wsup - dW ;

        if ( Winf > localtarget )
        {
            psup = pinf;
            Wsup = Winf;
        }

        while ( psup - pinf > 1 )
        {
            p = ( pinf + psup ) / 2 ;

            dW = (MTree->CWeight)[p] - (MTree->CWeight)[pinf] ;
            W = Winf + dW ;

            if ( W >= localtarget )
            {
                psup = p ;
                Wsup = W ;
            }
            else
            {
                pinf = p ;
                Winf = W ;
            }
        }

        MTree->Weight = Wsup ;
        MTree->CurrentPosition = psup;
        MTree->TreeWeight = Wsup + SW ;
        MTree->RootCell = (MTree->PositionRange)[psup];
    }
    
    return(0);

}

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int test_Up ( MeshTree * MTree )
{
    int upcell ;

    if ( MTree->cnt )
    {
        return(0);
    }
    
    if ( MTree->CurrentPosition == 0 )
    {
        return(0);
    }

    if (MTree->nchildren == 1)
    {
        upcell = (MTree->PositionRange)[(MTree->CurrentPosition)-1] ;

        if ( upcell == MTree->Child[0]->RootCell )
        {
            return(0);
        }
    }

    return(1);
}

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int test_Down ( MeshTree * MTree )
{
    int downcell ;

    if ( MTree->cnt )
    {
        return(0);
    }

    if ( MTree->CurrentPosition == ((MTree->RangeLength)-1) )
    {
        return(0);
    }

    if ( MTree->Parent )
    {
        downcell = (MTree->PositionRange)[(MTree->CurrentPosition)+1] ;

        if ( downcell == MTree->Parent->RootCell )
        {
            return(0);
        }
    }
    else
    {
        return(0);
    }

    return(1);
}

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int BuildPositionRange_ ( FlowDirGrid * F , FloatRaster * CW , FloatRaster * PK , MeshTree * MTree )
{
    int i,j,nr,nc,rec;
    int ** PathFromChild ;
    int * PathLength ;
    int * FlowLine ;
    int length;
    float f;
    set<int> TmpSet , NewTmpSet  ;
    set<int>::iterator isi ;
    map<float,int> TmpMap ;
    map<float,int>::iterator imfi ;
    MeshTree * MTreeC ;

    //--------------------------------------------------------------------------
    // Extraction des biefs reliant chaque fils a l'exutoire de la maille

    if ( ! ( PathFromChild = (int **) malloc (  MTree->nchildren * sizeof(int *) ) ) )
    {
        SetErreur("BuildPositionRange_ : could not allocate int* pointer PathFromChild\n");
        return(-1);
    }

    if ( ! ( PathLength = (int *) malloc (  MTree->nchildren * sizeof(int) ) ) )
    {
        SetErreur("BuildPositionRange_ : could not allocate integer pointer PathLength\n");
        return(-1);
    }

    for(i=0;i<MTree->nchildren;i++)
    {
        if ( MakeFlowLineBetweenPoints ( F , (MTree->Child)[i]->RootCell , MTree->RootCell , PathFromChild + i , PathLength + i ) )
        {
            SetErreur("BuildPositionRange_ : could not build flow line between cells %d -> %d\n",(MTree->Child)[i]->RootCell , MTree->RootCell);
            return(-1);
        }

        sort ( PathFromChild[i] , PathFromChild[i] + PathLength[i] ) ;
    }

    //--------------------------------------------------------------------------
    // Appels sur les arbres amont

    for(i=0;i<MTree->nchildren;i++)
    {
        // Appel recursif : construit la partie amont

        MTreeC = (MTree->Child)[i] ;

        BuildPositionRange_ ( F , CW , PK , MTreeC ) ;
    
        // Identification de l'ensemble des positions aval possibles du noeud (MTree->Child)[i] :
        // partie du bief (MTree->Child)[i]->RootCell ~~> MTree->RootCell  partagee avec aucun des autres fils

        TmpSet.clear();
        NewTmpSet.clear();

        TmpSet.insert ( PathFromChild[i] , PathFromChild[i] + PathLength[i] ) ;

        for(j=0;j<MTree->nchildren;j++)
        {
            if(j!=i)
            {
                set_difference ( TmpSet.begin() , TmpSet.end() , PathFromChild[j] , PathFromChild[j] + PathLength[j] , inserter(NewTmpSet,NewTmpSet.begin()) );
                TmpSet.clear();
                TmpSet.insert(NewTmpSet.begin(),NewTmpSet.end());         // Mise a jour de la difference
                NewTmpSet.clear();
            }
        }

        // Ajout des PK (set => map)

        TmpSet.erase(MTreeC->RootCell); // La position courante (racine) est deja presente dans la partie amont
        TmpMap.clear();

        for(isi=TmpSet.begin();isi!=TmpSet.end();isi++)
        {
            rec = (*isi);
            (void) rec2RC ( &(PK->Header) , & rec , & nr , & nc );
            f = PK->Data[nr][nc];
            TmpMap.insert ( make_pair ( -f , rec ) );       // classement d'amont en aval
       }

        TmpSet.clear();

        // On complete MTreeC->PositionRange avec la portion aval identifiee

        j = MTreeC->RangeLength;
        MTreeC->RangeLength += TmpMap.size();
        
        if ( ! ( MTreeC->PositionRange = (int *) realloc ( MTreeC->PositionRange , (MTreeC->RangeLength) * sizeof(int) ) ) )
        {
            SetErreur("BuildPositionRange_ : could not reallocate integer pointer MTree->PositionRange\n");
            return(-1);
        }

        if ( ! ( MTreeC->CWeight = (double *) realloc ( MTreeC->CWeight , (MTreeC->RangeLength) * sizeof(double) ) ) )
        {
            SetErreur("BuildPositionRange_ : could not reallocate double pointer MTree->CWeight\n");
            return(-1);
        }

        for(imfi=TmpMap.begin();imfi!=TmpMap.end();imfi++)
        {
            rec = (*imfi).second ;
            (MTreeC->PositionRange)[j] = rec ;

            (void) rec2RC ( &(CW->Header) , & rec , & nr , & nc );
            f = CW->Data[nr][nc];
            (MTreeC->CWeight)[j] = (double) f ;

            j++;
        }

        TmpMap.clear();
    }

    //--------------------------------------------------------------------------
    // Construction de l'intersection des "PathFromChild"

    TmpSet.clear();
    NewTmpSet.clear();

    if (MTree->nchildren > 0)
    {
        TmpSet.insert ( PathFromChild[0] , PathFromChild[0] + PathLength[0] ) ;

        for(j=1;j<MTree->nchildren;j++)
        {
            set_intersection ( TmpSet.begin() , TmpSet.end() , PathFromChild[j] , PathFromChild[j] + PathLength[j] , inserter(NewTmpSet,NewTmpSet.begin()) );
            TmpSet.clear();
            TmpSet.insert ( NewTmpSet.begin() , NewTmpSet.end() );         // Mise a jour de l'intersection
            NewTmpSet.clear();
        }
    }
    else
    {
 
        if ( MakeFlowLineThroughPoint_ ( F , CW , & (MTree->RootCell) , & FlowLine , & length ) )
        {
            SetErreur("BuildPositionRange_ : could not build main drain upstream cell %d\n",MTree->RootCell);
            return(-1);
        }

        sort ( FlowLine , FlowLine + length ) ;
        TmpSet.insert( FlowLine , FlowLine + length );
        free(FlowLine);
    }


    if ( TmpSet.size()<=0 )
    {
        SetErreur("BuildPositionRange_ : invalid position range, upstream part is empty\n");
        return(-1);
    }

    //--------------------------------------------------------------------------
    // Ajout des PK (set => map)

    TmpMap.clear();

    for(isi=TmpSet.begin();isi!=TmpSet.end();isi++)
    {
        rec = (*isi);
        (void) rec2RC ( &(PK->Header) , & rec , & nr , & nc );
        f = PK->Data[nr][nc];
        TmpMap.insert ( make_pair ( -f , rec ) );       // classement d'amont en aval
   }

    TmpSet.clear();

    // On initialise MTree->PositionRange avec la portion amont

    MTree->RangeLength = TmpMap.size();
    
    if ( ! ( MTree->PositionRange = (int *) malloc ( (MTree->RangeLength) * sizeof(int) ) ) )
    {
        SetErreur("BuildPositionRange_ : could not allocate integer pointer MTree->PositionRange\n");
        return(-1);
    }

    if ( ! ( MTree->CWeight = (double *) malloc ( (MTree->RangeLength) * sizeof(double) ) ) )
    {
        SetErreur("BuildPositionRange_ : could not allocate double pointer MTree->CWeight\n");
        return(-1);
    }

    MTree->CurrentPosition = (MTree->RangeLength) - 1;
    j=0;

    for(imfi=TmpMap.begin();imfi!=TmpMap.end();imfi++)
    {
        rec = (*imfi).second ;
        (MTree->PositionRange)[j] = rec ;
        
        (void) rec2RC ( &(CW->Header) , & rec , & nr , & nc );
         f = CW->Data[nr][nc];
        (MTree->CWeight)[j] = (double) f ;

        j++;
    }

    TmpMap.clear();

    if ( (MTree->PositionRange)[MTree->CurrentPosition] != MTree->RootCell )
    {
        SetErreur("BuildPositionRange_ : invalid position range, initial position is not identical to last position of upstream part\n");
        return(-1);
    }

    //--------------------------------------------------------------------------
    // Desallocation des pointeurs

    for(i=0;i<MTree->nchildren;i++)
    {
        free(PathFromChild[i]);
    }
    
    free(PathFromChild);
    free(PathLength);

    return(0);
}

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int UpdateMeshTree_ ( MeshTree * MTreeDst , MeshTree * MTreeSrc )
{
    int i;

    // Mise aÃ‚Â  jour des elements statiques

    MTreeDst->RootCell = MTreeSrc->RootCell ;
    MTreeDst->SSE = MTreeSrc->SSE ;
    MTreeDst->Weight = MTreeSrc->Weight ;
    MTreeDst->cnt = MTreeSrc->cnt ;

    if ( MTreeDst->nchildren != MTreeSrc->nchildren )
    {
        SetErreur("UpdateMeshTree_ : trees to be mirrored do not have the same number of children\n");
        return(-1);
    }

    if ( MTreeDst->RangeLength != MTreeSrc->RangeLength )
    {
        SetErreur("UpdateMeshTree_ : accessible position ranges for the trees to be mirrored, do not have identical sizes\n");
        return(-1);
    }

    MTreeDst->CurrentPosition = MTreeSrc->CurrentPosition ;
   
    // Mise a jour recursive

    for(i=0;i<MTreeSrc->nchildren;i++)
    {
        if ( UpdateMeshTree_ ( MTreeDst->Child[i] , MTreeSrc->Child[i] ) )
        {
            SetErreur("UpdateMeshTree_ : recursive update failed for child %d\n",i);
            return(-1);
        }
    }

    return(0);
}

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int CopyMeshTree_ ( MeshTree * MTree , MeshTree ** _MTreeCopy )
{
    int i;

    if ( ! ((*_MTreeCopy) = (MeshTree *) malloc ( sizeof(MeshTree) )) )
    {
        SetErreur("CopyMeshTree_ : could not allocate root tree\n");
        return(-1);
    }

    // Copie des elements statiques

    (*_MTreeCopy)->RootCell = MTree->RootCell ;
    (*_MTreeCopy)->SSE = MTree->SSE ;
    (*_MTreeCopy)->Weight = MTree->Weight ;
    (*_MTreeCopy)->cnt = MTree->cnt ;
    (*_MTreeCopy)->nchildren = MTree->nchildren ;
    (*_MTreeCopy)->Parent = NULL ;

    // Copie des elements dynamiques

    (*_MTreeCopy)->CurrentPosition = MTree->CurrentPosition ;
    (*_MTreeCopy)->RangeLength = MTree->RangeLength ;
    (*_MTreeCopy)->PositionRange = (int *) malloc ( (MTree->RangeLength)*sizeof(int) );
    (*_MTreeCopy)->CWeight = (double *) malloc ( (MTree->RangeLength)*sizeof(double) );

    for (i=0;i<MTree->RangeLength;i++)
    {
        ((*_MTreeCopy)->PositionRange)[i] = (MTree->PositionRange)[i] ;
        ((*_MTreeCopy)->CWeight)[i] = (MTree->CWeight)[i] ;
    }

    // Copie recursive

    if ( !((*_MTreeCopy)->Child = (MeshTree **) malloc ( (MTree->nchildren) * sizeof ( MeshTree *) )) )
    {
        SetErreur("CopyMeshTree_ : could not allocate pointer towards child trees\n");
        return(-1);
    }

    for(i=0;i<MTree->nchildren;i++)
    {
        if ( CopyMeshTree_ ( (MTree->Child)[i] , (*_MTreeCopy)->Child + i ) )
        {
            SetErreur("CopyMeshTree_ : recursive copy failed for child %d\n",i);
            return(-1);
        }

        (*_MTreeCopy)->Child[i]->Parent = (*_MTreeCopy) ;
    }

    return(0);
}

//*********************************************************************************************************************************************************
//*********************************************************************************************************************************************************

int DestroyMeshTree_ ( MeshTree ** _MTree )   // destruction rÃƒÆ’Ã†â€™Ãƒâ€šÃ‚Â©cursive d'un arbre construit par CopyMeshTree_
{
    int i;

    free ( (*_MTree)->PositionRange );
    free ( (*_MTree)->CWeight );

    for(i=0;i<(*_MTree)->nchildren;i++)
    {
        DestroyMeshTree_ ( (*_MTree)->Child + i );
    }

    free((*_MTree)->Child);
    free(*_MTree);

    return(0);

}

