-- EgtMachine.lua by EgalTech s.r.l.     11/05/2015

-- Tavola per definizione modulo (serve ma non usata)
local EgtMachine = {}

print( 'EgtMachine started')

-- Include
require( 'EgtBase')

-- Funzioni ausiliarie
local function CalcSqrDiff( Ax, Vx, Vxpre)
   local Diff = Vx - Vxpre
   if Ax == 'C' or Ax == 'B' then
      Diff = Diff * 10
   end
   return Diff * Diff
end

-- Movimento degli assi (da 1 a 5)
function MoveAxes( A1, V1, A2, V2, A3, V3, A4, V4, A5, V5)
   local Len = 0
   local V1pre = 0
   if A1 and V1 then
      V1pre = EgtGetAxisPos(A1)
      Len = Len + CalcSqrDiff( A1, V1, V1pre)
   end
   local V2pre = 0
   if A2 and V2 then
      V2pre = EgtGetAxisPos(A2)
      Len = Len + CalcSqrDiff( A2, V2, V2pre)
   end   
   local V3pre = 0
   if A3 and V3 then
      V3pre = EgtGetAxisPos(A3)
      Len = Len + CalcSqrDiff( A3, V3, V3pre)
   end
   if A4 and V4 then
      V4pre = EgtGetAxisPos(A4)
      Len = Len + CalcSqrDiff( A4, V4, V4pre)
   end
   if A5 and V5 then
      V5pre = EgtGetAxisPos(A5)
      Len = Len + CalcSqrDiff( A5, V5, V5pre)
   end
   Len = sqrt( Len)
   local Steps = max( ceil( Len / 50), 1)
   for i = 0, Steps do
      local sOut = ''
      local dFraz = i / Steps 
      if A1 and V1 then
         local dPos1 = V1pre * ( 1 - dFraz) + V1 * dFraz
         EgtSetAxisPos( A1, dPos1)
         sOut = sOut .. ' ' .. A1 .. string.format('%10.3f',dPos1)
      end
      if A2 and V2 then
         local dPos2 = V2pre * ( 1 - dFraz) + V2 * dFraz
         EgtSetAxisPos( A2, dPos2)
         sOut = sOut .. ' ' .. A2 .. string.format('%10.3f',dPos2)
      end
      if A3 and V3 then
         local dPos3 = V3pre * ( 1 - dFraz) + V3 * dFraz
         EgtSetAxisPos( A3, dPos3)
         sOut = sOut .. ' ' .. A3 .. string.format('%10.3f',dPos3)
      end
      if A4 and V4 then
         local dPos4 = V4pre * ( 1 - dFraz) + V4 * dFraz
         EgtSetAxisPos( A4, dPos4)
         sOut = sOut .. ' ' .. A4 .. string.format('%10.3f',dPos4)
      end
      if A5 and V5 then
         local dPos5 = V5pre * ( 1 - dFraz) + V5 * dFraz
         EgtSetAxisPos( A5, dPos5)
         sOut = sOut .. ' ' .. A5 .. string.format('%10.3f',dPos5)
      end
      EgtDraw()
      EgtTestBreak(0,0)
      EgtOutText(sOut)
   end
end

-- Riporta gli assi alla posizione home istantaneamente
function ResetAllAxes()
   EgtResetAxisPos( 'X')
   EgtResetAxisPos( 'Y')
   EgtResetAxisPos( 'Z')
   EgtResetAxisPos( 'C')
   EgtResetAxisPos( 'B')
   EgtDraw()
end  

-- Salgo a Zmax
function MoveToZHome()
   -- posizione home della Z
   local Zhome = EgtGetAxisHomePos( 'Z')
   -- risalita in Z
   MoveAxes( 'Z', Zhome)
end   

-- Vado al cambio utensili
function MoveToToolChange()
   -- posizioni home degli assi (tranne X)
   local Yhome = EgtGetAxisHomePos( 'Y')
   local Zhome = EgtGetAxisHomePos( 'Z')
   local Chome = EgtGetAxisHomePos( 'C')
   local Bhome = EgtGetAxisHomePos( 'B')
   -- risalita in Z
   MoveAxes( 'Z', Zhome)
   -- altri assi
   MoveAxes( 'Y', Yhome, 'C', Chome, 'B', Bhome)
end

-- Riporta gli assi alla posizione home con movimento prima Z poi XYCB
function MoveToHome()
   -- posizioni home degli assi
   local Xhome = EgtGetAxisHomePos( 'X')
   local Yhome = EgtGetAxisHomePos( 'Y')
   local Zhome = EgtGetAxisHomePos( 'Z')
   local Chome = EgtGetAxisHomePos( 'C')
   local Bhome = EgtGetAxisHomePos( 'B')
   -- risalita in Z
   MoveAxes( 'Z', Zhome)
   -- altri assi
   MoveAxes( 'X', Xhome, 'Y', Yhome, 'C', Chome, 'B', Bhome)
end
 
function SimuDrill( nId, dMaxMat) 

   local bOkR, nStatR, dA1, dB1, dA2, dB2
   local bOkL, nStatL, dX, dY, dZ
   local bOkS, nStatS
   local vtV, vtIn
   local ptP, ptOut, ptIn
   
   -- Recupero versore estrusione, versore profondità e punto foro
   vtV = EgtCurveExtrusion( nId, GDB_ID.ROOT)
   vtIn = EgtET( nId, GDB_ID.ROOT)
   ptP = EgtCP( nId, GDB_ID.ROOT)
   -- calcolo angoli macchina
   bOkR, nStatR, dA1, dB1, dA2, dB2 = EgtGetCalcAngles( vtV)
      -- punto fuori
   ptOut = ptP + vtV * 80
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptOut, dA1, dB1)
   bOkS, nStatS = EgtVerifyOutstroke( dX, dY, dZ, dA1, dB1)
   if not bOkS  or  nStatS ~= 0 then
      if not EgtOutBox( "Out Of Stroke", '', 'Error') then
         return false
      end
   end
   MoveAxes( 'X', dX, 'Y', dY, 'C', dA1, 'B', dB1)
   MoveAxes( 'Z', dZ)
   EgtTestBreak(0,100)
   -- se previsto massimo materiale, limito il punto dentro
   if dMaxMat and vtIn:len() > dMaxMat then
      vtIn = vtIn / vtIn:len() * dMaxMat
   end  
   -- punto dentro
   ptIn = ptP + vtIn
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptIn, dA1, dB1)
   bOkS, nStatS = EgtVerifyOutstroke( dX, dY, dZ, dA1, dB1)
   if not bOkS  or  nStatS ~= 0 then
      if not EgtOutBox( "Out Of Stroke", '', 'Error') then
         return false
      end
   end
   MoveAxes( 'X', dX, 'Y', dY, 'Z', dZ)
   EgtTestBreak(0,200)
   -- ritorno fuori
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptOut, dA1, dB1)
   MoveAxes( 'X', dX, 'Y', dY, 'Z', dZ)
   EgtTestBreak(0,100)
   return true
end 

function SimuUnderDrill( nId, dMaxMat, angOut, dLen) 

   local bOkR, nStatR, dA1, dB1, dA2, dB2
   local bOkL, nStatL, dX, dY, dZ
   local bOkS, nStatS
   local vtV, vtIn
   local ptP, ptOut, ptIn
   
   -- Recupero versore estrusione, versore profondità e punto foro
   vtV = EgtCurveExtrusion( nId, GDB_ID.ROOT)
   vtIn = EgtET( nId, GDB_ID.ROOT)
   ptP = EgtCP( nId, GDB_ID.ROOT)
   -- Verifico versore foro come Z-
   if not AreSameVectorApprox( vtV, - Z_AX()) then
      EgtOutBox( "Hole not from bottom", '', 'Error')
      return false
   end
   -- calcolo angoli macchina
   bOkR, nStatR, dA1, dB1, dA2, dB2 = EgtGetCalcAngles( vtV, VectorFromPolar( 1, angOut))
   -- punto di approccio
   ptOut = ptP + vtV * 20
   ptAppr = ptOut + VectorFromPolar( dLen, angOut)
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptAppr, dA1, dB1)
   bOkS, nStatS = EgtVerifyOutstroke( dX, dY, dZ, dA1, dB1)
   if not bOkS  or  nStatS ~= 0 then
      if not EgtOutBox( "Out Of Stroke", '', 'Error') then
         return false
      end
   end
   MoveAxes( 'X', dX, 'Y', dY, 'C', dA1, 'B', dB1)
   MoveAxes( 'Z', dZ)
   EgtTestBreak(0,100)   
   -- punto fuori
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptOut, dA1, dB1)
   bOkS, nStatS = EgtVerifyOutstroke( dX, dY, dZ, dA1, dB1)
   if not bOkS  or  nStatS ~= 0 then
      if not EgtOutBox( "Out Of Stroke", '', 'Error') then
         return false
      end
   end
   MoveAxes( 'X', dX, 'Y', dY, 'Z', dZ)
   EgtTestBreak(0,100)
   -- se previsto massimo materiale, limito il punto dentro
   if dMaxMat and vtIn:len() > dMaxMat then
      vtIn = vtIn / vtIn:len() * dMaxMat
   end  
   -- punto dentro
   ptIn = ptP + vtIn
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptIn, dA1, dB1)
   bOkS, nStatS = EgtVerifyOutstroke( dX, dY, dZ, dA1, dB1)
   if not bOkS  or  nStatS ~= 0 then
      if not EgtOutBox( "Out Of Stroke", '', 'Error') then
         return false
      end
   end
   MoveAxes( 'X', dX, 'Y', dY, 'Z', dZ)
   EgtTestBreak(0,200)
   -- ritorno fuori
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptOut, dA1, dB1)
   MoveAxes( 'X', dX, 'Y', dY, 'Z', dZ)
   EgtTestBreak(0,100)
   -- ritorno approccio
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptAppr, dA1, dB1)
   MoveAxes( 'X', dX, 'Y', dY, 'Z', dZ)
   EgtTestBreak(0,100)
   return true
end 

function SimuCut( nId, nCrv, dUnder, bHeadRight, bNotInvert, bOut, dAgg)
   -- verifico sia una retta
   --if EgtGetType( nId) ~= GDB_TY.CRV_LINE then
   --   return
   --end
   -- estremi retta, versore tg, affondamento
   local ptP1 = EgtUP( nId, ( nCrv - 1), GDB_ID.ROOT)
   local ptP2 = EgtUP( nId, nCrv, GDB_ID.ROOT)
   local vtTg = EgtUV( nId, nCrv, - 1, GDB_ID.ROOT)
   local dAff = EgtCurveThickness( nId, GDB_ID.ROOT)
   if dAff then
      dAff = abs(dAff)
   else
      dAff = 0
   end
   -- versore perpendicolare
   local vtPerp = Vector3d( vtTg)
   if bHeadRight then
      vtPerp:rotate( Z_AX(), -90)
   else
      vtPerp:rotate( Z_AX(), 90)
   end  
   -- se da invertire
   if not bNotInvert then
      local ptT = ptP1
      ptP1 = ptP2
      ptP2 = ptT
      vtTg = - vtTg
   end
   
   -- dati utensile
   local Tool = EgtGetCalcTool()
   EgtTdbSetCurrTool(Tool)
   local dRad = EgtTdbGetCurrToolParam(MCH_TP.DIAM) / 2
   local dTh = EgtTdbGetCurrToolParam(MCH_TP.THICK)
   
   -- angoli macchina
   local bOkR, nStatR, dA1, dB1, dA2, dB2 = EgtGetCalcAngles( vtPerp)   
   
   local bOkL, nStatL, dX, dY, dZ
   local bOkS, nStatS
   
   -- punto iniziale
   local dSu = dRad - dAff - dUnder
   local ptIni = ptP1 + Z_AX() * dSu
   if not bHeadRight then
      ptIni = ptIni - vtPerp * dTh
   end
   if bOut then
      local dOut = sqrt( dRad * dRad - dSu * dSu)
      ptIni = ptIni - vtTg * dOut
   end
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptIni, dA1, dB1)
   bOkS, nStatS = EgtVerifyOutstroke( dX, dY, dZ, dA1, dB1)
   if not bOkS  or  nStatS ~= 0 then
      if not EgtOutBox( "Out Of Stroke", '', 'Error') then
         return false
      end
   end
   -- verifico se mi muovo anche in Z
   local bPause = false
   local dOldZ = EgtGetAxisPos('Z')
   if dOldZ then
      if abs( dZ - dOldZ) > 100 * GEO.EPS_SMALL then
         bPause = true
      end
   end
   MoveAxes( 'X', dX, 'Y', dY, 'C', dA1, 'B', dB1)
   if bPause then
      EgtTestBreak(0,50)
   end
   MoveAxes( 'Z', dZ)   
   if bPause then
      EgtTestBreak(0,50)  
   end
  
   -- punto finale
   local ptFin = ptP2 + Z_AX() * dSu
   if dAgg then
      ptFin = ptFin + vtTg * dAgg 
   end
   if not bHeadRight then
      ptFin = ptFin - vtPerp * dTh
   end
   bOkL, nStatL, dX, dY, dZ = EgtGetCalcPositions( ptFin, dA1, dB1)
   bOkS, nStatS = EgtVerifyOutstroke( dX, dY, dZ, dA1, dB1)
   if not bOkS  or  nStatS ~= 0 then
      if not EgtOutBox( "Out Of Stroke", '', 'Error') then
         return false
      end
   end
   MoveAxes( 'X', dX, 'Y', dY)
   EgtTestBreak(0,5)
   return true
end  
 
return EgtMachine