-- CalcToolPath.lua by Egaltech s.r.l. 2022/04/19
-- Calcolo percorsi di lavoro per Stampa 3d

-- Tabella per definizione modulo
local CalcToolPath = {}

-- Intestazioni
require( 'EgtBase')

EgtOutLog( ' CalcToolPath started', 1)

-- Dati
local AMD = require( 'AddManData')

---------------------------------------------------------------------
local function GetLayerParamsForToolPathCalc()
   local nParamsGrp = EgtGetFirstNameInGroup( GDB_ID.ROOT, PARAMS_GRP)
   local LayerParams = {}
   LayerParams.dStrand = EgtGetInfo( nParamsGrp, KEY_STRAND, 'd')
   LayerParams.dLayHeight = EgtGetInfo( nParamsGrp, KEY_SLICE_STEP, 'd')
   LayerParams.dOffs = EgtGetInfo( nParamsGrp, KEY_OFFSET_SLICE, 'd')
   LayerParams.bInvert = ( EgtGetInfo( nParamsGrp, KEY_PRINT_DIRECTION, 'i') == PRINT_DIRECTION.CW)
   LayerParams.nOrder = EgtGetInfo( nParamsGrp, KEY_PRINT_ORDER, 'i')
   LayerParams.nLinkType = EgtGetInfo( nParamsGrp, KEY_LINK_TYPE, 'i')
   LayerParams.dLinkParam = EgtGetInfo( nParamsGrp, KEY_LINK_PARAM, 'd')
   LayerParams.nLeadInType = EgtGetInfo( nParamsGrp, KEY_LEAD_IN_TYPE, 'i')
   LayerParams.dLeadInTangDist = EgtGetInfo( nParamsGrp, KEY_LEAD_IN_TANG_DIST, 'd')
   LayerParams.dLeadInOrthoDist = EgtGetInfo( nParamsGrp, KEY_LEAD_IN_ORTHO_DIST, 'd')
   LayerParams.nLeadOutType = EgtGetInfo( nParamsGrp, KEY_LEAD_OUT_TYPE, 'i')
   LayerParams.dLeadOutTangDist = EgtGetInfo( nParamsGrp, KEY_LEAD_OUT_TANG_DIST, 'd')
   LayerParams.dLeadOutOrthoDist = EgtGetInfo( nParamsGrp, KEY_LEAD_OUT_ORTHO_DIST, 'd')
   LayerParams.dOffsetLP = EgtGetInfo( nParamsGrp, KEY_OFFSET_LEAD_POINT, 'd')   
   LayerParams.dSPOffs = EgtGetInfo( nParamsGrp, KEY_SP_OFFSET_ON_SLICE, 'd')
   LayerParams.dCoastingLen = EgtGetInfo( nParamsGrp, KEY_COASTING_LEN, 'd')
   LayerParams.dWipeLen = EgtGetInfo( nParamsGrp, KEY_WIPE_LEN, 'd')
   LayerParams.vtSlicing = EgtGetInfo( nParamsGrp, KEY_SLICING_DIR, 'v')
   LayerParams.dTDiam = EgtGetInfo( nParamsGrp, KEY_TOOL_DIAM, 'd')

   return LayerParams 
end

--------------------------------------------------------------------
local function ComputeZCorrection( LayerParams)
   local _, dAng, _ = SphericalFromVector( LayerParams.vtSlicing)
   -- correzione per l'offset
   local dCorr1 = LayerParams.dOffs * sin( dAng)
   -- eventuale correzione per il diametro ugello
   local dCorr2 = max( LayerParams.dTDiam * 0.5 * sin( dAng) - LayerParams.dLayHeight * cos( dAng) - LayerParams.dStrand * 0.5 * sin( dAng) + 10 * GEO.EPS_SMALL, 0.0)
   return dCorr1 + dCorr2 
end

--------------------------------------------------------------------
local function AddLeadIn( nCrvId, LayerParams, nGrpId)
 
   local ptE = EgtSP( nCrvId)
   local vtTang = EgtSV( nCrvId)   
   local vtOrtho = Vector3d( vtTang)
   local dAng = 90
   if ( LayerParams.bInvert and LayerParams.nOrder == PRINT_ORDER.INF_INT_EXT) or 
      ( not LayerParams.bInvert and LayerParams.nOrder == PRINT_ORDER.EXT_INT_INF) then
      dAng = - 90
   end

   vtOrtho:rotate( LayerParams.vtSlicing, dAng)
   local ptS = ptE - LayerParams.dLeadInTangDist * vtTang + LayerParams.dLeadInOrthoDist * vtOrtho

   local nLeadInCrv = GDB_ID.NULL
   if LayerParams.nLeadInType == LEAD_TYPE.LINEAR then
      nLeadInCrv = EgtCurveCompoFromPoints( nGrpId, {ptS, ptE})
   elseif LayerParams.nLeadInType == LEAD_TYPE.ARC then
      nLeadInCrv = EgtArc2PV( nGrpId, ptE, ptS, -vtTang)
      EgtInvertCurve( nLeadInCrv)
   end      
   EgtRelocate( nLeadInCrv, nGrpId, GDB_IN.FIRST_SON)   
   EgtModifyCurveExtrusion( nLeadInCrv, LayerParams.vtSlicing, GDB_RT.GLOB)
   EgtSetInfo( nLeadInCrv, KEY_TYPE, TYPE.LINK)
end

--------------------------------------------------------------------
local function AddLeadOut( nCrvId, LayerParams, nGrpId)

   local ptS = EgtEP( nCrvId)
   local vtTang = EgtEV( nCrvId)
   local vtOrtho = Vector3d( vtTang)
   local dAng = - 90
   if ( LayerParams.bInvert and LayerParams.nOrder == PRINT_ORDER.INF_INT_EXT) or
      ( not LayerParams.bInvert and LayerParams.nOrder == PRINT_ORDER.EXT_INT_INF) then
      dAng = 90
   end
   
   vtOrtho:rotate( LayerParams.vtSlicing, dAng)
   local ptE = ptS + LayerParams.dLeadOutTangDist * vtTang + LayerParams.dLeadOutOrthoDist * vtOrtho
   
   local nLeadOutCrv = GDB_ID.NULL
   if LayerParams.nLeadOutType == LEAD_TYPE.LINEAR then
      nLeadOutCrv = EgtCurveCompoFromPoints( nGrpId, {ptS, ptE})  
   elseif LayerParams.nLeadOutType == LEAD_TYPE.ARC then
      nLeadOutCrv = EgtArc2PV( nGrpId, ptS, ptE, vtTang)      
   end
   EgtRelocate( nLeadOutCrv, nGrpId, GDB_IN.LAST_SON)   
   EgtModifyCurveExtrusion( nLeadOutCrv, LayerParams.vtSlicing, GDB_RT.GLOB)
   EgtSetInfo( nLeadOutCrv, KEY_TYPE, TYPE.LINK)
end

--------------------------------------------------------------------
local function AddRetraction( nCrvId, dCoastingLen, dWipeLen)
   
   local nCoastingId
   if dCoastingLen > GEO.EPS_SMALL then
      nCoastingId = EgtCopyGlob( nCrvId, nCrvId, GDB_IN.AFTER)
      local dPar = EgtCurveParamAtLength( nCoastingId, EgtCurveLength( nCoastingId) - dCoastingLen)
      if not dPar then 
         EgtErase( nCoastingId)
         return
      end      
      EgtTrimCurveStartAtParam( nCoastingId, dPar)      
      -- aggiorno la curva originale 
      if dPar > GEO.EPS_SMALL then
         EgtTrimCurveEndAtParam( nCrvId, dPar)
      else
         EgtErase( nCrvId)
      end
      EgtSetName( nCoastingId, COASTING_CRV)
      EgtSetInfo( nCoastingId, KEY_TYPE, TYPE.COASTING)
      EgtSetColor( nCoastingId, EgtStdColor('ORANGE'))
   end

   if dWipeLen > GEO.EPS_SMALL then
      local nWipeId = EgtCopyGlob( nCrvId, EgtIf( nCoastingId, nCoastingId, nCrvId), GDB_IN.AFTER)
      EgtTrimCurveEndAtLen( nWipeId, dWipeLen) 
      EgtSetName( nWipeId, WIPE_CRV)
      EgtSetInfo( nWipeId, KEY_TYPE, TYPE.WIPE)
      EgtSetColor( nWipeId, EgtStdColor('TEAL'))
   end
   
   return nCoastingId
end

--------------------------------------------------------------------
local function AddRetractionOnLastCrv( nTpathGrpId, LayerParams)

   local nCrvId = EgtGetLastNameInGroup( nTpathGrpId, SHELL_CRV .. "*")              
   if LayerParams.nLeadOutType == LEAD_TYPE.NONE then
      AddRetraction( nCrvId, LayerParams.dCoastingLen, LayerParams.dWipeLen)
   else
      local nLeadOutId = EgtGetLastInGroup( nTpathGrpId)
      local dLen = EgtCurveLength( nLeadOutId)
      if dLen > LayerParams.dCoastingLen - 500 * GEO.EPS_SMALL then
         -- coinvolge solo la curva di lead out
         local dNewCoastingLen = EgtIf( abs( LayerParams.dCoastingLen - dLen) < 500 * GEO.EPS_SMALL, dLen, LayerParams.dCoastingLen)   -- verifico se interamente coinvolta 
         AddRetraction( nLeadOutId, dNewCoastingLen, 0.0)
      else
         -- coinvolge parte dell'ultima shell crv
         local dNewCoastingLen = LayerParams.dCoastingLen - dLen
         local nCoastingId = AddRetraction( nCrvId, dNewCoastingLen, 0.0)
         EgtAddCurveCompoCurve( nCoastingId, nLeadOutId)
      end
   end
end

---------------------------------------------------------------------
function CalcToolPath.Exec( nPartId)

   -- Verifico il pezzo
   if not nPartId then
      EgtOutBox( 'Error missing part', 'ToolPathCalc')
      return 
   end

   -- Recupero i layer da processare
   local vLayIds = EgtGetNameInGroup( nPartId, SLICE_LAYER.."*") 
   if not vLayIds then
      EgtOutBox( 'Error missing slices', 'ToolPathCalc')
      return 
   end

   -- recupero i parametri per calcolo dei toolpath
   local LayerParams = GetLayerParamsForToolPathCalc()
      
   -- Ciclo sui layer
   for nIdx = 1, #vLayIds do
      
      -- scorro tutti i gruppi di contorni
      local nCrvGrpId = EgtGetFirstNameInGroup( vLayIds[ nIdx], CONTOUR_GRP.."*")
      while nCrvGrpId do
         -- recupero il gruppo dei percorsi
         local nPathGrpId = EgtGetFirstNameInGroup( nCrvGrpId, PATH_GRP)
         if not nPathGrpId then
            EgtOutBox( 'Error missing paths', 'ToolPathCalc')
            return 
         else
            EgtSetStatus( nPathGrpId, GDB_ST.OFF)
         end         
         -- recupero il gruppo dei percorsi utensile
         local nTpathGrpId = EgtGetFirstNameInGroup( nCrvGrpId, TOOLPATH_GRP)
         if not nTpathGrpId then
            nTpathGrpId = EgtGroup( nCrvGrpId)
            EgtSetName( nTpathGrpId, TOOLPATH_GRP)
         else
            EgtEmptyGroup( nTpathGrpId)
         end
         -- creo il percorso di lavoro :
         -- copio le curve, le ordino, le oriento e le muovo in Z
         local nEntId = EgtGetFirstInGroup( nPathGrpId)
         while nEntId do
            local nNewEntId = EgtCopyGlob( nEntId, nTpathGrpId, EgtIf( LayerParams.nOrder == PRINT_ORDER.INF_INT_EXT, GDB_IN.FIRST_SON, GDB_IN.LAST_SON))
            -- correggo posizione in Z ( per essere sicuri di appoggiare sul piano)
            local dCorr = ComputeZCorrection( LayerParams)
            EgtMove( nNewEntId, dCorr * Z_AX(), GDB_RT.GLOB)
            -- mi sposto dell'altezza layer
            EgtMove( nNewEntId, LayerParams.dLayHeight * LayerParams.vtSlicing, GDB_RT.GLOB)
            if LayerParams.bInvert then
               EgtInvertCurve( nNewEntId)
            end   
            EgtModifyCurveExtrusion( nNewEntId, LayerParams.vtSlicing, GDB_RT.GLOB)
            EgtSetColor( nNewEntId, EgtStdColor('GRAY'))
            nEntId = EgtGetNext( nEntId)
         end

         -- aggiungo gli opportuni raccordi
         local nPrevId = EgtGetFirstInGroup( nTpathGrpId)
         local nCurrId = EgtGetNext( nPrevId or GDB_ID.NULL)
         k = 0
         local nPrevShellNbr
         if nPrevId then nPrevShellNbr = tonumber( EgtGetName( nPrevId):sub(6)) end
         while nCurrId do

            local nCurrShellNbr = tonumber( EgtGetName( nCurrId):sub(6))
            if nCurrShellNbr ~= nPrevShellNbr then k = k + 1 end

            -- se nessun raccordo modifico solo lo start point della curva
            if LayerParams.nLinkType == LINK_TYPE.NONE then
               local dLen = k * LayerParams.dSPOffs 
               if LayerParams.dSPOffs < 0 then
                  dLen = EgtCurveLength( nCurrId) + dLen
               end
               local dPar = EgtCurveParamAtLength( nCurrId, dLen)
               if dPar then EgtChangeClosedCurveStart( nCurrId, dPar) end
               
            else
               -- altrimenti verifico se ha senso creare raccordo
               if abs( dist( EgtEP( nPrevId), EgtSP( nCurrId))) <  LayerParams.dStrand + 10 then   
                  local nLinkId
                  if k > 1 then
                     -- modifico lo start point
                     local dLen = EgtCurveLength( nPrevId) - abs( LayerParams.dSPOffs)
                     EgtTrimCurveEndAtLen( nPrevId, dLen)
                     EgtChangeClosedCurveStartPoint( nCurrId, EgtEP( nPrevId))
                  end
                  -- aggiungo il raccordo
                  EgtTrimCurveEndAtLen( nPrevId, EgtCurveLength( nPrevId) - LayerParams.dLinkParam / 2)
                  EgtTrimCurveStartAtLen( nCurrId, LayerParams.dLinkParam / 2)
                  if LayerParams.nLinkType == LINK_TYPE.LINEAR then
                     nLinkId = EgtCurveCompoFromPoints( nTpathGrpId, {EgtEP( nPrevId), EgtSP( nCurrId)})
                  elseif LayerParams.nLinkType == LINK_TYPE.BIARC then
                     local frLoc = Frame3d( ORIG(), LayerParams.vtSlicing)
                     local nGrpTmp = EgtGroup( nTpathGrpId, frLoc)
                     local _, _, dAngIni = SphericalFromVector( EgtEV( nPrevId, nGrpTmp))
                     local _, _, dAngFin = SphericalFromVector( EgtSV( nCurrId, nGrpTmp))                    
                     nLinkId = EgtBiArc( nGrpTmp, EgtEP( nPrevId, nGrpTmp), EgtSP( nCurrId, nGrpTmp), dAngIni, dAngFin, 0.5)
                     EgtRelocateGlob( nLinkId, nTpathGrpId, GDB_IN.LAST_SON)
                     EgtErase( nGrpTmp)    
                  end

                  if nLinkId then
                     EgtRelocate( nLinkId, nCurrId, GDB_IN.BEFORE)                  
                     EgtModifyCurveExtrusion( nLinkId, LayerParams.vtSlicing, GDB_RT.GLOB)
                     EgtSetInfo( nLinkId, KEY_TYPE, TYPE.LINK)  
                  end
               end
            end
          
            nPrevShellNbr = nCurrShellNbr
            nPrevId = nCurrId    
            nCurrId = EgtGetNext( nCurrId)
         end
         
         -- aggiungo leadin/leadout
         local nFirstCurve = EgtGetFirstInGroup( nTpathGrpId)
         if nFirstCurve and LayerParams.nLeadInType ~= LEAD_TYPE.NONE then
            EgtTrimCurveStartAtLen( nFirstCurve, LayerParams.dOffsetLP)
            AddLeadIn( nFirstCurve, LayerParams, nTpathGrpId)
         end
         local nLastCurve = EgtGetLastInGroup( nTpathGrpId)
         if nLastCurve and LayerParams.nLeadOutType ~= LEAD_TYPE.NONE then
            EgtTrimCurveEndAtLen( nLastCurve, EgtCurveLength( nLastCurve) - LayerParams.dOffsetLP)
            AddLeadOut( nLastCurve, LayerParams, nTpathGrpId)
         end
       
         -- aggiungo coasting/wipe
         local nId = EgtGetFirstNameInGroup( nTpathGrpId, SHELL_CRV .. "*")
         local nFirstId = nId
         local nLastId = EgtGetLastNameInGroup( nTpathGrpId, SHELL_CRV .. "*")         
         while nId do            
            if nId == nLastId then
               AddRetractionOnLastCrv( nTpathGrpId, LayerParams)
            else
               local nNextId = EgtGetNext( nId)
               if EgtGetInfo( nNextId, KEY_TYPE, 'i') ~= TYPE.LINK then
                  -- aggiungo retrazione
                  if nId == nFirstId then
                     local dWipeLen = EgtIf( LayerParams.nLeadInType == LEAD_TYPE.NONE, LayerParams.dWipeLen, 0)
                     AddRetraction( nId, LayerParams.dCoastingLen, dWipeLen)
                  else
                     AddRetraction( nId, LayerParams.dCoastingLen, LayerParams.dWipeLen)
                  end
               end
            end
            nId = EgtGetNextName( nId, SHELL_CRV .. "*")
         end
         
         --passo al gruppo di contorni successivo
         nCrvGrpId = EgtGetNextName( nCrvGrpId, CONTOUR_GRP.."*")
      end
   end
end

---------------------------------------------------------------------
return CalcToolPath