//----------------------------------------------------------------------------
//  EgalTech 2025-2025
//----------------------------------------------------------------------------
// File : MachOptimization.cpp        Data : 31.03.25        Versione : 2.7a1
// Contenuto : Classe per calcolo ottimizzato per le lavorazioni.
//
//
// Modifiche : 31.03.2025 RE Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "DllMain.h"
#include "MachOptimization.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include "/EgtDev/Include/EGkGeoConst.h"
#include "/EgtDev/Include/ENkShortestPath.h"
#include "/EgtDev/Include/EgtPointerOwner.h"
#include <set>
#include <new>

/* Nomenclatura :
   - TC = Tool Change ( cambio utensile )
*/

#define _DEBUG_OPT 0
#define _TIME_DEBUG 0
#if _TIME_DEBUG
   #include "/EgtDev/Include/EgtPerfCounter.h"
#endif

using namespace std ;

// Tempo massimo di lavorazione
static const int MAXTIME = 1073741823U ;
// Dato che tutte le distanze sono salvate come intere ( se ho 4 lavorazioni disposte sui vertici di un
// quadrato unitario, la distanza tra i vertici di uno stesso lato e tra i vertici delle diagonali
// risulta sempre 1, quindi moltiplico per COEFF in modo da avere una maggiore precisione.
// ( vertici su un lato -> dist = 1000, vertici sulla diagonale -> dist = 1414)
// invece che ( vertici su un lato -> dist = int( 1) = 1, vertici sulla diagonale -> dist = int( sqrt( 2)) = 1)
static const int COEFF = 1000 ;

//----------------------------------------------------------------------------
IMachOptimization*
CreateMachOptimization( void)
{
   return static_cast<IMachOptimization*> ( new( nothrow) MachOptimization) ;
}

//----------------------------------------------------------------------------
MachOptimization::MachOptimization( void)
{
   m_vMachOptm.clear() ;
   m_vToolOptm.clear() ;
   m_vDependences.clear() ;
   m_vSuggDependences.clear() ;
   m_dFeedL = 1. ;
   m_dFeedA = 1. ;
   m_bAllMandatory = false ;
   m_bOptInGroup = false ;
   m_vOpenBounds.clear() ;
}

//----------------------------------------------------------------------------
MachOptimization::~MachOptimization( void)
{
}

//----------------------------------------------------------------------------
/* Funzione per inserire un Utensile */
bool
MachOptimization::InsertTool( int nId, double dTCX, double dTCY, double dTCZ, double dTCA,
                              double dTCB, double dTCC, bool bX, bool bY, bool bZ, bool bA,
                              bool bB, bool bC, double dTLoad, double dTUnL)
{
  // Controllo se Id già presente
   for ( const ToolOptm& currTool : m_vToolOptm) {
      if ( currTool.nId == nId) {
         LOG_ERROR( GetENkLogger(), ( "Duplicated Tool id : " + ToString( nId)).c_str()) ;
         return false ;
      }
   }
  // Tempi sempre positivi
   if ( dTLoad < 0.  ||  dTUnL < 0.) {
      LOG_ERROR( GetENkLogger(), string( ( "Time must be positive :")).c_str()) ;
      return false ;
   }
  // Inserisco il nuovo record
   m_vToolOptm.emplace_back( nId, dTCX, dTCY, dTCZ, dTCA, dTCB, dTCC, bX, bY, bZ, bA, bB, bC, dTLoad, dTUnL) ;
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per inserire una Lavorazione */
bool
MachOptimization::InsertMachining( int nId, int nToolId, int nGroup,
                                   double dX_Start, double dY_Start, double dZ_Start,
                                   double dA_Start, double dB_Start, double dC_Start,
                                   double dX_End, double dY_End, double dZ_End,
                                   double dA_End, double dB_End, double dC_End)
{
  // Controllo se Id già presente
   for ( const MachOptm& currMach : m_vMachOptm) {
      if ( currMach.nId == nId) {
         LOG_ERROR( GetENkLogger(), ( "Duplicated Machining id : " + ToString( nId)).c_str()) ;
         return false ;
      }
   }
  // Controllo che l'utensile sia stato precedentemente inserito
   bool bOk = false ;
   for ( const ToolOptm& currTool : m_vToolOptm) {
      bOk = currTool.nId == nToolId ;
      if ( bOk)
         break ;
   }
   if ( ! bOk) {
      LOG_ERROR( GetENkLogger(), ( "Tool not found : " + ToString( nToolId)).c_str()) ;
      return false ;
   }
  // Inserisco il nuovo record
   m_vMachOptm.emplace_back( nId, nToolId, nGroup, dX_Start, dY_Start, dZ_Start,
                             dA_Start, dB_Start,dC_Start, dX_End, dY_End, dZ_End, dA_End, dB_End, dC_End) ;
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per l'inserimento delle dipendenze di ordine tra le lavorazioni [devono essere rispettate]
   ( nIdPrev deve essere eseguita prima di nIdNext ) */
bool
MachOptimization::InsertDependence( int nIdPrev, int nIdNext)
{
  // Check della dipendenza con quelle inserite
   bool bExist = false ;
   bool bConflict = false ;
   if ( ! CheckDependences( nIdPrev, nIdNext, m_vDependences, false, bExist, bConflict))
      return false ;
  // Se esiste di già, non faccio nulla
   if ( bExist)
      return true ;
  // Se in conflitto, allora errore
   if ( bConflict)
      return false ;
  // Inserisco la nuova dipendenza
   m_vDependences.emplace_back( make_pair( nIdPrev, nIdNext)) ;
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per l'inserimento delle dipendenze di ordine tra le lavorazioni [si cerca di rispettarle]
   ( nIdPrev dovrebbe essere eseguita prima di nIdNext ) */
bool
MachOptimization::InsertSuggestedDependences( int nIdPrev, int nIdNext)
{
  // Check della dipendenza con quelle inserite
   bool bExist = false ;
   bool bConflict = false ;
   if ( ! CheckDependences( nIdPrev, nIdNext, m_vSuggDependences, false, bExist, bConflict))
      return false ;
  // Se già presente non faccio nulla
   if ( bExist)
      return true ;
  // Inserisco la nuova dipendenza
   m_vSuggDependences.emplace_back( make_pair( nIdPrev, nIdNext)) ;
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per impostare la prima Lavorazione */
bool
MachOptimization::SetFirstMachining( int nId)
{
  // Controllo che l'Id sia presente
   if ( find_if( m_vMachOptm.begin(), m_vMachOptm.end(), [&]( const MachOptm& Lav) {
           return Lav.nId == nId ;
        }) == m_vMachOptm.end()) {
      LOG_ERROR( GetENkLogger(), ( "Machining id not found : " + ToString( nId)).c_str()) ;
      return false ;
   }
  // Assegno la prima lavorazione
   for ( int i = 0 ; i < int( m_vMachOptm.size()) ; ++ i) {
      if ( m_vMachOptm[i].nId == nId)
         continue ;
      if ( ! InsertDependence( nId, m_vMachOptm[i].nId))
         return false ;
   }
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per impostare l'ultima Lavorazione */
bool
MachOptimization::SetLastMachining( int nId)
{
  // Controllo che l'Id sia presente
   if ( find_if( m_vMachOptm.begin(), m_vMachOptm.end(), [&]( const MachOptm& Lav) {
           return Lav.nId == nId ;
        }) == m_vMachOptm.end()) {
      LOG_ERROR( GetENkLogger(), ( "Machining id not found : " + ToString( nId)).c_str()) ;
      return false ;
   }
  // Assegno l'ultima lavorazione
   for ( int i = 0 ; i < int( m_vMachOptm.size()) ; ++ i) {
      if ( m_vMachOptm[i].nId == nId)
         continue ;
      if ( ! InsertDependence( m_vMachOptm[i].nId, nId))
         return false ;
   }
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per impostare la Feed Lineare e Angolare */
bool
MachOptimization::SetFeeds( double dFeedL, double dFeedA)
{
  // Controllo dei valori
   if ( dFeedL < EPS_ZERO  ||  dFeedA < EPS_ZERO) {
      LOG_ERROR( GetENkLogger(), string( " Feeds must be positive").c_str()) ;
      return false ;
   }
  // Assegno le Feed
   m_dFeedL = dFeedL ;
   m_dFeedA = dFeedA ;
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per impostare ogni dipendenza da Gruppo come vincolo */
bool
MachOptimization::SetAllGroupsAsMandatory( bool bAllMandatory)
{
   m_bAllMandatory = bAllMandatory ;
   return true ;
}

//----------------------------------------------------------------------------
bool
MachOptimization::SetOptimizationForGroups( bool bOptForGroups)
{
   m_bOptInGroup = bOptForGroups ;
   return true ;
}

//-----------------------------------------------------------------------------
bool
MachOptimization::SetOpenBound( bool bStartVsEnd, int nFlag, double dX, double dY, double dZ)
{
  // Se parametro Start o End già impostato, lo sovrascrivo
   for ( int i = 0 ; i < int( m_vOpenBounds.size()) ; ++ i) {
      if ( m_vOpenBounds[i].bStartVsEnd == bStartVsEnd) {
         m_vOpenBounds[i].nFlag = nFlag ;
         m_vOpenBounds[i].dX = dX ;
         m_vOpenBounds[i].dY = dY ;
         m_vOpenBounds[i].dZ = dZ ;
         return true ;
      }
   }
  // altrimenti lo inserisco
   m_vOpenBounds.emplace_back( -1, bStartVsEnd, nFlag, dX, dY, dZ) ;
   return true ;
}

//----------------------------------------------------------------------------
bool
MachOptimization::SetOpenBoundForGroups( int nGroup, bool bStartVsEnd, int nFlag, double dX, double dY, double dZ)
{
  // Se parametro Start o End del Gruppo scelto già impostato, lo sovrascrivo
   for ( int i = 0 ; i < int( m_vOpenBounds.size()) ; ++ i) {
      if ( m_vOpenBounds[i].nGroup == nGroup  &&  m_vOpenBounds[i].bStartVsEnd == bStartVsEnd) {
         m_vOpenBounds[i].nFlag = nFlag ;
         m_vOpenBounds[i].dX = dX ;
         m_vOpenBounds[i].dY = dY ;
         m_vOpenBounds[i].dZ = dZ ;
         return true ;
      }
   }
  // altrimenti lo inserisco
   m_vOpenBounds.emplace_back( nGroup, bStartVsEnd, nFlag, dX, dY, dZ) ;
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione che assegna le dipendenze suggerite in base ai Gruppi */
bool
MachOptimization::SetGroupDependences( void)
{
  // Se ho meno di due lavorazioni, non devo fare nulla
   if ( int( m_vMachOptm.size()) < 2)
      return true ;
  // Recupero i diversi gruppi ( ordinati per priorità, quindi ordine decrescente)
   set<int, greater<int>> setGroup ;
   for ( const MachOptm& currLav : m_vMachOptm)
      setGroup.insert( currLav.nGroup) ;
  // Se presente un solo gruppo, non devo fare nulla
   if ( int( setGroup.size()) == 1)
      return true ;
  // Scorro i gruppi ordinati per priorità e assegno le dipendenze suggerite
   for ( auto it = setGroup.begin() ; it != setGroup.end() ; ++ it) {
     // Cerco tutti gli indici delle lavorazioni con quella priorità e quelli con priorità inferiore
      INTVECTOR vIndSamePriorityLav ; vIndSamePriorityLav.reserve( m_vMachOptm.size()) ;
      INTVECTOR vIndLowerPriorityLav ; vIndLowerPriorityLav.reserve( m_vMachOptm.size()) ;
      for ( int i = 0 ; i < int( m_vMachOptm.size()) ; ++ i) {
         if ( m_vMachOptm[i].nGroup == *it)
            vIndSamePriorityLav.emplace_back( i) ;
         else if ( m_vMachOptm[i].nGroup < *it)
            vIndLowerPriorityLav.emplace_back( i) ;
      }
     // Scorro le Lavorazioni che hanno la priorità massima corrente
      for ( const int& nInd : vIndSamePriorityLav) {
        // Scorro le Lavorazioni con priorità inferiore
         for ( const int& nInd_LowerPriority : vIndLowerPriorityLav) {
           // Assegno la dipendenze
            if ( m_bAllMandatory) {
               if ( ! InsertDependence( m_vMachOptm[nInd].nId, m_vMachOptm[nInd_LowerPriority].nId))
                  return false ;
            }
            else {
               if ( ! InsertSuggestedDependences( m_vMachOptm[nInd].nId, m_vMachOptm[nInd_LowerPriority].nId))
                  return false ;
            }
         }
      }
   }
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per il controllo delle dipendenze inserite : se già presenti o se in conflitto */
bool
MachOptimization::CheckDependences( int nIdPrev, int nIdNext, const INTINTVECTOR& vDep,
                                    bool bBlockMessages, bool& bExist, bool& bConflict) const 
{
   bExist = false ;
   bConflict = false ;
  // controllo che tali Id siano entrambi presenti tra le lavorazioni, in caso opposto errore
   bool bOkIdPrev = false ;
   bool bOkIdNext = false ;
   for ( const MachOptm& currLav : m_vMachOptm) {
      if ( ! bOkIdPrev  &&  currLav.nId == nIdPrev)
         bOkIdPrev = true ;
      if ( ! bOkIdNext  &&  currLav.nId == nIdNext)
         bOkIdNext = true ;
      if ( bOkIdPrev  &&  bOkIdNext)
         break ;
   }
   if ( ! bOkIdPrev  ||  ! bOkIdNext) {
      if ( ! bBlockMessages)
         LOG_ERROR( GetENkLogger(), ( "Discharged Dependence, Id not found (" + ToString( nIdPrev) + "," + ToString( nIdNext) + ")").c_str()) ;
      return false ;
   }

  // controllo se la dipendenza è già stata inserita e che non sia presente una dipendeza inversa (conflitto)
  // (... (Id_i, Id_j) , ... , ( Id_j, Id_i), ...)
   for ( const auto& currDep : vDep) {
      if ( currDep.first == nIdPrev  &&  currDep.second == nIdNext) {
         bExist = true ;
         if ( ! bBlockMessages)
            LOG_INFO( GetENkLogger(), ( "Already found Dependence (" + ToString( nIdPrev) + "," + ToString( nIdNext) + ")").c_str()) ;
         return true ;
      }
      if ( currDep.second == nIdPrev && currDep.first == nIdNext) {
         bConflict = true ;
         if ( ! bBlockMessages)
            LOG_ERROR( GetENkLogger(), ( "Conflict Dependence (" + ToString( nIdPrev) + "," + ToString( nIdNext) + ")").c_str()) ;
         return true ;
      }
   }
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per il calcolo della distanza euclidea tra assi Lineari o Angolari di due lavorazioni
   Considerando lo stesso utensile */
double
MachOptimization::CalcTimeLavLav( const MachOptm& Lav1, const MachOptm& Lav2, bool bLin) const
{
   double dDist = 0. ;
   double dFeed = EPS_SMALL ;
   if ( bLin) {
     /* Lav1 -- Feed Lineare --> Lav2 */
      dDist = sqrt( ( Lav2.dX_Start - Lav1.dX_End) * ( Lav2.dX_Start - Lav1.dX_End) +
                    ( Lav2.dY_Start - Lav1.dY_End) * ( Lav2.dY_Start - Lav1.dY_End) +
                    ( Lav2.dZ_Start - Lav1.dZ_End) * ( Lav2.dZ_Start - Lav1.dZ_End)) ;
      dFeed = m_dFeedL ;
   }
   else {
    /* Lav1 -- Feed Angolare --> Lav2 */
     dDist = sqrt( ( Lav2.dA_Start - Lav1.dA_End) * ( Lav2.dA_Start - Lav1.dA_End) +
                   ( Lav2.dB_Start - Lav1.dB_End) * ( Lav2.dB_Start - Lav1.dB_End) +
                   ( Lav2.dC_Start - Lav1.dC_End) * ( Lav2.dC_Start - Lav1.dC_End)) ;
     dFeed = m_dFeedA ;
   }
   return COEFF * ( dDist / dFeed) ;
}

//----------------------------------------------------------------------------
/* Funzione per il calcolo della distanza euclidea tra assi Lineari o Angolari tra una lavorazione e
   il suo TC */
bool
MachOptimization::CalcTimeLavLavTC( const MachOptm& Lav1, const MachOptm& Lav2, bool bLin,
                                    double& dTimeLav1TC1, double& dTimeTC1TC2, double& dTimeTC2Lav2,
                                    double& dTUnload1, double& dTLoad2) const
{
  /*
    dTimeLav1TC1 : Tempo tra EndPoint di Lav1 e Posizione di TC per Utensile di Lav1
    dTimeTC1TC2 : Tempo di cambio utensile tra Tool di Lav1 e Lav2
    dTimeTC2Lav2 : Tempo tra TC per utensile Lav2 e StartPoint di Lav2
  */
   dTimeLav1TC1 = 0. ;
   dTimeTC1TC2 = 0. ;
   dTimeTC2Lav2 = 0. ;
   dTUnload1 = 0. ;
   dTLoad2 = 0. ;
  // Recupero Utensile Lav1 e Lav2
   int nIndToolLav1 = -1 ;
   int nIndToolLav2 = -1 ;
   bool bFoundToolLav1 = false ;
   bool bFoundToolLav2 = false ;
   for ( int i = 0 ; i < int( m_vToolOptm.size()) ; ++ i) {
      bFoundToolLav1 = ( m_vToolOptm[i].nId == Lav1.nIdTool) ;
      if ( bFoundToolLav1)
         nIndToolLav1 = i ;
      bFoundToolLav2 = ( m_vToolOptm[i].nId == Lav2.nIdTool) ;
      if ( bFoundToolLav2)
         nIndToolLav2 = i ;
      if ( nIndToolLav1 != -1  &&  nIndToolLav2 != -1)
         break ;
   }
   if ( nIndToolLav1 == -1)
      LOG_INFO( GetENkLogger(), ( "Tool Not Found :" + ToString( Lav1.nIdTool)).c_str()) ;
   if ( nIndToolLav2 == -1)
      LOG_INFO( GetENkLogger(), ( "Tool Not Found :" + ToString( Lav2.nIdTool)).c_str()) ;
   if ( nIndToolLav1 == -1  ||  nIndToolLav2 == -1)
      return false ;
   const ToolOptm& ToolLav1 = m_vToolOptm[nIndToolLav1] ;
   const ToolOptm& ToolLav2 = m_vToolOptm[nIndToolLav2] ;
  // Imposto i tempi di carico e scarico degli utensili
   dTUnload1 = COEFF * ToolLav1.dTUnLoad ;
   dTLoad2 = COEFF * ToolLav2.dTLoad ;
  // Se distanza lineare
   if ( bLin) {
     // Come primo tempo considero quello tra la fine di Lav1 e TC1
      double dXi = Lav1.dX_End ;
      double dXe = ( ToolLav1.bTCX ? ToolLav1.dTCX : dXi) ;
      double dYi = Lav1.dY_End ;
      double dYe = ( ToolLav1.bTCY ? ToolLav1.dTCY : dYi) ;
      double dZi = Lav1.dZ_End ;
      double dZe = ( ToolLav1.bTCZ ? ToolLav1.dTCZ : dZi) ;
      double dDist = sqrt( ( dXe - dXi) * ( dXe - dXi) +  ( dYe - dYi) * ( dYe - dYi) + ( dZe - dZi) * ( dZe - dZi)) ;
      dTimeLav1TC1 = COEFF * ( dDist / m_dFeedL) ;
     // Come secondo tempo considero quello tra TC1 e TC2
      dXi = dXe ;
      dXe = ( ToolLav2.bTCX ? ToolLav2.dTCX : dXi) ;
      dYi = dYe ;
      dYe = ( ToolLav2.bTCY ? ToolLav2.dTCY : dYi) ;
      dZi = dZe ;
      dZe = ( ToolLav2.bTCZ ? ToolLav2.dTCZ : dZi) ;
      dDist = sqrt( ( dXe - dXi) * ( dXe - dXi) +  ( dYe - dYi) * ( dYe - dYi) + ( dZe - dZi) * ( dZe - dZi)) ;
      dTimeTC1TC2 = COEFF * ( dDist / m_dFeedL) ;
     // Come terzo tempo considero quello tra TC2 e Lav2
      dXi = dXe ;
      dYe = Lav2.dX_Start ;
      dYi = dYe ;
      dYe = Lav2.dY_Start ;
      dZi = dZe ;
      dZe = Lav2.dZ_Start ;
      dDist = sqrt( ( dXe - dXi) * ( dXe - dXi) +  ( dYe - dYi) * ( dYe - dYi) + ( dZe - dZi) * ( dZe - dZi)) ;
      dTimeTC2Lav2 = COEFF * ( dDist / m_dFeedL) ;
   }
  // Se distanza angolare
   else {
     // Come primo tempo considero quello tra la fine di Lav1 e TC1
      double dAi = Lav1.dA_Start ;
      double dAe = ( ToolLav1.dTCA ? ToolLav1.dTCA : dAi) ;
      double dBi = Lav1.dB_Start ;
      double dBe = ( ToolLav1.dTCB ? ToolLav1.dTCB : dBi) ;
      double dCi = Lav1.dC_Start ;
      double dCe = ( ToolLav1.dTCC ? ToolLav1.dTCC : dCi) ;
      double dDist = sqrt( ( dAe - dAi) * ( dAe - dAi) + ( dBe - dBi) * ( dBe - dBi) + ( dCe - dCi) * ( dCe - dCi)) ;
      dTimeLav1TC1 = COEFF * ( dDist / m_dFeedA) ;
     // Come secondo tempo considero quello tra TC1 e TC2
      dAi = dAe ;
      dAe = ( ToolLav2.bTCA ? ToolLav2.dTCA : dAi) ;
      dBi = dBe ;
      dBe = ( ToolLav2.bTCB ? ToolLav2.dTCB : dBi) ;
      dCi = dCe ;
      dCe = ( ToolLav2.bTCC ? ToolLav2.dTCC : dCi) ;
      dDist = sqrt( ( dAe - dAi) * ( dAe - dAi) +  ( dBe - dBi) * ( dBe - dBi) + ( dCe - dCi) * ( dCe - dCi)) ;
      dTimeTC1TC2 = COEFF * ( dDist / m_dFeedA) ;
     // Come terzo tempo considero quello tra TC2 e Lav2
      dAi = dAe ;
      dAe = Lav2.dA_Start ;
      dBi = dBe ;
      dBe = Lav2.dB_Start ;
      dCi = dCe ;
      dCe = Lav2.dC_Start ;
      dDist = sqrt( ( dAe - dAi) * ( dAe - dAi) +  ( dBe - dBi) * ( dBe - dBi) + ( dCe - dCi) * ( dCe - dCi)) ;
      dTimeTC2Lav2 = COEFF * ( dDist / m_dFeedA) ;
   }
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per calcolare il tempo nel passare dalla lavorazione nInd1-esima alla lavorazione
   nInd2-esima */
bool
MachOptimization::CalcMachTime( int nInd1, int nInd2, int& nTime) const
{
  // Se Indice fuori dal range, errore
   nTime = 0 ;
   if ( nInd1 < 0  ||  nInd1 >= int( m_vMachOptm.size())  ||
        nInd2 < 0  ||  nInd2 >= int( m_vMachOptm.size()))
      return false ;

  // Recupero riferimento delle due lavorazioni
   const MachOptm& Lav1 = m_vMachOptm[nInd1] ;
   const MachOptm& Lav2 = m_vMachOptm[nInd2] ;

   return ( CalcMachTime( Lav1, Lav2, nTime)) ;
}

//----------------------------------------------------------------------------
/* Funzione per calcolare il tempo nel passare da una lavorazione all'altra */
bool
MachOptimization::CalcMachTime( const MachOptm& Lav1, const MachOptm& Lav2, int& nTime) const
{
  // Se l'utensile è lo stesso
   if ( Lav1.nIdTool == Lav2.nIdTool)
      nTime = int( max( CalcTimeLavLav( Lav1, Lav2, true), CalcTimeLavLav( Lav1, Lav2, false))) ;
   else {
      double dLTime1, dLTime2, dLTime3 ;
      double dATime1, dATime2, dATime3 ;
      double dTUnload1, dTLoad2 ;
      if ( ! CalcTimeLavLavTC( Lav1, Lav2, true, dLTime1, dLTime2, dLTime3, dTUnload1, dTLoad2)  ||
           ! CalcTimeLavLavTC( Lav1, Lav2, false, dATime1, dATime2, dATime3, dTUnload1, dTLoad2)) {
         LOG_ERROR( GetENkLogger(), "Error in computing time") ;
         return false ;
      }
      nTime += int( max( dLTime1, dATime1) + max( dLTime2, dATime2) + max( dLTime3, dATime3)) ;
     // Sommo i tempi di carico e scarico dei due utensili
      nTime += int( dTUnload1) + int( dTLoad2) ;
   }
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione per il calcolo della Matrice dei tempi (NxN), dove N = #Lavorazioni */
bool
MachOptimization::ComputeTimeMatrix( INTMATRIX& mTimes) const
{
  // Creo una copia dei vettori delle dipendenze ( le cancello di volta in volta che le assegno)
   const int N = int( m_vMachOptm.size()) ;
  // Matrice NxN ( con Flag per cella già calcolata)
   mTimes.clear() ;
   mTimes.resize( N) ;
   for ( auto& Row : mTimes)
      Row.resize( N) ;
  // Calcolo i tempi tra tutte le lavorazioni
   for ( int i = 0 ; i < N ; ++ i) {
      for ( int j = 0 ; j < N ; ++ j) {
        // Se sono sulla diagonale, allora il tempo è infinito
         if ( i == j) {
            mTimes[i][j] = MAXTIME ;
            continue ;
         }
        // Se sono nella cella ( i, j), calcolo il tempo necessario
         if ( ! CalcMachTime( i, j, mTimes[i][j]))
            return false ;
      }
   }
   #if _DEBUG_OPT
   string sRowSplit = "" ;
   for ( int i = 0 ; i < N ; ++ i)
      sRowSplit += "----------" ;
   for ( int i = -1 ; i < N ; ++ i) {
     // Intestazione
      if ( i == -1) {
         LOG_INFO( GetENkLogger(), sRowSplit.c_str()) ;
         string sCell = "**********" ;
         for ( int j = 0 ; j < N ; ++ j) {
            int nId = m_vMachOptm[j].nId ;
            string sId = ToString( nId) ;
            int nLen = sId.length() ;
            for ( int k = 0 ; k < 10 - nLen ; ++ k)
               sCell += " " ;
            sCell += sId ;
         }
         LOG_INFO( GetENkLogger(), sCell.c_str()) ;
         continue ;
      }
     // Prima riga della colonna
      string sRow = "|" ;
      int nId = m_vMachOptm[i].nId ;
      string sId = ToString( nId) ;
      int nLen = sId.length() ;
      for ( int k = 0 ; k < 8 - nLen ; ++ k)
         sRow += " " ;
      sRow += sId ;
      sRow += "|" ;
      for ( int j = 0 ; j < N ; ++ j) {
         double dTime = mTimes[i][j] ;
         string sTime = ( dTime > MAXTIME - 1 ? "inf" : ToString( dTime, 1)) ;
         int nLen = sTime.length() ;
         for ( int k = 0 ; k < 9 - nLen ; ++ k)
            sRow += " " ;
         sRow += sTime ;
         sRow += "|" ;
      }
      LOG_INFO( GetENkLogger(), sRow.c_str()) ;
   }
   LOG_INFO( GetENkLogger(), sRowSplit.c_str()) ;
   #endif
   return true ;
}

//----------------------------------------------------------------------------
/* Funzione che restituisce la posizione della lavorazione di Id pari a nId nel vettore delle lavorazioni */
int
MachOptimization::GetIndById( int nId) const
{
  // Scorro le lavorazioni
   for ( int i = 0 ; i < int( m_vMachOptm.size()) ; ++ i) {
      if ( m_vMachOptm[i].nId == nId)
         return i ;
   }
   return -1 ;
}

//----------------------------------------------------------------------------
/* Funzione per separare le lavorazioni e le dipendenze a seconda dei gruppi */
bool
MachOptimization::SplitMachOptsInGroups( MACHOPTMMATRIX& matMachOpt, vector<INTINTVECTOR>& matDep,
                                         vector<INTINTVECTOR>& matSuggDep)
{
  // Se non ho lavorazioni, non faccio nulla
   if ( m_vMachOptm.empty())
      return true ;
  // Se non devo ottimizzare per gruppi, errore
   if ( ! m_bOptInGroup)
      return false ;

  // Ordino le lavorazioni in base al gruppo
   sort( m_vMachOptm.begin(), m_vMachOptm.end(), []( const MachOptm& MachA, const MachOptm& MachB) {
      return ( MachA.nGroup > MachB.nGroup) ;
   }) ;

  // Separo le lavorazioni
   int nHighestPriority = m_vMachOptm[0].nGroup ;
   matMachOpt.emplace_back( MACHOPTMVECTOR{}) ;
   matMachOpt.back().emplace_back( m_vMachOptm[0]) ;
   for ( int i = 1 ; i < int( m_vMachOptm.size()) ; ++ i) {
      if ( m_vMachOptm[i].nGroup < nHighestPriority) {
         matMachOpt.emplace_back( MACHOPTMVECTOR{}) ;
         nHighestPriority = m_vMachOptm[i].nGroup ;
      }
      matMachOpt.back().emplace_back( m_vMachOptm[i]) ;
   }

  // Recupero le Dipendeze Obbligatorie e Suggerite tra i vari gruppi
   BOOLVECTOR vbDep( m_vDependences.size(), false) ;
   BOOLVECTOR vbSuggDep( m_vSuggDependences.size(), false) ;
   for ( const MACHOPTMVECTOR& vMachOpt : matMachOpt) {
     // Recupero gli Id delle lavorazioni correnti
      INTVECTOR vId ; vId.reserve( vMachOpt.size()) ;
      for ( const MachOptm& Lav : vMachOpt)
         vId.push_back( Lav.nId) ;
     // Inizializzo i vettori delle dipendenze
      matDep.emplace_back( INTINTVECTOR{}) ;
      matSuggDep.emplace_back( INTINTVECTOR{}) ;
     // Recupero le dipendenze associate a questo gruppo
     // NB. Devono essere solamente dipendenze interne al gruppo ( le altre vengono scartate a priori)
      for ( int i = 0 ; i < int( m_vDependences.size()) ; ++ i) {
         if ( ! vbDep[i]  &&
              find( vId.begin(), vId.end(), m_vDependences[i].first) != vId.end()  &&
              find( vId.begin(), vId.end(), m_vDependences[i].second) != vId.end()) {
            matDep.back().emplace_back( m_vDependences[i]) ;
            vbDep[i] = true ;
         }
      }
      for ( int i = 0 ; i < int( m_vSuggDependences.size()) ; ++ i) {
         if ( ! vbSuggDep[i]  &&
              find( vId.begin(), vId.end(), m_vSuggDependences[i].first) != vId.end()  &&
              find( vId.begin(), vId.end(), m_vSuggDependences[i].second) != vId.end()){
            matSuggDep.back().emplace_back( m_vSuggDependences[i]) ;
            vbSuggDep[i] = true ;
         }
      }
   }

   return true ;
}

//----------------------------------------------------------------------------
bool
MachOptimization::GetGlobalResult( INTVECTOR& vIds)
{
  // Inserisco le lavorazioni per ordine di inserimento
   for ( const MachOptm& myLav : m_vMachOptm)
      vIds.push_back( myLav.nId) ;
  // Inizializzo le dipendenze suggerite dai gruppi di lavorazione
   if ( ! SetGroupDependences()) {
      LOG_ERROR( GetENkLogger(), "Error in defining Group depedences") ;
      return false ;
   }
  // Calcolo la matrice dei tempi
   INTMATRIX mTimes ;
   if ( ! ComputeTimeMatrix( mTimes)) {
      LOG_ERROR( GetENkLogger(), "Error in Computing Time Matrix") ;
      return false ;
   }
  // Istanzio il Problema ShortesPath con vincoli
   PtrOwner<IShortestPath> mySP( CreateShortestPath()) ;
   if ( IsNull( mySP))
      return false ;
  // Per ogni lavorazione inserisco gli estremi
   for ( const MachOptm& Lav : m_vMachOptm) {
      if ( ! mySP->AddPoint( Lav.dX_Start, Lav.dY_Start, Lav.dZ_End, 0., 0.,
                             Lav.dX_End, Lav.dY_End, Lav.dZ_End, 0., 0.)) {
         LOG_ERROR( GetENkLogger(), "Error adding Machining Bounds") ;
         return false ;
      }
   }
  // Assegno la matrice delle distanze calcolata in precedenza
   if ( ! mySP->SetDistMatrix( mTimes)) {
      LOG_ERROR( GetENkLogger(), "Error setting distances matrix") ;
      return false ;
   }
  // Per ogni dipendeza obbligatoria assegno un vincolo
   for ( const INTINT& nnDep : m_vDependences) {
      if ( ! mySP->SetConstraintOrder( GetIndById( nnDep.first), GetIndById( nnDep.second))) {
         LOG_ERROR( GetENkLogger(), "Error in adding Constraints") ;
         return false ;
      }
   }
  // Aggiungo le dipendenza suggerite
   for ( const INTINT& nnSuggDep : m_vSuggDependences) {
      if ( ! mySP->SetSuggestedOrder( GetIndById( nnSuggDep.first), GetIndById( nnSuggDep.second))) {
         LOG_ERROR( GetENkLogger(), "Error in adding Suggested Dependence") ;
         return false ;
      }
   }
  // Se presenti dei incoli di OpenBound, li assegno
   if ( ! m_vOpenBounds.empty()) {
      for ( const OpenBoundOptm& OB : m_vOpenBounds) {
         if ( OB.nGroup == -1) {
            if ( ! mySP->SetOpenBound( OB.bStartVsEnd, OB.nFlag, OB.dX, OB.dY, OB.dZ, 0., 0.)) {
               LOG_ERROR( GetENkLogger(), "Error in adding OpenBound") ;
               return false ;
            }
         }
      }
   }
  // Calcolo la soluzione
   #if _TIME_DEBUG
      PerformanceCounter PC ; PC.Start() ;
   #endif
   if ( ! mySP->Calculate( IShortestPath::SP_OPEN)) {
      LOG_ERROR( GetENkLogger(), "Error in computing solution") ;
      return false ;
   }
   #if _TIME_DEBUG
      string sTime = string( to_string( m_vDependences.size()) + string( "Time : ") + ToString( PC.Stop())) ;
      LOG_INFO( GetENkLogger(), sTime.c_str()) ;
   #endif
  // Recupero l'ordine
   INTVECTOR U ;
   if ( ! mySP->GetOrder( U)) {
      LOG_ERROR( GetENkLogger(), "Error in ordering") ;
      return false ;
   }
  // Restituisco gli Id ordinati
   vIds.resize( m_vMachOptm.size()) ;
   for ( int nInd = 0 ; nInd < int( U.size()) ; ++ nInd)
      vIds[nInd] = m_vMachOptm[U[nInd]].nId ;

   return true ;
}

//----------------------------------------------------------------------------
bool
MachOptimization::GetGroupResult( INTVECTOR& vIds)
{
  // Se Flag ottimizzazione di gruppo non attivo, errore
   if ( ! m_bOptInGroup)
      return false ;

  // Recupero tutti i gruppi e le dipendenze separatamente ed ordinate per priorità di gruppo
   MACHOPTMMATRIX matMachOpt ;
   vector<INTINTVECTOR> matDep, matSuggDep ;
   if ( ! SplitMachOptsInGroups( matMachOpt, matDep, matSuggDep))
      return false ;
  // Per ognuna di esse eseguo l'algoritmo di ShortestPath
   MachOptm LastMach ;
   for ( int i = 0 ; i < int( matMachOpt.size()) ; ++ i) {
      m_vMachOptm.clear() ;
      m_vDependences.clear() ;
      m_vSuggDependences.clear() ;
      if ( matMachOpt.empty())
         continue ;
      m_vMachOptm = matMachOpt[i] ;
      m_vDependences = matDep[i] ;
      m_vSuggDependences = matSuggDep[i] ;
     // Calcolo la matrice dei tempi
      INTMATRIX mTimes ;
      if ( ! ComputeTimeMatrix( mTimes)) {
         LOG_ERROR( GetENkLogger(), "Error in Computing Time Matrix") ;
         return false ;
      }
     // Istanzio il Problema ShortesPath con vincoli
      PtrOwner<IShortestPath> mySP( CreateShortestPath()) ;
      if ( IsNull( mySP))
         return false ;
     // Per ogni lavorazione inserisco gli estremi
      for ( const MachOptm& Lav : m_vMachOptm) {
         if ( ! mySP->AddPoint( Lav.dX_Start, Lav.dY_Start, Lav.dZ_End, 0., 0.,
                                Lav.dX_End, Lav.dY_End, Lav.dZ_End, 0., 0.)) {
            LOG_ERROR( GetENkLogger(), "Error adding Machining Bounds") ;
            return false ;
         }
      }
     // Assegno la matrice delle distanze calcolata in precedenza
      if ( ! mySP->SetDistMatrix( mTimes)) {
         LOG_ERROR( GetENkLogger(), "Error setting distances matrix") ;
         return false ;
      }
     // Se presente un vincolo OpenBound lo assegno
      bool bForStart = false ;
      for ( const OpenBoundOptm& OB : m_vOpenBounds) {
         if ( OB.nGroup == m_vMachOptm[0].nGroup) {
            bForStart = ( bForStart || OB.bStartVsEnd) ;
            if ( ! mySP->SetOpenBound( OB.bStartVsEnd, OB.nFlag, OB.dX, OB.dY, OB.dZ, 0., 0.)) {
               LOG_ERROR( GetENkLogger(), "Error in adding OpenBound") ;
               return false ;
            }
         }
      }
     // Se ho una lavorazione impostata in precedenza e nessun vincolo OpenBound
      if ( ! bForStart && LastMach.nId != -1) {
        // Cerco le lavorazioni che non presentano precedenze
         MACHOPTMVECTOR vLav ; vLav.reserve( m_vMachOptm.size()) ;
         if ( m_vDependences.empty())
            vLav = m_vMachOptm ;
         else {
            INTSET setSecondDep ;
            for ( const INTINT& Dep : m_vDependences)
               setSecondDep.insert( Dep.second) ;
            for ( int i = 0 ; i < int( m_vMachOptm.size()) ; ++ i) {
               if ( setSecondDep.find( m_vMachOptm[i].nId) == setSecondDep.end())
                  vLav.emplace_back( m_vMachOptm[i]) ;
            }
         }
        // Tra quelle valide cerco la migliore da cui partire
         int nBestLavId = 0 ;
         int nBestTime = MAXTIME ;
         for ( int i = 0 ; i < int( vLav.size()) ; ++ i) {
            int nCurrTime = MAXTIME ;
            CalcMachTime( LastMach, vLav[i], nCurrTime) ;
            if ( nCurrTime < nBestTime) {
               nBestTime = nCurrTime ;
               nBestLavId = vLav[i].nId ;
            }
         }
        // Imposto la prima lavorazione
         if ( ! SetFirstMachining( nBestLavId))
            return false ;
      }
     // Per ogni dipendeza obbligatoria assegno un vincolo
      for ( const INTINT& nnDep : m_vDependences) {
         if ( ! mySP->SetConstraintOrder( GetIndById( nnDep.first), GetIndById( nnDep.second))) {
            LOG_ERROR( GetENkLogger(), "Error in adding Constraints") ;
            return false ;
         }
      }
     // Aggiungo le dipendenza suggerite
      for ( const INTINT& nnSuggDep : m_vSuggDependences) {
         if ( ! mySP->SetSuggestedOrder( GetIndById( nnSuggDep.first), GetIndById( nnSuggDep.second))) {
            LOG_ERROR( GetENkLogger(), "Error in adding Suggested Dependence") ;
            return false ;
         }
      }
     // Calcolo la soluzione
      #if _TIME_DEBUG
         PerformanceCounter PC ; PC.Start() ;
      #endif
      if ( ! mySP->Calculate( IShortestPath::SP_OPEN)) {
         LOG_ERROR( GetENkLogger(), "Error in computing solution") ;
         return false ;
      }
     // Recupero l'ordine
      INTVECTOR U ;
      if ( ! mySP->GetOrder( U)) {
         LOG_ERROR( GetENkLogger(), "Error in ordering") ;
         return false ;
      }
     // Restituisco gli Id ordinati
      for ( int nInd = 0 ; nInd < int( U.size()) ; ++ nInd)
         vIds.emplace_back( m_vMachOptm[U[nInd]].nId) ;
     // Memorizzo l'ultima lavorazione per il gruppo successivo
      LastMach = ( U.empty() ? MachOptm() : m_vMachOptm[U.back()]) ;
   }

   return true ;
}

//----------------------------------------------------------------------------
bool
MachOptimization::Calculate( INTVECTOR& vIds)
{
  // Se non ho lavorazioni, allora non faccio nulla
   vIds.clear() ;
   if ( m_vMachOptm.empty())
      return true ;
  // Se ho una sola lavorazione allora ritorno se stessa
   if ( int( m_vMachOptm.size()) == 1) {
      vIds.push_back( m_vMachOptm[0].nId) ;
      return true ;
   }

  // --- Se richiesta ottimizzazione solo interna per i gruppi
   if ( m_bOptInGroup)
      return ( GetGroupResult( vIds)) ;
  // --- Se invece ottimizzazione globale
   return ( GetGlobalResult( vIds)) ;
}