//----------------------------------------------------------------------------
//  EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : MachineCalc.cpp       Data : 12.05.15          Versione : 1.6e3
// Contenuto : Implementazione gestione macchina : funzioni di calcolo.
//
//
//
// Modifiche : 12.05.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "MachMgr.h"
#include "GeoCalc.h"
#include "DllMain.h"
#include "Axis.h"
#include "Head.h"
#include "Exit.h"
#include "/EgtDev/Include/EGkGeoVector3d.h"
#include "/EgtDev/Include/EMkToolConst.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include "/EgtDev/Include/EGnFileUtils.h"

using namespace std ;

//----------------------------------------------------------------------------
bool
Machine::SetCurrTable( const string& sTable)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return false ;
  // recupero il gruppo della tavola
   m_nCalcTabId = GetGroup( sTable) ;
   if ( m_nCalcTabId == GDB_ID_NULL  ||  ! IsTableGroup( m_nCalcTabId)) {
      m_nCalcTabId = GDB_ID_NULL ;
      return false ;
   }
   return true ;
}

//----------------------------------------------------------------------------
int
Machine::GetCurrTable( void)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return GDB_ID_NULL ;
  // recupero identificativo della tavola corrente
   return m_nCalcTabId ;
}

//----------------------------------------------------------------------------
bool
Machine::GetCurrTable( string& sTable)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return false ;
  // recupero nome della tavola corrente
   return m_pGeomDB->GetName( m_nCalcTabId, sTable) ;
}

//----------------------------------------------------------------------------
bool
Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return false ;
  // azzero tutto
   m_nCalcHeadId = GDB_ID_NULL ;
   m_nCalcExitId = GDB_ID_NULL ;
   m_nCalcToolId = GDB_ID_NULL ;
   m_dCalcTLen = 0 ;
  // recupero il gruppo della testa
   int nHeadId = GetGroup( sHead) ;
  // recupero i dati della testa
   Head* pHead = GetHead( nHeadId) ;
   if ( pHead == nullptr)
      return false ;
  // recupero il gruppo dell'uscita
   string sExit = MCH_EXIT + ToString( nExit) ;
   int nExitId = m_pGeomDB->GetFirstNameInGroup( nHeadId, sExit) ;
  // recupero i dati dell'uscita
   Exit* pExit = GetExit( nExitId) ;
   if ( pExit == nullptr)
      return false ;
  // recupero i dati dell'utensile
   if ( ! LoadTool( sHead, nExit, sTool))
      return false ;
   int nToolId = m_pGeomDB->GetFirstNameInGroup( nExitId, sTool) ;
   if ( nToolId == GDB_ID_NULL  ||  m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP)
      return false ;
   double dTLen = 0 ;
   if ( ! m_pMchMgr->TdbSetCurrTool( sTool)  ||  ! m_pMchMgr->TdbGetCurrToolParam( TPA_LEN, dTLen))
      return false ;
  // assegno tutti i dati
   m_nCalcHeadId = nHeadId ;
   m_nCalcExitId = nExitId ;
   m_nCalcToolId = nToolId ;
   m_dCalcRot1W = pHead->GetRot1W() ;
   m_ptCalcPos = pExit->GetPos() ;
   m_vtCalcDir = pExit->GetTDir() ;
   m_vtCalcADir = pHead->GetADir() ;
   m_dCalcTLen = dTLen ;
  // determino la catena cinematica
   return CalculateKinematicChain() ;
}

//----------------------------------------------------------------------------
int
Machine::GetCurrTool( void)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return GDB_ID_NULL ;
  // recupero identificativo dell'utensile
   return m_nCalcToolId ;
}

//----------------------------------------------------------------------------
bool
Machine::GetCurrTool( string& sTool)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return false ;
  // recupero nome gruppo dell'utensile
   return m_pGeomDB->GetName( m_nCalcToolId, sTool) ;
}

//----------------------------------------------------------------------------
double
Machine::GetCurrRot1W( void)
{
  // restituisco il peso del primo asse rotante nei confronti di movimento
   return m_dCalcRot1W ;
}

//----------------------------------------------------------------------------
bool
Machine::CalculateKinematicChain( void)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return false ;
  // azzero tutti gli assi della catena cinematica
   m_vCalcLinAx.clear() ;
   m_vCalcRotAx.clear() ;
  // recupero gli assi di tavola
   if ( m_nCalcTabId == GDB_ID_NULL)
      return false ;
   int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ;
   if ( nTParId == GDB_ID_NULL)
      return false ;
   while ( IsAxisGroup( nTParId)) {
      if ( ! AddKinematicAxis( false, nTParId))
         return false ;
      nTParId = m_pGeomDB->GetParentId( nTParId) ;
   }
  // recupero gli assi di testa
   if ( m_nCalcHeadId == GDB_ID_NULL)
      return false ;
   int nHParId = m_pGeomDB->GetParentId( m_nCalcHeadId) ;
   if ( nHParId == GDB_ID_NULL)
      return false ;
   while ( IsAxisGroup( nHParId)) {
      if ( ! AddKinematicAxis( true, nHParId))
         return false ;
      nHParId = m_pGeomDB->GetParentId( nHParId) ;
   }
  // verifiche sugli assi lineari :
   // devono essere 3
   if ( m_vCalcLinAx.size() != 3)
      return false ;
   // devono essere ordinabili come XYZ
   if ( ! m_vCalcLinAx[0].vtDir.IsXplus()) {
      if ( m_vCalcLinAx[1].vtDir.IsXplus())
         swap( m_vCalcLinAx[0], m_vCalcLinAx[1]) ;
      else if ( m_vCalcLinAx[2].vtDir.IsXplus())
         swap( m_vCalcLinAx[0], m_vCalcLinAx[2]) ;
      else
         return false ;
   }
   if ( ! m_vCalcLinAx[1].vtDir.IsYplus()) {
      if ( m_vCalcLinAx[2].vtDir.IsYplus())
         swap( m_vCalcLinAx[1], m_vCalcLinAx[2]) ;
      else
         return false ;
   }
  // verifiche sugli assi rotanti :
   // se 0 o 1 va bene
   if ( m_vCalcRotAx.size() <= 1)
      return true ;
   // se 2 va bene
   if ( m_vCalcRotAx.size() == 2) {
     // se entrambi di testa devo invertirne l'ordine
      if ( m_vCalcRotAx[0].bHead  &&  m_vCalcRotAx[1].bHead)
         swap( m_vCalcRotAx[0], m_vCalcRotAx[1]) ;
      return true ;
   }
   // altrimenti non ancora gestito, quindi errore
   return false ;
}

//----------------------------------------------------------------------------
bool
Machine::AddKinematicAxis( bool bOnHead, int nId)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return false ;
  // recupero il gestore dell'asse
   Axis* pAx = GetAxis( nId) ;
   if ( pAx == nullptr)
      return false ;
  // ne recupero i dati
   KinAxis kAx ;
   kAx.nGrpId = nId ;
   kAx.bLinear = ( pAx->GetType() != MCH_AT_ROTARY) ;
   kAx.bHead = bOnHead ;  // posizione su catena cinematica
   kAx.ptPos = pAx->GetPos() ;
   kAx.vtDir = pAx->GetDir() ;
   kAx.stroke = pAx->GetStroke() ;
  // se lineare di tavola, devo invertirlo
   if ( kAx.bLinear  &&  ! kAx.bHead)
      kAx.vtDir.Invert() ;
  // lo inserisco nella opportuna lista degli assi
   if ( kAx.bLinear)
      m_vCalcLinAx.emplace_back( kAx) ;
   else
      m_vCalcRotAx.emplace_back( kAx) ;
   return true ;
}

//----------------------------------------------------------------------------
string
Machine::GetKinematicAxis( int nInd)
{
  // controllo GeomDB
   if ( m_pGeomDB == nullptr)
      return "" ;
  // controllo indice
   int nLinAxTot = int( m_vCalcLinAx.size()) ;
   int nRotAxTot = int( m_vCalcRotAx.size()) ;
   if ( nInd < 0  ||  nInd >= nLinAxTot + nRotAxTot)
      return "" ;
  // recupero nome
   string sName ;
   if ( nInd < nLinAxTot)
      m_pGeomDB->GetName( m_vCalcLinAx[nInd].nGrpId, sName) ;
   else
      m_pGeomDB->GetName( m_vCalcRotAx[nInd-nLinAxTot].nGrpId, sName) ;
   return sName ;
}

//----------------------------------------------------------------------------
bool
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
                    int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2)
{
  // annullo tutti gli angoli
   nStat = 0 ; dAngA1 = 0 ; dAngB1 = 0 ; dAngA2 = 0 ; dAngB2 = 0 ;
  // se nessun asse rotante, non c'è alcunchè da calcolare
   if ( m_vCalcRotAx.size() == 0) {
      nStat = 1 ;
      return true ;
   }
  // direzione fresa normalizzata
   Vector3d vtDirTn = vtDirT ;
   if ( ! vtDirTn.Normalize())
      return false ;
  // direzione ausiliaria normalizzata
   Vector3d vtDirAn = vtDirA ;
   vtDirAn.Normalize() ;
  // direzione fresa su testa a riposo
   Vector3d vtDirH = m_vtCalcDir ;
  // direzione ausiliaria su testa a riposo
   Vector3d vtDirI = m_vtCalcADir ;
  // direzione primo asse rotante
   Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ;
  // se asse di tavola, ne inverto la direzione
   if ( ! m_vCalcRotAx[0].bHead)
      vtAx1.Invert() ;
  // componente versore fresa desiderato su direzione primo asse rotante
   double dCompTSuAxR1 = vtDirTn * vtAx1 ; 
  // se c'è secondo asse rotante, si calcola angolo per avere il componente appena calcolato
   bool bDet = true ;
   Vector3d vtDirH1, vtDirH2 ;
   Vector3d vtDirI1, vtDirI2 ;
   if ( m_vCalcRotAx.size() == 2) {
     // direzione secondo asse rotante
      Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
     // se asse di tavola, ne inverto la direzione
      if ( ! m_vCalcRotAx[1].bHead)
         vtAx2.Invert() ;
     // calcolo secondo angolo di rotazione
      nStat = GetRotationComponent( vtDirH, dCompTSuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ;
     // aggiornamento direzione fresa su testa
      if ( nStat >= 1) {
        // se indeterminato lo azzero
         if ( ! bDet)
            dAngB1 = 0 ;
        // eseguo aggiornamento    
         vtDirH1 = vtDirH ;
         vtDirH1.Rotate( vtAx2, dAngB1) ;
         vtDirI1 = vtDirI ;
         vtDirI1.Rotate( vtAx2, dAngB1) ;
      }
      if ( nStat == 2) {
         vtDirH2 = vtDirH ;
         vtDirH2.Rotate( vtAx2, dAngB2) ;
         vtDirI2 = vtDirI ;
         vtDirI2.Rotate( vtAx2, dAngB2) ;
      }
   }
  // altrimenti verifico se compatibili
   else {
     // componente versore utensile su direzione primo asse
      double dCompHSuAxR1 = vtDirH * vtAx1 ; 
     // componenti versori fresa e utensile perpendicolari direzione primo asse
      double dTemp = 1 - dCompTSuAxR1 * dCompTSuAxR1 ;
      double dCompTOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ;
      dTemp = 1 - dCompHSuAxR1 * dCompHSuAxR1 ;
      double dCompHOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ;
      // verifica ( max delta angolare < 0.002 deg)
      const double SIN_ANG_ERROR = sin( 0.002 * DEGTORAD) ;
      if ( fabs( dCompTOrtAxR1 * dCompHSuAxR1 - dCompHOrtAxR1 * dCompTSuAxR1) < SIN_ANG_ERROR) {
         nStat = 1 ;
         vtDirH1 = vtDirH ;
        // reset secondo angolo
         dAngB1 = 0 ;
      }
   }

  // calcolo primo angolo di rotazione per seconda soluzione
   bool bDet2 = true ;
   if ( nStat == 2) {
      if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, dAngA2, bDet2) )
         nStat = 1 ;
      else {
        // se indeterminato, provo a determinarlo con la direzione ausiliaria
         if ( ! bDet2) {
            bool bDetX ;
            vtDirI2.GetRotation( vtDirAn, vtAx1, dAngA2, bDetX) ;
         }
      }
   }

  // calcolo primo angolo di rotazione per prima soluzione
   bool bDet1 = true ;
   if ( nStat >= 1) {
      if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, dAngA1, bDet1) )
         nStat = 0 ;
      else {
        // se indeterminato, provo a determinarlo con la direzione ausiliaria
         if ( ! bDet1) {
            bool bDetX ;
            vtDirI1.GetRotation( vtDirAn, vtAx1, dAngA1, bDetX) ;
         }
      }
   }

  // verifiche dei limiti di corsa
   if ( nStat >= 2) {
     // se non riesco ad aggiustare, elimino
      if ( ! AdjustAngleInStroke( 1, dAngA2)  ||  ! AdjustAngleInStroke( 2, dAngB2))
         -- nStat ;
   }
   if ( nStat >= 1) {
     // se non riesco ad aggiustare, elimino
      if ( ! AdjustAngleInStroke( 1, dAngA1)  ||  ! AdjustAngleInStroke( 2, dAngB1)) {
         -- nStat ;
        // riloco eventuale soluzione rimasta
         if ( nStat >= 1) {
            dAngA1 = dAngA2 ;
            dAngB1 = dAngB2 ;
         }
      }
   }

  // modifico stato per angolo indeterminato
   if ( ( nStat >= 2  &&  ! bDet2)  ||   ( nStat >= 1  &&  ! bDet1))
      nStat = - nStat ;

   return true ;
}

//----------------------------------------------------------------------------
bool
Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
                       int& nStat, double& dX, double& dY, double& dZ)
{
  // la posizione deve essere espressa rispetto allo ZERO MACCHINA
  // per ora gestisco solo gli assi di testa

  // posizione e direzione fresa su testa a riposo
   Point3d  ptPosH = m_ptCalcPos ;
   Vector3d vtDirH = m_vtCalcDir ;

  // se c'è secondo asse rotante di testa
   if ( m_vCalcRotAx.size() >= 2  &&  m_vCalcRotAx[1].bHead) {
     // posizione e direzione primo asse rotante
      Point3d  ptAx2 = m_vCalcRotAx[1].ptPos ;
      Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
     // ruoto dati a riposo
      ptPosH.Rotate( ptAx2, vtAx2, dAngB) ;
      vtDirH.Rotate( vtAx2, dAngB) ;
   }

  // se c'è primo asse rotante di testa
   if ( m_vCalcRotAx.size() >= 1  &&  m_vCalcRotAx[0].bHead) {
     // posizione e direzione primo asse rotante
      Point3d  ptAx1 = m_vCalcRotAx[0].ptPos ;
      Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ;
     // ruoto dati a riposo
      ptPosH.Rotate( ptAx1, vtAx1, dAngA) ;
      vtDirH.Rotate( vtAx1, dAngA) ;
   }

  // assegno l'offset testa
   Vector3d vtDtHe = ORIG - m_ptCalcPos ;

  // calcolo il recupero degli assi : è l'opposto dello spostamento della posizione
   Vector3d vtDtAx = m_ptCalcPos - ptPosH ;
   
  // calcolo il recupero di lunghezza utensile
   Vector3d vtDtTL = vtDirH * m_dCalcTLen ;
   
  // calcolo le posizioni degli assi lineari
   dX = ptP.x + vtDtHe.x + vtDtAx.x + vtDtTL.x ;
   dY = ptP.y + vtDtHe.y + vtDtAx.y + vtDtTL.y ;
   dZ = ptP.z + vtDtHe.z + vtDtAx.z + vtDtTL.z ; 

   nStat = 0 ;
   return true ;
}

//----------------------------------------------------------------------------
bool
Machine::AdjustAngleInStroke( int nId, double& dAng)
{
  // se non ci sono assi rotanti, non c'è alcunchè da fare
   if ( m_vCalcRotAx.size() == 0)
      return true ;
  // se primo angolo ed esiste primo asse rotante
   if ( nId == 1  &&  m_vCalcRotAx.size() >= 1) {
      while ( dAng < m_vCalcRotAx[0].stroke.Min)
         dAng += ANG_FULL ;
      while ( dAng > m_vCalcRotAx[0].stroke.Max)
         dAng -= ANG_FULL ;
      return ( dAng >= m_vCalcRotAx[0].stroke.Min   &&
               dAng <= m_vCalcRotAx[0].stroke.Max) ;
   }
  // se secondo angolo ed esiste secondo asse rotante
   if ( nId == 2  &&  m_vCalcRotAx.size() >= 2) {
      while ( dAng < m_vCalcRotAx[1].stroke.Min)
         dAng += ANG_FULL ;
      while ( dAng > m_vCalcRotAx[1].stroke.Max)
         dAng -= ANG_FULL ;
      return ( dAng >= m_vCalcRotAx[1].stroke.Min   &&
               dAng <= m_vCalcRotAx[1].stroke.Max) ;
   }
  // errore
   return false ;
}

//----------------------------------------------------------------------------
bool
Machine::VerifyOutOfStroke( double dX, double dY, double dZ, double dAngA, double dAngB, int& nStat)
{
  // default tutto ok
   nStat = 0 ;
  // primo lineare
   if ( m_vCalcLinAx.size() >= 1) {
      if ( dX < m_vCalcLinAx[0].stroke.Min)
         nStat += 1 ;
      else if( dX > m_vCalcLinAx[0].stroke.Max)
         nStat += 2 ;
   }
  // secondo lineare
   if ( m_vCalcLinAx.size() >= 2) {
      if ( dY < m_vCalcLinAx[1].stroke.Min)
         nStat += 4 ;
      else if( dY > m_vCalcLinAx[1].stroke.Max)
         nStat += 8 ;
   }
  // terzo lineare
   if ( m_vCalcLinAx.size() >= 3) {
      if ( dZ < m_vCalcLinAx[2].stroke.Min)
         nStat += 16 ;
      else if( dZ > m_vCalcLinAx[2].stroke.Max)
         nStat += 32 ;
   }
  // eventuale primo rotante
   if ( m_vCalcRotAx.size() >= 1) {
      if ( dAngA < m_vCalcRotAx[0].stroke.Min)
         nStat += 64 ;
      else if( dAngA > m_vCalcRotAx[0].stroke.Max)
         nStat += 128 ;
   }
  // eventuale secondo rotante
   if ( m_vCalcRotAx.size() >= 2) {
      if ( dAngB < m_vCalcRotAx[1].stroke.Min)
         nStat += 256 ;
      else if( dAngB > m_vCalcRotAx[1].stroke.Max)
         nStat += 512 ;
   }
   return true ;
}

//----------------------------------------------------------------------------
int
Machine::GetCurrLinAxes( void)
{
   return int( m_vCalcLinAx.size()) ;
}

//----------------------------------------------------------------------------
int
Machine::GetCurrRotAxes( void)
{
   return int( m_vCalcRotAx.size()) ;
}

//----------------------------------------------------------------------------
bool
Machine::GetAllCurrAxesName( STRVECTOR& vAxName)
{
   vAxName.clear() ;
   bool bOk = true ;
  // ciclo sugi assi lineari correnti
   for ( auto& CalcLinAx : m_vCalcLinAx) {
      Axis* pAx = GetAxis( CalcLinAx.nGrpId) ;
      if ( pAx != nullptr)
         vAxName.emplace_back( pAx->GetName()) ;
      else
         bOk = false ;
   }
  // ciclo sugi assi rotanti correnti
   for ( auto& CalcRotAx : m_vCalcRotAx) {
      Axis* pAx = GetAxis( CalcRotAx.nGrpId) ;
      if ( pAx != nullptr)
         vAxName.emplace_back( pAx->GetName()) ;
      else
         bOk = false ;
   }
   return bOk ;
}

//----------------------------------------------------------------------------
bool
Machine::GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal)
{
   vAxHomeVal.clear() ;
   bool bOk = true ;
  // ciclo sugi assi lineari correnti
   for ( auto& CalcLinAx : m_vCalcLinAx) {
      Axis* pAx = GetAxis( CalcLinAx.nGrpId) ;
      if ( pAx != nullptr)
         vAxHomeVal.emplace_back( pAx->GetHomeVal()) ;
      else
         bOk = false ;
   }
  // ciclo sugi assi rotanti correnti
   for ( auto& CalcRotAx : m_vCalcRotAx) {
      Axis* pAx = GetAxis( CalcRotAx.nGrpId) ;
      if ( pAx != nullptr)
         vAxHomeVal.emplace_back( pAx->GetHomeVal()) ;
      else
         bOk = false ;
   }
   return bOk ;
}