//----------------------------------------------------------------------------
//  EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : Simulator.cpp           Data : 19.10.15          Versione : 1.6j2
// Contenuto : Implementazione della classe Simulator.
//
//
//
// Modifiche : 19.10.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "Simulator.h"
#include "MachMgr.h"
#include "Machining.h"
#include "MachiningConst.h"
#include "/EgtDev/Include/EMkToolConst.h"
#include "/EgtDev/Include/EMkOperationConst.h"

using namespace std ;

//----------------------------------------------------------------------------
Simulator::Simulator( void)
{
   m_pMchMgr = nullptr ;
   m_pGeomDB = nullptr ;
   m_dStep = 5.0  ;
   m_nOpId = GDB_ID_NULL ;
   m_nCLPathId = GDB_ID_NULL ;
   m_nEntId = GDB_ID_NULL ;
   m_dCoeff = 0 ;
   m_AxesName.reserve( 8) ;
   m_AxesVal.reserve( 8) ;
}

//----------------------------------------------------------------------------
Simulator::~Simulator( void)
{

}

//----------------------------------------------------------------------------
bool
Simulator::Init( MachMgr* pMchMgr)
{
  // verifico ambiente
   if ( pMchMgr == nullptr  ||  pMchMgr->GetContextId() == 0  ||  pMchMgr->GetGeomDB() == nullptr)
      return false ;
   m_pMchMgr = pMchMgr ;
   m_pGeomDB = m_pMchMgr->GetGeomDB() ;
   return true ;
}

//----------------------------------------------------------------------------
bool
Simulator::Start( void)
{
  // verifico ci sia una macchinata corrente
   if ( m_pMchMgr == nullptr)
      return false ;
   int nMachGrp = m_pMchMgr->GetCurrMachGroup() ;
   if ( nMachGrp == GDB_ID_NULL)
      return false ;
  // verifico la disposizione iniziale della macchinata
   int nOpId = m_pMchMgr->GetFirstOperation() ;
   if ( m_pMchMgr->GetOperationType( nOpId) != OPER_DISP)
      return false ;
  // cerco la prima lavorazione
   nOpId = m_pMchMgr->GetNextOperation( nOpId) ;
   while ( nOpId != GDB_ID_NULL  &&  ! IsValidMachiningType( m_pMchMgr->GetOperationType( nOpId)))
      nOpId = m_pMchMgr->GetNextOperation( nOpId) ;
   if ( nOpId == GDB_ID_NULL)
      return false ;
   m_nOpId = nOpId ;

  // aggiornamenti legati al cambio di lavorazione (utensile e assi conseguenti)
   if ( ! UpdateTool()  ||  ! UpdateAxes())
      return false ;

  // porto la macchina in home
   if ( ! GoHome())
      return false ;

  // determino il primo percorso di lavoro
   int nClId = m_pGeomDB->GetFirstNameInGroup( m_nOpId, MCH_CL) ;
   m_nCLPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
   m_nEntId = m_pGeomDB->GetFirstInGroup( m_nCLPathId) ;
   m_dCoeff = 0 ;

   return true ;
}

//----------------------------------------------------------------------------
bool
Simulator::Move( void)
{
  // Verifiche
   if ( m_pMchMgr == nullptr  ||  m_pGeomDB == nullptr)
      return false ;
  // Se arrivato alla fine dell'interpolazione, recupero una nuova entità
   if ( m_dCoeff > 0.999) {
      m_nEntId = m_pGeomDB->GetNext( m_nEntId) ;
      m_dCoeff = 0 ;
   }
  // Se arrivato alla fine di un percorso di lavoro, recupero un nuovo CLpath
   if ( m_nEntId == GDB_ID_NULL) {
      m_nCLPathId = m_pGeomDB->GetNextGroup( m_nCLPathId) ;
      m_nEntId = m_pGeomDB->GetFirstInGroup( m_nCLPathId) ;
      m_dCoeff = 0 ;
   }
  // Se arrivato alla fine di una lavorazione, recupero la successiva
   if ( m_nCLPathId == GDB_ID_NULL) {
      m_nOpId = m_pMchMgr->GetNextOperation( m_nOpId) ;
     // se non ce ne sono altre, sono alla fine
      if ( m_nOpId == GDB_ID_NULL)
         return false ;
     // aggiorno gli altri dati
      int nClId = m_pGeomDB->GetFirstNameInGroup( m_nOpId, MCH_CL) ;
      m_nCLPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
      m_nEntId = m_pGeomDB->GetFirstInGroup( m_nCLPathId) ;
      m_dCoeff = 0 ;
     // aggiorno utensile e assi conseguenti
      if ( ! UpdateTool()  ||  ! UpdateAxes())
         return false ;
   }

  // Cerco prossima posizione su interpolazione corrente
   CamData* pCamData = dynamic_cast<CamData*>( m_pGeomDB->GetUserObj( m_nEntId)) ;
   if ( pCamData == nullptr  ||  pCamData->GetAxesStatus() != CamData::AS_OK)
      return false ;
   const DBLVECTOR& AxesEnd = pCamData->GetAxisVal() ;
  // Verifico se movimento in rapido
   bool bRapid = ( pCamData->GetFeed() < EPS_SMALL) ;
  // Calcolo distanza di movimento
   double dDist = 0 ;
   for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i)
      dDist += ( AxesEnd[i] - m_AxesVal[i]) * ( AxesEnd[i] - m_AxesVal[i]) ;
   dDist = sqrt( dDist) ;
   m_dCoeff += ( bRapid ? 4 : 1) * m_dStep / dDist ;
   if ( m_dCoeff > 1)
      m_dCoeff = 1 ;
  // Eseguo movimento
   for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) {
      double dVal = m_AxesVal[i] * ( 1 - m_dCoeff) + AxesEnd[i] * m_dCoeff ;
      m_pMchMgr->SetAxisPos( m_AxesName[i], dVal) ;
   }

  // Se arrivato a fine interpolazione movimento, salvo posizioni
   if ( m_dCoeff > 0.999)
      m_AxesVal = AxesEnd ;

   return true ;
}

//----------------------------------------------------------------------------
bool
Simulator::GetAxisInfoPos( int nI, string& sName, double& dVal)
{
  // Verifiche
   if ( m_pMchMgr == nullptr  ||  m_pGeomDB == nullptr)
      return false ;
  // recupero i dati dell'asse
   if ( nI < 0  ||  nI >= int( m_AxesName.size()))
      return false ;
   sName = m_AxesName[nI] ;
   return m_pMchMgr->GetAxisPos( sName, dVal) ;
}

//----------------------------------------------------------------------------
bool
Simulator::SetStep( double dStep)
{
  const double MIN_STEP = 0.1 ;
  const double MAX_STEP = 100.0 ;
  if ( dStep < MIN_STEP)
     m_dStep = MIN_STEP ;
  else if ( dStep > MAX_STEP)
     m_dStep = MAX_STEP ;
  else
     m_dStep = dStep ;
  return true ;
}

//----------------------------------------------------------------------------
bool
Simulator::Stop( void)
{
   m_nOpId = GDB_ID_NULL ;
   m_nCLPathId = GDB_ID_NULL ;
   m_nEntId = GDB_ID_NULL ;
   m_dCoeff = 0 ;
   return true ;
}

//----------------------------------------------------------------------------
bool
Simulator::UpdateTool( void)
{
  // Recupero l'utensile della lavorazione corrente
   string sTool ;
   Machining* pMch = dynamic_cast<Machining*>( m_pGeomDB->GetUserObj( m_nOpId)) ;
   if ( pMch == nullptr  ||  ! pMch->GetParam( MPA_TOOL, sTool))
      return false ;
   if ( ! m_pMchMgr->TdbSetCurrTool( sTool))
      return false ;
  // se cambiato, attivo l'utensile della lavorazione
   if ( sTool != m_sTool) {
      string sHead ; m_pMchMgr->TdbGetCurrToolParam( TPA_HEAD, sHead) ;
      int nExit ; m_pMchMgr->TdbGetCurrToolParam( TPA_EXIT, nExit) ;
      if ( ! m_pMchMgr->SetCalcTool( sTool, sHead, nExit))
         return false ;
      m_sTool = sTool ;
   }
   return true ;
}

//----------------------------------------------------------------------------
bool
Simulator::UpdateAxes( void)
{
  // Macchina corrente
   Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
   if ( pMachine == nullptr)
      return false ;
  // Carico i nomi degli assi macchina attivi
   return pMachine->GetAllCurrAxesName( m_AxesName) ;
}

//----------------------------------------------------------------------------
bool
Simulator::GoHome( void)
{
  // porto la macchina in home
   m_pMchMgr->ResetAllAxesPos() ;
  // assegno valori home degli assi macchina attivi
   return m_pMchMgr->GetAllCalcAxesHomePos( m_AxesVal) ;
}