//----------------------------------------------------------------------------
//  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 "/EgtDev/Include/EGkGeoVector3d.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include "/EgtDev/Include/EGnFileUtils.h"

using namespace std ;

//----------------------------------------------------------------------------
bool
Machine::SetCurrTable( const string& sTable)
{
  // 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 ;
}

//----------------------------------------------------------------------------
bool
Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
{
  // 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) ;
   if ( nHeadId == GDB_ID_NULL  ||  ! IsHeadGroup( nHeadId))
      return false ;
  // recupero il gruppo dell'uscita
   string sExit = "T" + ToString( nExit) ;
   int nExitId = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nHeadId, sExit) ;
   if ( nExitId == GDB_ID_NULL  ||  m_pMchMgr->m_pGeomDB->GetGdbType( nExitId) != GDB_TY_GROUP)
      return false ;
  // recupero posizione e direzione a riposo
   Point3d ptPos ;
   if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nExitId, "Pos", ptPos))
      return false ;
   Vector3d vtDir ;
   if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nExitId, "TDir", vtDir))
      return false ;
  // recupero i dati dell'utensile
   if ( ! LoadTool( sHead, nExit, sTool))
      return false ;
   int nToolId = m_pMchMgr->m_pGeomDB->GetFirstNameInGroup( nExitId, sTool) ;
   if ( nToolId == GDB_ID_NULL  ||  m_pMchMgr->m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP)
      return false ;
   double dTLen ;
   if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nToolId, "L", dTLen))
      return false ;
  // assegno tutti i dati
   m_nCalcHeadId = nHeadId ;
   m_nCalcExitId = nExitId ;
   m_nCalcToolId = nToolId ;
   m_ptCalcPos = ptPos ;
   m_vtCalcDir = vtDir ;
   m_dCalcTLen = dTLen ;
  // determino la catena cinematica
   return CalculateKinematicChain() ;
}

//----------------------------------------------------------------------------
bool
Machine::CalculateKinematicChain( void)
{
  // 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_pMchMgr->m_pGeomDB->GetParentId( m_nCalcTabId) ;
   if ( nTParId == GDB_ID_NULL)
      return false ;
   while ( IsAxisGroup( nTParId)) {
      if ( ! AddKinematicAxis( false, nTParId))
         return false ;
      nTParId = m_pMchMgr->m_pGeomDB->GetParentId( nTParId) ;
   }
  // recupero gli assi di testa
   if ( m_nCalcHeadId == GDB_ID_NULL)
      return false ;
   int nHParId = m_pMchMgr->m_pGeomDB->GetParentId( m_nCalcHeadId) ;
   if ( nHParId == GDB_ID_NULL)
      return false ;
   while ( IsAxisGroup( nHParId)) {
      if ( ! AddKinematicAxis( true, nHParId))
         return false ;
      nHParId = m_pMchMgr->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)
{
   KinAxis kAx ;
  // assegno id
   kAx.nGrpId = nId ;
  // recupero il tipo di asse
   string sType ;
   if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Type", sType))
      return false ;
   kAx.bLinear = ( sType != MCH_AXIS + ToString( MCH_AT_ROTARY)) ;
  // assegno posizione su catena cinematica
   kAx.bHead = bOnHead ;
  // recupero la posizione
   if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Pos", kAx.ptPos))
      return false ;
  // recupero la direzione e la normalizzo
   if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Dir", kAx.vtDir)  ||  ! kAx.vtDir.Normalize())
      return false ;
  // recupero i limiti di corsa
   string sStroke ;
   if ( ! m_pMchMgr->m_pGeomDB->GetInfo( nId, "Stroke", sStroke) || ! FromString( sStroke, kAx.stroke.v))
      return false ;
  // 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 ;
}

//----------------------------------------------------------------------------
bool
Machine::GetAngles( const Vector3d& vtDirT,
                    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 fresa su testa a riposo
   Vector3d vtDirH = m_vtCalcDir ;
  // 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 ;
   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) ;
      }
      if ( nStat == 2) {
         vtDirH2 = vtDirH ;
         vtDirH2.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
   if ( nStat == 2) {
      bool bDet ;
      if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, dAngA2, bDet) )
         nStat = 1 ;
      else {
        // se indeterminato ...
         if ( ! bDet) {
            nStat = - 2 ;
            dAngA2 = 0 ;
         }
      }
   }

  // calcolo primo angolo di rotazione per prima soluzione
   if ( nStat >= 1) {
      bool bDet ;
      if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, dAngA1, bDet) )
         nStat = 0 ;
      else {
        // se indeterminato ...
         if ( ! bDet) {
            nStat = - nStat ;
            dAngA1 = 0 ;
         }
      }
   }

   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) ;
   }

  // 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 + vtDtAx.x + vtDtTL.x ;
   dY = ptP.y + vtDtAx.y + vtDtTL.y ;
   dZ = ptP.z + vtDtAx.z + vtDtTL.z ; 

   return true ;
}

//----------------------------------------------------------------------------
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 ;
}