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

-- Tabella per definizione modulo
local PreSimulationLib = {}

-- Include
require( 'EgtBase')
local BeamLib = require( 'BeamLib')
local BeamData = require( 'BeamDataNew')

EgtOutLog( ' PreSimulationLib started', 1)


-------------------------------------------------------------------------------------------------------------
-- costruzione trimesh del grezzo restante (con leggera sovrapposizione)
local function GetRestlengthSurfTm( Part)
   
   -- 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)

   return idRestLengthBoxTm
end

-------------------------------------------------------------------------------------------------------------
-- calcolo punto sull'uscita testa a partire dal punto di lavorazione o di attacco sul diametro utensile
local function GetToolExitPoint( ptMachining, vtNEdge, vtHead, Tool, bIsDownUp)

   local ptToolExitPoint = Point3d( ptMachining + vtNEdge * Tool.dDiameter / 2) + vtHead * EgtIf( bIsDownUp, ( Tool.dLength - Tool.dThickness), Tool.dLength)

   return ptToolExitPoint
end

-------------------------------------------------------------------------------------------------------------
-- calcolo pivot in riferimento globale, datipunto sull'uscita utensile e direzioni
local function GetGlobalPivot( ptToolExit, vtC, vtHead, vtMovePivot)

   -- frame solidale all'utensile (lo stesso in cui vtMovePivot è definito)
   local frTool = Frame3d( ptToolExit, vtHead, vtC)
   local vtMovePivotGlob = Vector3d( vtMovePivot)
   vtMovePivotGlob:toGlob( frTool)
   local ptPivot = ptToolExit + vtMovePivotGlob

   return ptPivot
end

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

   local PointsToCheck = {}

   -- 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 bIsEdgeParallelToMainDirection =
      AreSameOrOppositeVectorApprox( Edge.vtEdge, X_AX())
      or AreSameOrOppositeVectorApprox( Edge.vtEdge, Y_AX())
      or AreSameOrOppositeVectorApprox( Edge.vtEdge, Z_AX())

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

   return PointsToCheck
end

-------------------------------------------------------------------------------------------------------------
local function CheckOutOfStrokePoint( ptToolExitToCheck, vtC, vtHead, PreCollisionData)

   local bOutOfStroke = false
   local ptPivot = GetGlobalPivot( ptToolExitToCheck, vtC, vtHead, PreCollisionData.vtMovePivot)


   return bOutOfStroke
end

-------------------------------------------------------------------------------------------------------------
-- check finecorsa: se non vengono passati punti si controllano inizio e fine del lato
function PreSimulationLib.CheckOutOfStroke( Parameters, OptionalParameters )

   -- parametri obbligatori
   local Edge = Parameters.Edge
   local vtNFace = Parameters.vtNFace
   local vtHead = Parameters.vtHead
   local Tool = Parameters.Tool

   -- parametri opzionali
   OptionalParameters = OptionalParameters or {}
   local PointsToCheck = OptionalParameters.PointsToCheck or { Edge.ptStart, Edge.ptEnd}

   local bIsDownUp = AreOppositeVectorApprox( vtNFace, vtHead)

   -- punti curva collisione e direzioni check da macchina
   -- TODO serve pivot???
   local vtMovePivot = nil

   -- check collisione sui punti in centro lama su naso mandrino o aggregato. In base a direzione e punto
   for i = 1, #PointsToCheck do

      local ToolExitPoint = GetToolExitPoint( PointsToCheck[i], Edge.vtN, vtHead, Tool, bIsDownUp)

      --local bOutOfStroke = CheckOutOfStrokePoint( ptToolExitToCheck, vtC, vtHead, vtMovePivot)

      -- se trovato finecorsa inutile procedere con gli altri punti
      if bOutOfStroke then
         return true
      end
   end

   -- arrivati qui nessun finecorsa
   return false
end

-------------------------------------------------------------------------------------------------------------
local function CheckCollisionPoint( ptToolExitToCheck, vtC, vtHead, PreCollisionData, Part, bCheckOnlyRestlength)

   local ptPivot = GetGlobalPivot( ptToolExitToCheck, vtC, vtHead, PreCollisionData.vtMovePivot)

   -- orientamento del riferimento locale
   local vtDirectionX = PreCollisionData.Directions.vtDirectionX
   local vtDirectionY = PreCollisionData.Directions.vtDirectionY
   local vtDirectionZ = PreCollisionData.Directions.vtDirectionZ

   -- costruzione trimesh a partire dalla curva di collisione
   -- recupero punti da macchina per costruire trimesh
   local CollisionCurvePoints = PreCollisionData.Points
   -- curva di collisione in riferimento locale
   local idCollisionCurve = EgtCurveCompoFromPoints( Part.idTempGroup, CollisionCurvePoints)
   -- curva in riferimento globale
   local frReference = Frame3d( ptPivot, vtDirectionZ, vtDirectionX)
   EgtTransform( idCollisionCurve, frReference, GDB_RT.GLOB)
   -- trimesh di collisione
   local idCollisionSurfTm
   if PreCollisionData.bSurfTmByRevolve then
      idCollisionSurfTm = EgtSurfTmByRevolve( Part.idTempGroup, idCollisionCurve, ptPivot, vtDirectionY, true, 0.05, GDB_RT.GLOB)
   else
      local vtPreMove = -vtDirectionZ * ( PreCollisionData.dExtrusionDepth / 2)
      EgtMove( idCollisionCurve, vtPreMove, GDB_RT.GLOB)
      local vtExtrusion = vtDirectionZ * PreCollisionData.dExtrusionDepth
      idCollisionSurfTm = EgtSurfTmByRegionExtrusion( Part.idTempGroup, idCollisionCurve, vtExtrusion, 0.05, GDB_RT.GLOB)
   end

   -- check collisione con pezzo
   local bCollisionFoundPiece = false
   if not bCheckOnlyRestlength then
      bCollisionFoundPiece = EgtTestSurfaceSurface( Part.idBoxTm, idCollisionSurfTm, BeamData.COLL_SIC)
      if not type( bCollisionFoundPiece) == "boolean" then
         error( 'EgtTestSurfaceSurface fail')
      end
      if EgtGetDebugLevel() >= 3 and bCollisionFoundPiece then
         EgtSetColor( idCollisionSurfTm, RED())
      end

      -- se trovata collisione con pezzo è inutile procedere con il grezzo
      if bCollisionFoundPiece then
         return true
      end
   end

   -- check collisione con grezzo restante, se con il pezzo non c'è collisione
   local bCollisionFoundRestLength = false
   if not bCollisionFoundPiece then
      local idRestLengthSurfFr = GetRestlengthSurfTm( Part)
      bCollisionFoundRestLength = EgtTestSurfaceSurface( idRestLengthSurfFr, idCollisionSurfTm, BeamData.COLL_SIC)
      if not type( bCollisionFoundRestLength) == "boolean" then
         error( 'EgtTestSurfaceSurface fail')
      end
      if EgtGetDebugLevel() >= 3 and bCollisionFoundRestLength then
         EgtSetColor( idCollisionSurfTm, ORANGE())
      end
   end

   return false, bCollisionFoundRestLength
end

-------------------------------------------------------------------------------------------------------------
-- controllo collisione con verifica intersezione trimesh e geometrie da macchina
local function CheckCollisionWithAxis( sAxis, MachiningParameters, OptionalParameters)

   -- parametri obbligatori
   local Edge = MachiningParameters.Edge
   local vtNFace = MachiningParameters.vtNFace
   local vtHead = MachiningParameters.vtHead
   local Part = MachiningParameters.Part
   local Tool = MachiningParameters.Tool
   local dDepthToMachine = MachiningParameters.dDepthToMachine

   -- parametri opzionali
   OptionalParameters = OptionalParameters or {}
   local bCheckOnlyRestlength = OptionalParameters.bCheckOnlyRestlength or false

   -- se normale faccia non parallela a direzione testa c'è qualcosa che non va
   if not AreSameOrOppositeVectorApprox( vtNFace, vtHead) then
      error( 'CheckCollisionWithAxis : 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 PointsToCheck = OptionalParameters.PointsToCheck or GetPointsToCheck( 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, #PointsToCheck do
      ToolExitPoints[i] = GetToolExitPoint( PointsToCheck[i], Edge.vtN, vtHead, Tool, bIsDownUp)
   end

   -- vtC punta sempre verso il corpo dell'asse C o verso l'aggregato
   local nSCC = Tool.SetupInfo.GetSCC( Edge.vtN, Edge.vtEdge, vtNFace)
   local vtSCC = BeamLib.GetDirectionFromSCC( nSCC)
   local vtC = vtHead ^ Tool.SetupInfo.vtRotationAxisC
   vtC:normalize()
   if vtC:isZero() then
      vtC = vtSCC
   elseif vtC * vtSCC < GEO.EPS_SMALL then
      vtC = -vtC
   end

   -- punti curva collisione e direzioni check da macchina
   local PreCollisionData = Tool.SetupInfo.GetPreCollisionData( sAxis, vtC, vtHead)

   local bMoveAfterSplit = false

   -- 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

      local bCollisionFoundPiece, bCollisionFoundRestLength = CheckCollisionPoint( ToolExitPoints[i], vtC, vtHead, PreCollisionData, Part, bCheckOnlyRestlength)

      -- se trovata collisione con pezzo è inutile controllare gli altri punti
      if bCollisionFoundPiece then

         return true

      -- se trovata collisione con grezzo si setta che la lavorazione è da fare dopo separazione e si prosegue con gli altri punti
      elseif bCollisionFoundRestLength then

         bMoveAfterSplit = true
      end
   end

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

-------------------------------------------------------------------------------------------------------------
function PreSimulationLib.CheckCollision( sBladeEngagement, Parameters, OptionalParameters)

   local bCollisionFound
   local bMoveAfterSplitZ, bMoveAfterSplitC, bMoveAfterSplitAB

   -- parametri opzionali, in parte da far transitare
   OptionalParameters = OptionalParameters or {}
   
   local OptionalParametersCheckCollisionWithAxis = {}
   OptionalParametersCheckCollisionWithAxis.bCheckOnlyRestlength = false
   OptionalParametersCheckCollisionWithAxis.PointsToCheck = OptionalParameters.PointsToCheck or nil

   -- asse Z si controlla sempre
   bCollisionFound, bMoveAfterSplitZ = CheckCollisionWithAxis( 'Z', Parameters, OptionalParametersCheckCollisionWithAxis)

   -- se cubetti in modalità standard (no DownUp), gli assi AB e C si controllano solo con grezzo (ci sarebbe collisione con il materiale già rimosso controllando AB e C con pezzo)
   if sBladeEngagement == 'Standard' then
      OptionalParametersCheckCollisionWithAxis.bCheckOnlyRestlength = OptionalParameters.bIsDicing or false
   end

   if not bCollisionFound then
      bCollisionFound, bMoveAfterSplitAB = CheckCollisionWithAxis( 'AB', Parameters, OptionalParametersCheckCollisionWithAxis)
   end

   if not bCollisionFound then
      bCollisionFound, bMoveAfterSplitC = CheckCollisionWithAxis( 'C', Parameters, OptionalParametersCheckCollisionWithAxis)
   end

   local bMoveAfterSplit = bMoveAfterSplitZ or bMoveAfterSplitC or bMoveAfterSplitAB

   return bCollisionFound, bMoveAfterSplit
end

-------------------------------------------------------------------------------------------------------------
-- nel piano perpendicolare alla direzione di taglio, se c'è intersezione tra motore e trave c'è collisione
-- ATTENZIONE NON PIU' USATA, verificare ed eliminare
function PreSimulationLib.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)
   
   -- costruzione flat region del pezzo e del grezzo restante nel piano perpendicolare al taglio
   local idPartSurfFr = GetPartSurfFrInPlane( Part, Edge.vtEdge)
   local idRestlengthSurfFr = GetRestlengthSurfFrInPlane( Part, Edge.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
   local bCollisionFoundPiece = not EgtSurfFrTestExternal( idPartSurfFr, idHeadSurfFr)

   -- intersezione tra grezzo restante e motore, se con il pezzo non c'è collisione
   local bCollisionFoundRestLength
   if not bCollisionFoundPiece then
      bCollisionFoundRestLength = not EgtSurfFrTestExternal( idRestlengthSurfFr, idHeadSurfFr)
   end

   return bCollisionFoundPiece, bCollisionFoundRestLength
end

















return PreSimulationLib