//----------------------------------------------------------------------------
//  EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : ShortestPathTsp.cpp      Data : 22.12.15        Versione : 1.6l4
// Contenuto : Calcolo del percorso minimo nel caso generale.
//
//
//
// Modifiche : 22.12.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "DllMain.h"
#include "ShortestPath.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include <new>

using namespace std ;

//----------------------------------------------------------------------------
#if defined( _DEBUG)
   #define MY_LOG(szOut)     LOG_INFO( GetENkLogger(), szOut) ;
#else
   #define MY_LOG(szOut)     ;
#endif

//----------------------------------------------------------------------------
bool
ShortestPath::Tsp( void)
{
  // almeno un punto
   if ( m_nNumPnts < 1)
      return false ;

  // per path aperta aggiungo un punto con distanze nulle da tutti gli altri ( per poter creare il lato nullo)
   if ( m_nType == SP_OPEN) {
      if ( m_nNumPnts <= MAX_SPPS) {
         m_Points[m_nNumPnts].dXi = 0 ;
         m_Points[m_nNumPnts].dYi = 0 ;
         m_Points[m_nNumPnts].dXf = 0 ;
         m_Points[m_nNumPnts].dYf = 0 ;
         ++ m_nNumPnts ;
      }
      else
         return false ;
   }

  // alloco la matrice delle distanze
   if ( m_Dists != nullptr)
      delete m_Dists ;
   m_Dists = new(nothrow) unsigned[ m_nNumPnts * m_nNumPnts] ;
   if ( m_Dists == nullptr)
      return false ;
  // alloco la matrice ausiliaria
   if ( m_Available != nullptr)
      delete m_Available ;
   m_Available = new(nothrow) bool[ m_nNumPnts * m_nNumPnts] ;
   if ( m_Available == nullptr)
      return false ;
  // alloco la path principale
   if ( m_pMain != nullptr)
      delete m_pMain ;
   m_pMain = new(nothrow) NODE[ m_nNumPnts] ;
   if ( m_pMain == nullptr)
      return false ;
  // alloco la copia della path principale
   if ( m_pDupl != nullptr)
      delete m_pDupl ;
   m_pDupl = new(nothrow) NODE[ m_nNumPnts] ;
   if ( m_pDupl == nullptr)
      return false ;
  // alloco la copia del path per controllo dipendenze
   if ( m_pCheckDep != nullptr)
      delete m_pCheckDep ;
   m_pCheckDep = new(nothrow) NODE[ m_nNumPnts] ;

  // calcolo la matrice delle distanze
   CalcDistances() ;

  // organizzo percorsi
   PreparePath( m_pMain) ;
   PreparePath( m_pDupl) ;
   PreparePath( m_pCheckDep) ;

  // inizializzo minimo costo
   m_nMinCost = LLONG_MAX ;

  // ---- applico algoritmo NearNeighbor con miglioramenti aggiuntivi ----
   long long unsigned nMinNN = NearNeighbor() ;
   string sOut = "-- NearNeighbor : TotalCost = " + to_string( nMinNN) ;
   MY_LOG( sOut.c_str()) ;
   _printPath( m_pMain, ToString( "NN : ")) ;
   // se migliora, salvo l'ordinamento
   if ( nMinNN < m_nMinCost) {
      MY_LOG( "    Improve") ;
      m_nMinCost = nMinNN ;
      UpdateOrder( m_pMain) ;
   }
  // salvo il percorso in un duplicato
   SavePath( m_pMain) ;
  // miglioramenti aggiuntivi
   CalculateImprovements( m_pMain) ;

  // ---- Applico percorso invertito, se risultato precedente scarso ----
   const double COEFF_INVERTED = 1.2 ;
   const int MIN_PNTS_INVERTED = 128 ;
   if ( ! ExistConstraints()) {
      if ( m_nMinCost > COEFF_INVERTED * m_nTotMin  ||  m_nNumPnts < MIN_PNTS_INVERTED) {
        // inverto il percorso
         NODE* pCurr = m_pDupl->pPrev ;
         NODE* pPath = m_pMain ;
         for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
            pPath->nPos = pCurr->nPos ;
            pPath = pPath->pNext ;
            pCurr = pCurr->pPrev ;
         }
        // ne calcolo il risultato
         long long unsigned nMinInv = TotalCost( m_pMain) ;
         _printPath( m_pMain, "Inverted NN : ") ;
         string sOut = "-- Inverted NN : TotalCost = " + to_string( nMinInv) ;
         MY_LOG( sOut.c_str()) ;
         // se migliora, salvo l'ordinamento
         if ( nMinInv < m_nMinCost) {
            MY_LOG( "    Improve") ;
            m_nMinCost = nMinInv ;
            UpdateOrder( m_pMain) ;
         }
        // salvo il percorso in un duplicato
         SavePath( m_pMain) ;
        // miglioramenti aggiuntivi
         CalculateImprovements( m_pMain) ;
      }
      else
         MY_LOG( "-- Inverted NN : Not necessary") ;
   }

  // ---- Applico pessima partenza, se risultato precedente scarso ----
   const double COEFF_FARNEIG = 1.4 ;
   const int MIN_PNTS_FARNEIG = 128 ;
   if ( m_nMinCost > COEFF_FARNEIG * m_nTotMin  ||  m_nNumPnts < MIN_PNTS_FARNEIG) {
      long long unsigned nMinFN = FarNeighbor() ;
      _printPath( m_pMain, "FN : ") ;
      string sOut = "-- FarNeighbor : TotalCost = " + to_string( nMinFN) ;
      MY_LOG( sOut.c_str()) ;
      // se migliora, salvo l'ordinamento
      if ( nMinFN < m_nMinCost) {
         MY_LOG( "    Improve") ;
         m_nMinCost = nMinFN ;
         UpdateOrder( m_pMain) ;
      }
     // salvo il percorso in un duplicato
      SavePath( m_pMain) ;
     // miglioramenti aggiuntivi
      CalculateImprovements( m_pMain) ;
   }
   else
      MY_LOG( "-- FarNeighbor : Not necessary") ;

   return true ;
}

//----------------------------------------------------------------------------
bool
ShortestPath::CalculateImprovements( NODE* pPath)
{
  // Ottimizzazione con movimento di singolo punto
   RestorePath( pPath) ;
   long long unsigned nPntOpt = PointOpt( pPath) ;
   _printPath( pPath, "PointOpt : ") ;
   string sOut = "   PointOpt : TotalCost = " + to_string( nPntOpt) ;
   MY_LOG( sOut.c_str()) ;
   // se migliora, salvo l'ordinamento
   if ( nPntOpt < m_nMinCost) {
      MY_LOG( "    Improve") ;
      m_nMinCost = nPntOpt ;
      UpdateOrder( pPath) ;
   }

  // Ottimizzazione con movimento di due punti (lanciata solo se non ci sono troppi punti)
   const int MAX_NODES_2OPT = 768 ;
   if ( m_nNumPnts <= MAX_NODES_2OPT) {
      RestorePath( pPath) ;
      long long unsigned nTwoOpt = TwoOpt( pPath) ;
      _printPath( pPath, "TwoOpt : ") ;
      string sOut = "   TwoOpt : TotalCost = " + to_string( nTwoOpt) ;
      MY_LOG( sOut.c_str()) ;
      // se migliora, salvo l'ordinamento
      if ( nTwoOpt < m_nMinCost) {
         MY_LOG( "    Improve") ;
         m_nMinCost = nTwoOpt ;
         UpdateOrder( pPath) ;
      }
   }

  // Ottimizzazione ibrida (lanciata solo se non ci sono troppi punti)
   const int MAX_NODES_HYBRID = 512 ;
   if ( m_nNumPnts <= MAX_NODES_HYBRID) {
      RestorePath( pPath) ;
      long long unsigned nHybOpt = Hybrid( pPath) ;
      _printPath( pPath, "Hybrid : ") ;
      string sOut = "   Hybrid : TotalCost = " + to_string( nHybOpt) ;
      MY_LOG( sOut.c_str()) ;
      // se migliora, salvo l'ordinamento
      if ( nHybOpt < m_nMinCost) {
         MY_LOG( "    Improve") ;
         m_nMinCost = nHybOpt ;
         UpdateOrder( pPath) ;
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
unsigned
ShortestPath::GetDistance( float dDx, float dDy, float dDz, float dDh, float dDv)
{
  // parte lineare
   unsigned nDist = unsigned( sqrt( dDx * dDx + dDy * dDy + dDz * dDz) + 0.5) ;
  // eventuali parti angolari
   if ( abs( dDh) > 1  &&  abs( dDv) > 1)
      nDist += unsigned( max( m_dAngHAdd, m_dAngVAdd) + m_dAngHMul * abs( dDh) + m_dAngVMul * abs( dDv)) ;
   else if ( abs( dDh) > 1)
      nDist += unsigned( m_dAngHAdd + m_dAngHMul * abs( dDh)) ;
   else if ( abs( dDv) > 1)
      nDist += unsigned( m_dAngVAdd + m_dAngVMul * abs( dDv)) ;
   return nDist ;
}

//----------------------------------------------------------------------------
void
ShortestPath::CalcDistances( void)
{
  // inizializzo somma delle minime distanze
   m_nTotMin = 0 ;

  // in generale si calcolano le distanze per tutti i punti
   unsigned nLimit = m_nNumPnts ;

  // nel caso di percorso aperto, ultima riga e ultima colonna si calcolano in modo speciale
   if ( m_nType == SP_OPEN) {
     // calcolo box dei punti
      float fXmin = (float) m_Points[0].dXi ;
      float fXmax = (float) m_Points[0].dXi ;
      float fYmin = (float) m_Points[0].dYi ;
      float fYmax = (float) m_Points[0].dYi ;
      float fZmin = (float) m_Points[0].dZi ;
      float fZmax = (float) m_Points[0].dZi ;
      // ciclo sui punti
      for ( unsigned i = 0 ; i < m_nNumPnts - 1 ; ++ i) {
        // X
         if ( m_Points[i].dXi < fXmin)
            fXmin = (float) m_Points[i].dXi ;
         if ( m_Points[i].dXi > fXmax)
            fXmax = (float) m_Points[i].dXi ;
         if ( m_Points[i].dXf < fXmin)
            fXmin = (float) m_Points[i].dXf ;
         if ( m_Points[i].dXf > fXmax)
            fXmax = (float) m_Points[i].dXf ;
        // Y
         if ( m_Points[i].dYi < fYmin)
            fYmin = (float) m_Points[i].dYi ;
         if ( m_Points[i].dYi > fYmax)
            fYmax = (float) m_Points[i].dYi ;
         if ( m_Points[i].dYf < fYmin)
            fYmin = (float) m_Points[i].dYf ;
         if ( m_Points[i].dYf> fYmax)
            fYmax = (float) m_Points[i].dYf ;
        // Z
         if ( m_Points[i].dZi < fZmin)
            fZmin = (float) m_Points[i].dZi ;
         if ( m_Points[i].dZi > fZmax)
            fZmax = (float) m_Points[i].dZi ;
         if ( m_Points[i].dZf < fZmin)
            fZmin = (float) m_Points[i].dZf ;
         if ( m_Points[i].dZf > fZmax)
            fZmax = (float) m_Points[i].dZf ;
      }
     // imposto minimo di linea
      unsigned nRowMinDist = INT_MAX ;
     // assegno le distanze sull'ultima riga ( dal precedente all'inizio del percorso aperto) tranne ultimo elemento
      for ( unsigned i = 0 ; i < m_nNumPnts - 1 ; ++ i) {
         unsigned nDist ;
        // il valore dipende dalla condizione imposta
         switch ( m_ObStart.nF) {
         case OB_NEAR_PNT : {
            float dDx = float( m_ObStart.dX - m_Points[i].dXi) ;
            float dDy = float( m_ObStart.dY - m_Points[i].dYi) ;
            float dDz = float( m_ObStart.dZ - m_Points[i].dZi) ;
            float dDh = float( m_ObStart.dH - m_Points[i].dHi) ;
            float dDv = float( m_ObStart.dV - m_Points[i].dVi) ;
            nDist = GetDistance( dDx, dDy, dDz, dDh, dDv) ;
            } break ;
         case OB_XMIN :
            nDist = unsigned( m_Points[i].dXi - fXmin + 0.5) ;
            break ;
         case OB_XMAX :
            nDist = unsigned( fXmax - m_Points[i].dXi + 0.5) ;
            break ;
         case OB_YMIN :
            nDist = unsigned( m_Points[i].dYi - fYmin + 0.5) ;
            break ;
         case OB_YMAX :
            nDist = unsigned( fYmax - m_Points[i].dYi + 0.5) ;
            break ;
         default :    // OB_NONE
            nDist = 0 ;
            break ;
         }
         m_Dists[ Index( m_nNumPnts - 1, i)] = min( nDist, MAXDIST) ;
        // verifico se nuovo minimo di linea
         if ( nDist < nRowMinDist)
            nRowMinDist = nDist ;
      }
     // aggiorno il totale delle minime distanze
      m_nTotMin += nRowMinDist ;
     // assegno le distanze sull'ultima colonna ( dalla fine del percorso aperto al successivo) tranne ultimo elemento
      for ( unsigned i = 0 ; i < m_nNumPnts - 1 ; ++ i) {
         unsigned nDist ;
        // il valore dipende dalla condizione imposta
         switch ( m_ObEnd.nF) {
         case OB_NEAR_PNT : {
            float dDx = float( m_ObEnd.dX - m_Points[i].dXf) ;
            float dDy = float( m_ObEnd.dY - m_Points[i].dYf) ;
            float dDz = float( m_ObEnd.dZ - m_Points[i].dZf) ;
            float dDh = float( m_ObEnd.dH - m_Points[i].dHf) ;
            float dDv = float( m_ObEnd.dV - m_Points[i].dVf) ;
            nDist = GetDistance( dDx, dDy, dDz, dDh, dDv) ;
            } break ;
         case OB_XMIN :
            nDist = unsigned( m_Points[i].dXf - fXmin + 0.5) ;
            break ;
         case OB_XMAX :
            nDist = unsigned( fXmax - m_Points[i].dXf + 0.5) ;
            break ;
         case OB_YMIN :
            nDist = unsigned( m_Points[i].dYf - fYmin + 0.5) ;
            break ;
         case OB_YMAX :
            nDist = unsigned( fYmax - m_Points[i].dYf + 0.5) ;
            break ;
         default :    // OB_NONE
            nDist = 0 ;
            break ;
         }
         m_Dists[ Index( i,  m_nNumPnts - 1)] = min( nDist, MAXDIST) ;
      }
     // punto su diagonale principale
      m_Dists[ Index( m_nNumPnts - 1, m_nNumPnts - 1)] = MAXDIST ;
     // devo ancora calcolare le righe/colonne precedenti
      nLimit = m_nNumPnts - 1 ;
   }

  // ciclo per calcolare le distanze tra tutte le coppie di nodi
   bool bCalcDistances = ( m_ExtDistMat.empty()) ;
   for ( unsigned i = 0 ; i < nLimit ; ++ i) {
      unsigned nRowMinDist = INT_MAX ;
      for ( unsigned j = 0 ; j < nLimit ; ++ j) {
        // se ho già impostato una matrice delle distanze, allora uso quella
         if ( ! bCalcDistances)
            m_Dists[Index( i, j)] = m_ExtDistMat[i][j] ;
        // altrimenti calcolo le rispettive distanze
         else {
            if ( i != j) {
              // calcolo distanza i -> j
               float dDx = float( m_Points[i].dXf - m_Points[j].dXi) ;
               float dDy = float( m_Points[i].dYf - m_Points[j].dYi) ;
               float dDz = float( m_Points[i].dZf - m_Points[j].dZi) ;
               float dDh = float( m_Points[i].dHf - m_Points[j].dHi) ;
               float dDv = float( m_Points[i].dVf - m_Points[j].dVi) ;
               unsigned nDist = min( GetDistance( dDx, dDy, dDz, dDh, dDv), MAXDIST) ;
               m_Dists[ Index( i, j)] = nDist ;
              // verifico se nuovo minimo di linea
               if ( nDist < nRowMinDist)
                  nRowMinDist = nDist ;
            }
            else
               m_Dists[ Index( i, j)] = MAXDIST ;
         }
      }
     // aggiorno il totale delle minime distanze
      m_nTotMin += nRowMinDist ;
   }

  // nel caso di dipendenze suggerite, calcolo il coefficiente di costo massimo associato
   if ( ExistSuggDependences())
      m_nMaxDistSuggDep = CalcMaxDist() ;

  // nel caso di percorso aperto senza vincoli, il numero delle distanze è uno meno di quello dei veri nodi
   if ( m_nType == SP_OPEN  &&
        m_ObStart.nF == OB_NONE  &&  m_ObEnd.nF == OB_NONE)
      m_nTotMin = (long long unsigned) ( m_nTotMin / (double) nLimit * ( nLimit - 1)) ;

  // stampe di debug
   #if 0
      MY_LOG( "----\nDistances :") ;
      for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
         string sOut ;
         for ( unsigned j = 0 ; j < m_nNumPnts ; ++ j) {
            sOut += ToString( int( m_Dists[ Index( i, j)]), 2) + "   " ;
         }
         MY_LOG( sOut.c_str()) ;
      }
   #endif
   string sOut = "MinCost = " + to_string( m_nTotMin) ;
   MY_LOG( sOut.c_str()) ;
}

//----------------------------------------------------------------------------
void
ShortestPath::PreparePath( NODE* pPath)
{
   NODE* pCurr = pPath ;
   for ( unsigned i = 1 ; i < m_nNumPnts ; ++ i) {
      pCurr->pNext = (NODE*) ( (size_t) pCurr + sizeof( NODE)) ;
      pCurr->pNext->pPrev = pCurr ;
      pCurr = pCurr->pNext ;
   }
   pPath->pPrev = pCurr ;
   pCurr->pNext = pPath ;
}

//----------------------------------------------------------------------------
void
ShortestPath::SavePath( NODE* pPath)
{
   NODE* pCurr = m_pDupl ;
   for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
      pCurr->nPos = pPath->nPos ;
      pPath = pPath->pNext ;
      pCurr = pCurr->pNext ;
   }
}

//----------------------------------------------------------------------------
void
ShortestPath::RestorePath( NODE* pPath)
{
   NODE* pCurr = m_pDupl ;
   for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
      pPath->nPos = pCurr->nPos ;
      pPath = pPath->pNext ;
      pCurr = pCurr->pNext ;
   }
}

//----------------------------------------------------------------------------
void
ShortestPath::CopyPath( NODE* pPath, NODE* pPathCopy)
{
   NODE* pCurr = pPathCopy ;
   for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
      pCurr->nPos = pPath->nPos ;
      pPath = pPath->pNext ;
      pCurr = pCurr->pNext ;
   }
}

//----------------------------------------------------------------------------
void
ShortestPath::CopyPathAdv( NODE* pPath, NODE* pPathCopy, NODE* pLast, NODE* pFirst, NODE* pKth,
                           NODE** pLastCopy, NODE** pFirstCopy, NODE** pKthCopy)
{
   CopyPath( pPath, pPathCopy) ;
   NODE* pCurr = pPath ;
   NODE* pCurrCopy = pPathCopy ;
   for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
      if ( pCurr == pLast)
         *pLastCopy = pCurrCopy ;
      if ( pCurr == pKth)
         *pKthCopy = pCurrCopy ;
      if ( pCurr == pFirst)
         *pFirstCopy = pCurrCopy ;
      pCurr = pCurr->pNext ;
      pCurrCopy = pCurrCopy->pNext ;
   }
}

//----------------------------------------------------------------------------
long long unsigned
ShortestPath::TotalCost( NODE* pPath)
{
  // ci devono essere almeno due nodi
   if ( pPath == nullptr  ||  pPath->pNext == nullptr)
      return 0 ;
  // lunghezza del primo arco
   long long unsigned nCost = ArcCost( pPath->nPos, pPath->pNext->nPos) ;
  // ciclo sui nodi successivi (calcolo la lunghezza degli archi che li uniscono)
   NODE* pCurr = pPath->pNext ;
   while ( pCurr != pPath) {
      nCost += ArcCost( pCurr->nPos, pCurr->pNext->nPos) ;
      pCurr = pCurr->pNext ;
   }
  // se sono presenti delle dipendenze suggerite violate, aumento il costo
   int nBreak = 0 ;
   if ( ! IsPathRespectingSuggestedDependences( pPath, nBreak))
      nCost += m_nMaxDistSuggDep * nBreak ;
   return nCost ;
}

//----------------------------------------------------------------------------
void
ShortestPath::UpdateOrder( NODE* pPath)
{
  // per circuiti aperti si parte dopo il nodo finale aggiunto per poter creare il lato nullo
   if ( m_nType == SP_OPEN) {
      NODE* pCurr = pPath ;
      for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
         if ( pCurr->nPos == m_nNumPnts - 1) {
            pPath = pCurr->pNext ;
            break ;
         }
         pCurr = pCurr->pNext ;
      }
   }
  // assegno l'ordinamento trovato
   for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
      m_nOrder[i] = pPath->nPos ;
      pPath = pPath->pNext ;
   }
  // stampe di debug
   #if 0
      MY_LOG( "Positions :") ;
      string sOut ;
      for ( unsigned i = 0 ; i < m_nNumPnts ; ++ i) {
         sOut += ToString( (int)m_nOrder[i]) + "  " ;
         if ( i % 20 == 19) {
            MY_LOG( sOut.c_str()) ;
            sOut.clear() ;
         }
      }
      if ( ! sOut.empty())
         MY_LOG( sOut.c_str()) ;
   #endif
}