-- 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( ptOnToolTipCenter, vtHead, OptionalParameters)

   -- parametri opzionali
   OptionalParameters = OptionalParameters or {}
   local LinearAxes = OptionalParameters.LinearAxes
   local RotativeAxes = OptionalParameters.RotativeAxes

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

   -- 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 LinearAxes then
      EgtOutLog( ' ' .. LinearAxes[1].sName .. ' = ' .. tostring( LinearAxes[1].dValue) .. ', ' .. LinearAxes[2].sName .. ' = ' .. tostring( LinearAxes[2].dValue) .. ', ' .. LinearAxes[3].sName .. ' = ' .. tostring( LinearAxes[3].dValue))
   end
   if RotativeAxes then
      EgtOutLog( ' ' .. RotativeAxes[1].sName .. ' = ' .. tostring( RotativeAxes[1].dValue) .. ', ' .. RotativeAxes[2].sName .. ' = ' .. tostring( RotativeAxes[2].dValue))
   end

   return
end

-------------------------------------------------------------------------------------------------------------
-- 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 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
   -- 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( ptOnFace, vtHead, vtNFace, vtNEdge, nSCC, Tool)

   local bIsDownUp = AreOppositeVectorApprox( vtNFace, vtHead)

   local ptOnToolTipCenter = ptOnFace + vtNEdge * Tool.dDiameter / 2 + vtNFace * EgtIf( bIsDownUp, Tool.dThickness, 0)

   -- impostazione utensile
   EgtSetCalcTool( Tool.sName, Tool.sHead, Tool.nExit)
   EgtLoadTool( Tool.sHead, Tool.nExit, Tool.sName)

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

   -- assi macchina da calcolare
   local LinearAxes, RotativeAxes = GetMachineAxes()

   -- se più di 3 assi lineari e 2 assi rotativi, macchina non supportata
   if #LinearAxes > 3 or #RotativeAxes > 2 then

      error(' CheckOutOfStrokePoint : too many axes')
   end

   -- calcolo assi rotativi
   local bOkAngles, nSolutionsAngles, dRot1, dRot2 = EgtGetCalcAngles( vtHead)
   RotativeAxes[1].dValue = dRot1
   RotativeAxes[2].dValue = dRot2

   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( ptOnToolTipCenter, vtHead)
      end
      return true
   end

   -- calcolo assi lineari
   local bOkPositions, _, dLinear1, dLinear2, dLinear3 = EgtGetCalcPositions( ptOnToolTipCenter, RotativeAxes[1].dValue, RotativeAxes[2].dValue)
   LinearAxes[1].dValue = dLinear1
   LinearAxes[2].dValue = dLinear2
   LinearAxes[3].dValue = dLinear3

   if not bOkPositions then

      error( ' CheckOutOfStrokePoint : error')
   end

   -- verifica finecorsa per tutti gli assi
   -- siccome non si sa a priori il numero di assi lineari e rotativi, si aggiungono tutti a Args in ordine
   -- Args viene poi esplosa in modo da passare a EgtVerifyOutstroke i valori separati
   local Args = {}

   for i = 1, #LinearAxes do
      Args[#Args+1] = LinearAxes[i].dValue
   end

   for i = 1, #RotativeAxes do
      Args[#Args+1] = RotativeAxes[i].dValue
   end

   local bAllAxesInStroke = EgtVerifyOutstroke( table.unpack( Args))

   if not bAllAxesInStroke then

      if EgtGetDebugLevel() >= 3 then
         LogOutstroke( ptOnToolTipCenter, vtHead, { LinearAxes = LinearAxes, RotativeAxes = RotativeAxes})
      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: se non vengono passati punti si controllano inizio e fine del lato
function PreSimulationLib.CheckOutOfStroke( PointsToCheck, Parameters)

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

   -- SCC
   local nSCC = Tool.SetupInfo.GetSCC( Edge.vtN, Edge.vtEdge, vtNFace)

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

      local bOutOfStroke = CheckOutOfStrokePoint( PointsToCheck[i], vtHead, vtNFace, Edge.vtN, nSCC, Tool)

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

   -- se arrivati qui, nessun extracorsa
   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 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: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

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