-- PreCollisionLib.lua by Egalware s.r.l. 2025/11/24
-- Libreria stima collisioni per travi

-- Tabella per definizione modulo
local PreCollisionLib = {}

-- Include
require( 'EgtBase')

-- Carico i dati globali


EgtOutLog( ' PreCollisionLib started', 1)


-------------------------------------------------------------------------------------------------------------
-- costruzione flat region del pezzo nel piano scelto
local function GetPartSurfFrInPlane( Part, vtDirection)

   -- si estrae prima la silhouette nel piano
   local idFirstCurve, nCurveCount = EgtGetSurfTmSilhouette( Part.idBoxTm, vtDirection, 10 * GEO.EPS_SMALL, Part.idTempGroup, GDB_RT.GLOB)
   -- se più curve c'è qualcosa che non va: errore
   if nCurveCount > 1 then
      error( 'GetPartSurfFrInPlane : error in part box')
   end
   -- creazione flat region dalla silhouette
   local idPartSurfFr = EgtSurfFlatRegion( Part.idTempGroup, idFirstCurve)

   return idPartSurfFr
end

-------------------------------------------------------------------------------------------------------------
-- costruzione flat region del grezzo restante nel piano scelto (con leggera sovrapposizione)
local function GetRestlengthSurfFrInPlane( Part, vtDirection)
   
   -- si costruisce il box in globale
   local ptRestLengthMax = Point3d( Part.b3Part:getMin()) + X_AX() * 500 * GEO.EPS_SMALL
   local ptRestLengthMin = Point3d( Part.b3Part:getMin():getX() - 2000, Part.b3Part:getMax():getY(), Part.b3Part:getMax():getZ())
   local b3RestLength = BBox3d( ptRestLengthMin, ptRestLengthMax)
   -- si crea la trimesh dal box
   local idRestLengthBoxTm = EgtSurfTmBBox( Part.idTempGroup, b3RestLength , false, GDB_RT.GLOB)
   -- si estrae la silhouette nel piano
   local idFirstCurve, nCurveCount = EgtGetSurfTmSilhouette( idRestLengthBoxTm, vtDirection, 10 * GEO.EPS_SMALL, Part.idTempGroup, GDB_RT.GLOB)
   -- se più curve c'è qualcosa che non va: errore
   if nCurveCount > 1 then
      error( 'CheckCollisionAxisZ : error in part box')
   end
   -- creazione flat region dalla silhouette
   local idRestlengthSurfFr = EgtSurfFlatRegion( Part.idTempGroup, idFirstCurve)

   return idRestlengthSurfFr
end

-------------------------------------------------------------------------------------------------------------
-- calcolo pivot in riferimento globale
local function GetGlobalPivot( ptRef, vtHead, vtNEdge, Tool)

   -- frame solidale all'utensile (lo stesso in cui vtPivot è definito)
   local frTool = Frame3d( ptRef, vtHead, vtNEdge)
   -- TODO vtPivotGlob dovrà arrivare da funzione perchè può cambiare con SCC (es: aggregato)
   local vtPivotGlob = Vector3d( Tool.SetupInfo.vtPivot)
   vtPivotGlob:toGlob( frTool)
   local ptPivot = ptRef + vtPivotGlob

   return ptPivot
end

-------------------------------------------------------------------------------------------------------------
-- restituisce i punti notevoli della lavorazioni in cui fare il controllo
local function GetMachiningPoints( Edge, dDepthToMachine)

   local MachiningPoints = {}

   -- punti notevoli
   local ptStart = Edge.ptStart + Edge.vtN * ( Edge.dElevation - dDepthToMachine)
   local ptEnd = Edge.ptEnd + Edge.vtN * ( Edge.dElevation - dDepthToMachine)
   local ptMid = Point3d( ( ptStart + ptEnd) / 2)

   -- caso ottimizzato: lato parallelo ad una direzione principale
   local vtEdge = Edge.ptEnd - Edge.ptStart
   vtEdge:normalize()
   local bIsEdgeParallelToMainDirection =
      AreSameOrOppositeVectorApprox( vtEdge, X_AX())
      or AreSameOrOppositeVectorApprox( vtEdge, Y_AX())
      or AreSameOrOppositeVectorApprox( vtEdge, Z_AX())

   -- aggiunta punti
   -- inizio e fine solo se lato obliquo
   if bIsEdgeParallelToMainDirection then
      table.insert( MachiningPoints, ptStart)
   end
   table.insert( MachiningPoints, ptMid)
   if bIsEdgeParallelToMainDirection then
      table.insert( MachiningPoints, ptEnd)
   end

   return MachiningPoints
end

-------------------------------------------------------------------------------------------------------------
-- si controlla la collisione con il carro Z nei 3 piani globali X, Y e Z. Se c'è collisione in tutti i piani, il carro sarà in collisione.
function PreCollisionLib.CheckCollisionAxisZ( Edge, vtNFace, vtHead, Part, Tool, dDepthToMachine)

   -- se faccia non parallela a direzione testa c'è qualcosa che non va
   if not AreSameOrOppositeVectorApprox( vtNFace, vtHead) then
      error( 'CheckCollision : invalid directions')
   end

   -- TODO modificare e rimettere (Hmax pezzo)
   -- se direzione utensile parallela ad uno degli assi principali non serve alcuna verifica
   -- if AreSameOrOppositeVectorApprox( Edge.vtN, X_AX())
   --    or AreSameOrOppositeVectorApprox( Edge.vtN, Y_AX())
   --    or AreSameOrOppositeVectorApprox( Edge.vtN, Z_AX()) then
         
   --    return false
   -- end

   -- punti notevoli della lavorazione in cui fare il check
   local MachiningPoints = GetMachiningPoints( Edge, dDepthToMachine)

   -- punti in centro lama su naso mandrino o aggregato. In base a direzione e punto
   local bIsDownUp = AreOppositeVectorApprox( vtNFace, vtHead)
   local ToolExitPoints = {}
   for i = 1, #MachiningPoints do
      ToolExitPoints[i] = Point3d( MachiningPoints[i] + Edge.vtN * Tool.dDiameter / 2) + vtHead * EgtIf( bIsDownUp, ( Tool.dLength - Tool.dThickness), Tool.dLength)
   end

   -- direzioni in cui fare il check
   -- TODO queste cambieranno in base all'asse da controllare
   local CheckDirections = {
      { vtDirectionX = X_AX(), vtDirectionY = Z_AX(), vtDirectionZ = -Y_AX(), sDirectionName = 'X'},
      { vtDirectionX = Y_AX(), vtDirectionY = Z_AX(), vtDirectionZ = X_AX(), sDirectionName = 'Y'},
      { vtDirectionX = X_AX(), vtDirectionY = Y_AX(), vtDirectionZ = Z_AX(), sDirectionName = 'Z'}
   }
      
   -- TEST
   local Color = Color3d( math.random(0, 255), math.random(0, 255), math.random(0, 255))
   -- TEST

   local bMoveAfterSplit = false

   -- nel singolo punto notevole si ha collisione se si trova intersezione in tutte le direzioni
   -- se almeno in un punto c'è collisione con il pezzo si ritorna collisione
   -- se non si trova collisione si ritorna se è necessario separare prima di effettuare la lavorazione (ossia non c'è collisione con il pezzo ma c'è con il grezzo restante)
   for i = 1, #ToolExitPoints do

      -- pivot in globale
      local ptRef = ToolExitPoints[i]
      local ptPivot = GetGlobalPivot( ptRef, vtHead, Edge.vtN, Tool)

      local nCollisionCountPiece = 0
      local nCollisionCountRestlength = 0

      for j = 1, #CheckDirections do

         local vtDirectionX = CheckDirections[j].vtDirectionX
         local vtDirectionZ = CheckDirections[j].vtDirectionZ
         local sDirectionName = CheckDirections[j].sDirectionName
         
         -- costruzione flat region della curva di collisione nel piano scelto
         -- recupero punti da macchina per costruire flat region
         local CollisionCurvePoints = Tool.SetupInfo.GetAxisPoints( 'Z', sDirectionName)
         -- curva di collisione in riferimento locale
         local idCollisionCurve = EgtCurveCompoFromPoints( Part.idTempGroup, CollisionCurvePoints)
         -- TEST
         EgtSetColor( idCollisionCurve, Color)
         -- TEST
         -- curva in riferimento globale
         local frReference = Frame3d( ptPivot, vtDirectionZ, vtDirectionX)
         EgtTransform( idCollisionCurve, frReference, GDB_RT.GLOB )
         -- flat region di collisione
         local idCollisionSurfFr = EgtSurfFlatRegion( Part.idTempGroup, idCollisionCurve)

         -- check collisione con pezzo (per la collisione deve esserci intersezione su tutti e 3 i piani)
         -- si controlla solo se nelle direzioni precedenti è stata trovata collisione col pezzo,
         -- altrimenti significa che non si è usciti dal ciclo solamente la collisione con grezzo per restituire bMoveAfterSplit
         local bCollisionFoundPiece = false
         if nCollisionCountPiece == j - 1 then
            local idPartSurfFr = GetPartSurfFrInPlane( Part, vtDirectionZ)
            local bIntersectSuccess = EgtSurfFrIntersect( idPartSurfFr, idCollisionSurfFr)
            if not bIntersectSuccess then
               error( 'CheckCollision : intersect fail')
            end

            bCollisionFoundPiece = EgtExistsObj( idPartSurfFr)
         end

         -- check collisione con grezzo restante, se con il pezzo non c'è collisione
         local bCollisionFoundRestLength = false
         if not bCollisionFoundPiece then
            local idRestLengthSurfFr = GetRestlengthSurfFrInPlane( Part, vtDirectionZ)
            local bIntersectSuccess = EgtSurfFrIntersect( idRestLengthSurfFr, idCollisionSurfFr)
            if not bIntersectSuccess then
               error( 'CheckCollision : intersect fail')
            end

            bCollisionFoundRestLength = EgtExistsObj( idRestLengthSurfFr)
         end

         -- se non trovata collisione si passa al punto successivo
         if not ( bCollisionFoundPiece or bCollisionFoundRestLength) then
            nCollisionCountPiece = 0
            nCollisionCountRestlength = 0
            break
         -- altrimenti si aggiorna il numero di collisioni con pezzo e con grezzo restante
         elseif bCollisionFoundPiece then
            nCollisionCountPiece = nCollisionCountPiece + 1
         elseif bCollisionFoundRestLength then
            nCollisionCountRestlength = nCollisionCountRestlength + 1
         end
      end

      -- collisione trovata
      if nCollisionCountPiece == #CheckDirections then
         return true, nil
      -- nessuna collisione trovata con pezzo ma serve separazione
      elseif nCollisionCountRestlength == #CheckDirections then
         bMoveAfterSplit = true
      end
   end

   -- se si arriva qui significa che non è stata trovata alcuna collisione con pezzo
   return false, bMoveAfterSplit
end

-------------------------------------------------------------------------------------------------------------
-- nel piano perpendicolare alla direzione di taglio, se c'è intersezione tra motore e trave c'è collisione
-- TODO da integrare in funzione sopra
function PreCollisionLib.CheckCollisionAxisAB( Edge, vtNFace, vtHead, Part, Tool, dDepthToMachine)

   -- se direzione utensile parallela ad uno degli assi principali non serve alcuna verifica
   if AreSameOrOppositeVectorApprox( Edge.vtN, X_AX())
      or AreSameOrOppositeVectorApprox( Edge.vtN, Y_AX())
      or AreSameOrOppositeVectorApprox( Edge.vtN, Z_AX()) then
         
      return false
   end

   -- riferimento perpendicolare al taglio, X nel verso della normale al lato e Y verso il motore
   local ptMachining = Point3d( ( Edge.ptStart + Edge.ptEnd) / 2) + Edge.vtN * ( Edge.dElevation - dDepthToMachine)
   local vtEdge = Edge.ptEnd - Edge.ptStart
   vtEdge:normalize()
   
   -- costruzione flat region del pezzo e del grezzo restante nel piano perpendicolare al taglio
   local idPartSurfFr = GetPartSurfFrInPlane( Part, vtEdge)
   local idRestlengthSurfFr = GetRestlengthSurfFrInPlane( Part, vtEdge)

   -- costruzione flat region approssimata del motore nel piano perpendicolare al taglio
   -- punto di congiunzione tra motore e lama
   local ptMaxElev = ptMachining + Edge.vtN * Tool.dMaxMaterial
   -- se taglio standard (no DownUp) il punto va spostato dello spessore lama
   if AreSameVectorApprox( vtNFace, vtHead) then
      ptMaxElev = ptMaxElev + vtNFace * Tool.dThickness
   end
   -- TODO qui sostituire con il valore reale letto dal SetupInfo
   -- per Fast si dovrà aggiungere la lunghezza del massimo utensile utilizzabile con lama
   local dHeadDepth = 480
   local dHeadWidth = 480
   local ptOpposite = ptMaxElev + Edge.vtN * dHeadWidth
   local ptCross = ptOpposite + vtHead * dHeadDepth
   local idHeadRectangle = EgtRectangle3P( Part.idTempGroup, ptMaxElev, ptCross, ptOpposite, GDB_RT.GLOB)
   -- flat region dal rettangolo
   local idHeadSurfFr = EgtSurfFlatRegion( Part.idTempGroup, idHeadRectangle)

   -- intersezione tra pezzo e motore (se fallisce si inverte la flat region del motore, probabile normale opposta)
   local bIntersectSuccess = EgtSurfFrIntersect( idPartSurfFr, idHeadSurfFr)
   if not bIntersectSuccess then
      EgtInvertSurf( idHeadSurfFr)
      bIntersectSuccess = EgtSurfFrIntersect( idPartSurfFr, idHeadSurfFr)
   end
   if not bIntersectSuccess then
      error( 'Check2DBladeCollision : intersect fail')
   end

   local bCollisionFoundPiece = EgtExistsObj( idPartSurfFr)

   -- intersezione tra grezzo restante e motore, se con il pezzo non c'è collisione
   local bCollisionFoundRestLength
   if not bCollisionFoundPiece then
      bIntersectSuccess = EgtSurfFrIntersect( idRestlengthSurfFr, idHeadSurfFr)
      if not bIntersectSuccess then
         EgtInvertSurf( idHeadSurfFr)
         bIntersectSuccess = EgtSurfFrIntersect( idRestlengthSurfFr, idHeadSurfFr)
      end
      if not bIntersectSuccess then
         error( 'Check2DBladeCollision : intersect fail')
      end

      bCollisionFoundRestLength = EgtExistsObj( idRestlengthSurfFr)
   end

   return bCollisionFoundPiece, bCollisionFoundRestLength
end

















return PreCollisionLib