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

-------------------------------------------------------------------------------------------------------------
local function GetMachineAxes()

   local LinearAxes = {}
   local RotativeAxes = {}

   -- si recuperano tutti gli assi, lineari e rotativi
   local AxesNames = EgtGetAllCurrAxesNames()
   for i = 1, #AxesNames do
      -- EgtGetAxisType restituisce vero se asse lineare, false se asse rotativo
      if EgtGetAxisType( AxesNames[i]) then
         LinearAxes[#LinearAxes + 1] = {}
         LinearAxes[#LinearAxes].sName = AxesNames[i]
      else
         RotativeAxes[#RotativeAxes + 1] = {}
         RotativeAxes[#RotativeAxes].sName = AxesNames[i]
      end
   end

   return LinearAxes, RotativeAxes
end

-------------------------------------------------------------------------------------------------------------
local function LogOutstroke( sToolName, ptOnToolTipCenter, vtHead, OptionalParameters)

   -- parametri opzionali
   OptionalParameters = OptionalParameters or {}
   local LinearAxesValues = OptionalParameters.LinearAxesValues
   local RotativeAxesValues = OptionalParameters.RotativeAxesValues

   -- gruppo per geometrie temporanee
   local idTempGroup = BeamLib.GetTempGroup()

   -- si disegnano punto e vettore
   local idPoint = EgtPoint( idTempGroup, ptOnToolTipCenter, GDB_RT.GLOB)
   local idVector = EgtVector( idTempGroup, vtHead, ptOnToolTipCenter, GDB_RT.GLOB)
   EgtSetColor( idPoint, RED())
   EgtSetColor( idVector, RED())

   -- nome utensile
   EgtOutLog( 'Tool ' .. sToolName)

   -- si loggano valori di punto e vettore
   EgtOutLog( ' Presimulation : OutStroke, Tip Point = ' .. tostring( ptOnToolTipCenter) .. ', id = ' .. idPoint .. ', vtHead = ' .. tostring( vtHead) .. ', id = ' .. idVector)

   -- se disponibili, si loggano anche i valori calcolati degli assi
   if LinearAxesValues then
      EgtOutLog( ' ' .. 'Lin1' .. ' = ' .. tostring( LinearAxesValues[1]) .. ', ' .. 'Lin2' .. ' = ' .. tostring( LinearAxesValues[2]) .. ', ' .. 'Lin3' .. ' = ' .. tostring( LinearAxesValues[3]))
   end
   if RotativeAxesValues then
      EgtOutLog( ' ' .. 'Rot1' .. ' = ' .. tostring( RotativeAxesValues[1]) .. ', ' .. 'Rot2' .. ' = ' .. tostring( RotativeAxesValues[2]) .. ', ' .. 'Rot3' .. ' = ' .. tostring( RotativeAxesValues[3]))
   end

   return
end

-------------------------------------------------------------------------------------------------------------
-- costruzione trimesh del grezzo restante in testa o in coda
local function GetRestlengthSurfTm( Part, sSide)

   -- si costruisce il box in globale
   local b3RestLength
   if sSide == 'Head' then
      local b3PartWithOvermaterial = BeamLib.GetPartBoxWithHeadTail( Part, sSide)
      local ptStartRestLength = Point3d( Part.b3Part:getMax():getX(), Part.b3Part:getMax():getY(), Part.b3Part:getMax():getZ())
      local ptEndRestLength = Point3d( b3PartWithOvermaterial:getMax():getX(), Part.b3Part:getMin():getY(), Part.b3Part:getMin():getZ())
      b3RestLength = BBox3d( ptStartRestLength, ptEndRestLength)
   elseif sSide == 'Tail' then
      local b3PartWithOvermaterial = BeamLib.GetPartBoxWithHeadTail( Part, sSide)
      local dXMin = b3PartWithOvermaterial:getMin():getX()
      -- si evita sempre di lavorare col motore dietro la coda della barra (potrebbe succedere con barra restante molto corta): in quel caso si deve sempre separare prima
      dXMin = min( dXMin, Part.b3Part:getMin():getX() - 2000)
      local ptStartRestLength = Point3d( Part.b3Part:getMin():getX(), Part.b3Part:getMax():getY(), Part.b3Part:getMax():getZ())
      local ptEndRestLength = Point3d( dXMin, Part.b3Part:getMin():getY(), Part.b3Part:getMin():getZ())
      b3RestLength = BBox3d( ptStartRestLength, ptEndRestLength)
   -- caso non testato, non dovrebbe mai finire qui (il default che arriva da fuori è Tail)
   else
      return nil
   end

   -- si crea la trimesh dal box
   local idRestLengthBoxTm = EgtSurfTmBBox( Part.idTempGroup, b3RestLength , false, GDB_RT.GLOB)

   return idRestLengthBoxTm
end

-------------------------------------------------------------------------------------------------------------
function PreSimulationLib.GetPointOnToolTipCenter( ptPointAtDepth, vtHead, vtNFace, vtToolOffset, Tool)

   return ptPointAtDepth + vtToolOffset * Tool.dDiameter / 2 + vtNFace * EgtIf( AreSameVectorApprox( vtHead, vtNFace), 0, Tool.dThickness)
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 GetCollisionPointsToCheck( 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
   table.insert( PointsToCheck, ptStart)
   if not bIsEdgeParallelToMainDirection then
      table.insert( PointsToCheck, ptMid)
   end
   table.insert( PointsToCheck, ptEnd)

   return PointsToCheck
end

-------------------------------------------------------------------------------------------------------------
local function CheckOutOfStrokePoint( ptOnToolTipCenter, vtHead, nSCC, Tool, vtAux, sBlockedAxis)

   -- impostazione utensile
   local bOkTool = EgtSetCalcTool( Tool.sName, Tool.sHead, Tool.nExit)
   if not bOkTool then
      error( 'CheckOutOfStrokePoint : cannot set calc tool')
   end

   -- settaggio SCC per discriminare soluzioni multiple
   EgtSetCalcSolCh( nSCC)

   -- se presente, settaggio asse bloccato
   if sBlockedAxis and type( sBlockedAxis) == "string" then
      local BlockedAxis = EgtSplitString( sBlockedAxis, '=')
      EgtSetRotAxisBlock( BlockedAxis[1], tonumber( BlockedAxis[2]))
   end

   -- calcolo assi rotativi
   local bOkAngles, nSolutionsAngles, RotativeAxesValues = EgtGetCalcAnglesEx( vtHead, vtAux)
   local dRotative1 = RotativeAxesValues[1]
   local dRotative2 = RotativeAxesValues[2]
   local dRotative3 = RotativeAxesValues[3]

   if not bOkAngles then
      error( ' CheckOutOfStrokePoint : error')
   end

   -- se nessuna soluzione dagli assi rotativi, è in extracorsa
   if nSolutionsAngles == 0 then

      if EgtGetDebugLevel() >= 3 then
         LogOutstroke( Tool.sName, ptOnToolTipCenter, vtHead, { RotativeAxesValues = { dRotative1, dRotative2, dRotative3}})
      end
      return true
   end

   -- calcolo assi lineari
   local bOkPositions, _, dLinear1, dLinear2, dLinear3 = EgtGetCalcPositions( ptOnToolTipCenter, dRotative1, dRotative2, dRotative3)

   if not bOkPositions then

      error( ' CheckOutOfStrokePoint : error')
   end

   -- verifica finecorsa per assi lineari (assi rotativi già verificati)
   local bAllAxesInStroke = EgtVerifyOutstroke( dLinear1, dLinear2, dLinear3)

   -- extracorsa
   if not bAllAxesInStroke then

      if EgtGetDebugLevel() >= 3 then
         LogOutstroke( Tool.sName, ptOnToolTipCenter, vtHead, { LinearAxesValues = { dLinear1, dLinear2, dLinear3}, RotativeAxesValues = { dRotative1, dRotative2, dRotative3}})
      end
      return true
   end

   -- EgtSetAxisPos( 'T', dT)
   -- EgtSetAxisPos( 'Y', dY)
   -- EgtSetAxisPos( 'Z', dZ)
   -- EgtSetAxisPos( 'C', dC1)
   -- EgtSetAxisPos( 'A', dA1)

   -- se si arriva qui, il punto non è in finecorsa
   return false
end

-------------------------------------------------------------------------------------------------------------
-- check extracorsa da punti sul tip dell'utensile
function PreSimulationLib.CheckOutOfStrokeFromPoints( PointsOnToolTipCenter, vtHead, nSCC, Tool, vtAux, sBlockedAxis)

   for i = 1, #PointsOnToolTipCenter do

      local bOutOfStroke = CheckOutOfStrokePoint( PointsOnToolTipCenter[i], vtHead, nSCC, Tool, vtAux, sBlockedAxis)

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

   -- se arrivati qui, nessun extracorsa
   return false
end

-------------------------------------------------------------------------------------------------------------
-- check extracorsa da geometria
-- TODO da considerare anche gli attacchi
function PreSimulationLib.CheckOutOfStrokeFromGeometry( idGeometry, vtHead, nSCC, Tool, vtAux, sBlockedAxis)

   local b3GeomMaxOffset = EgtGetBBoxGlob( idGeometry, GDB_BB.STANDARD)
   local ptBoxCenter = b3GeomMaxOffset:getCenter()
   local dBoxDimX = b3GeomMaxOffset:getDimX()
   local dBoxDimY = b3GeomMaxOffset:getDimY()
   local dBoxDimZ = b3GeomMaxOffset:getDimZ()

   -- si controlla il finecorsa nei punti al centro delle 6 facce del box
   local PointsOnToolTipCenter = {}
   -- X+
   table.insert( PointsOnToolTipCenter, Point3d( ptBoxCenter + dBoxDimX / 2 * X_AX()))
   -- X-
   table.insert( PointsOnToolTipCenter, Point3d( ptBoxCenter - dBoxDimX / 2 * X_AX()))
   -- Y+
   table.insert( PointsOnToolTipCenter, Point3d( ptBoxCenter + dBoxDimY / 2 * Y_AX()))
   -- Y-
   table.insert( PointsOnToolTipCenter, Point3d( ptBoxCenter - dBoxDimY / 2 * Y_AX()))
   -- Z+
   table.insert( PointsOnToolTipCenter, Point3d( ptBoxCenter + dBoxDimZ / 2 * Z_AX()))
   -- Z-
   table.insert( PointsOnToolTipCenter, Point3d( ptBoxCenter - dBoxDimZ / 2 * Z_AX()))

   local bOutOfStroke = PreSimulationLib.CheckOutOfStrokeFromPoints( PointsOnToolTipCenter, vtHead, nSCC, Tool, vtAux, sBlockedAxis)

   return bOutOfStroke
end

-------------------------------------------------------------------------------------------------------------
local function CheckCollisionPoint( ptToolExitToCheck, vtC, vtHead, PreCollisionData, Part, bCannotSplitRestLength, sRestLengthSideForPreSimulation, 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
      local idCheckCollisionTm = Part.idBoxTm
      -- se testa o coda attaccate, si considerano nella superficie di collisione
      if bCannotSplitRestLength then
         local b3CheckCollision = BeamLib.GetPartBoxWithHeadTail( Part, sRestLengthSideForPreSimulation)
         idCheckCollisionTm = EgtSurfTmBBox( Part.idTempGroup, b3CheckCollision, false, GDB_RT.GLOB)
      end
      bCollisionFoundPiece = EgtCDeSolidSolid( idCheckCollisionTm, idCollisionSurfTm, BeamData.COLL_SIC)
      if not type( bCollisionFoundPiece) == "boolean" then
         error( 'Presimulation 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 e non è un taglio di testa o coda
   local bCollisionFoundRestLength = false
   if not ( bCollisionFoundPiece or bCannotSplitRestLength) then
      local idRestLengthSurfFr = GetRestlengthSurfTm( Part, sRestLengthSideForPreSimulation)
      if idRestLengthSurfFr then
         bCollisionFoundRestLength = EgtCDeSolidSolid( idRestLengthSurfFr, idCollisionSurfTm, BeamData.COLL_SIC)
         if not type( bCollisionFoundRestLength) == "boolean" then
            error( 'Presimulation fail')
         end
         if EgtGetDebugLevel() >= 3 and bCollisionFoundRestLength then
            EgtSetColor( idCollisionSurfTm, ORANGE())
         end
      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
   local sRestLengthSideForPreSimulation = OptionalParameters.sRestLengthSideForPreSimulation or 'Tail'
   local bCannotSplitRestLength = OptionalParameters.bCannotSplitRestLength 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
   
   -- punti notevoli della lavorazione in cui fare il check
   local PointsToCheck = OptionalParameters.PointsToCheck or GetCollisionPointsToCheck( 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:isSmall() 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, bCannotSplitRestLength, sRestLengthSideForPreSimulation, 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
   OptionalParametersCheckCollisionWithAxis.sRestLengthSideForPreSimulation = OptionalParameters.sRestLengthSideForPreSimulation or 'Tail'
   OptionalParametersCheckCollisionWithAxis.bCannotSplitRestLength = OptionalParameters.bCannotSplitRestLength or false

   local bIsDicing = OptionalParameters.bIsDicing or false
   local bCheckOnlyRestlengthForAxisABC = false

   -- 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 bIsDicing and ( sBladeEngagement == 'Standard') then
      bCheckOnlyRestlengthForAxisABC = true
   -- se l'elevazione reale (rispetto al pezzo + eventuale materiale in testa/coda) è maggiore del massimo materiale è sempre collisione
   -- TODO rifare con funzione
   else
      local Edge = Parameters.Edge
      local vtNFace = Parameters.vtNFace
      local dDepthToMachine = Parameters.dDepthToMachine
      local b3BoxForElevationCheck = BBox3d( Parameters.Part.b3Part)
      -- se è un taglio di testa o coda va aggiunto il sovramateriale
      if OptionalParametersCheckCollisionWithAxis.bCannotSplitRestLength then
         b3BoxForElevationCheck = BeamLib.GetPartBoxWithHeadTail( Parameters.Part, OptionalParametersCheckCollisionWithAxis.sRestLengthSideForPreSimulation)
      end
      -- trimesh rettangolare con un lato corrispondente al lato da lavorare e larghezza come spessore utensile
      local idTrimesh = EgtSurfTmRectangle( Parameters.Part.idTempGroup, Edge.ptStart, Edge.ptEnd, Edge.ptEnd + vtNFace * Parameters.Tool.dThickness, GDB_RT.GLOB)
      local vtNTrimesh = EgtSurfTmFacetNormVersor( idTrimesh, 0, GDB_ID.ROOT)
      -- se trimesh con normale opposta a quella del lato, si inverte (non si sa a priori che verso avrà)
      if not AreSameVectorApprox( Edge.vtN, vtNTrimesh) then
         EgtInvertSurf( idTrimesh)
      end
      local dRealElevation = EgtSurfTmFacetElevationInBBox( idTrimesh, 0, b3BoxForElevationCheck, true, GDB_ID.ROOT)
      local dRealDepthToMachine = ( dRealElevation - Parameters.Edge.dElevation + dDepthToMachine)
      if dRealDepthToMachine > Parameters.Tool.dMaxDepth + 10 * GEO.EPS_SMALL then
         return true
      end
   end

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

   -- assi AB e C: se richiesto si controlla la collisione solo col grezzo
   OptionalParametersCheckCollisionWithAxis.bCheckOnlyRestlength = bCheckOnlyRestlengthForAxisABC

   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

-------------------------------------------------------------------------------------------------------------
return PreSimulationLib