Imports EgtUILib

Module VacuumCups
   ' Tipo manipolatore con ventosa (0=assente, 1=dietro, 2=laterale)
   Private m_nVacType As Integer = 0
   ' Dati del manipolatore
   Private m_nTempId As Integer = GDB_ID.NULL
   Private m_nVacId As Integer = GDB_ID.NULL
   Private m_nRefId As Integer = GDB_ID.NULL

   '-----------------------------------------------------------------------------------------------
   Friend Class RawMoveData

      Public m_nId As Integer
      Public m_vtMove As Vector3d
      Public m_vtDelta As Vector3d
      Public m_dAngRotDeg As Double
      Public m_sCups As String

      Sub New()
         m_nId = GDB_ID.NULL
         m_vtMove = Vector3d.NULL()
         m_vtDelta = Vector3d.NULL()
         m_dAngRotDeg = 0
         m_sCups = String.Empty
      End Sub

      Sub New(nId As Integer)
         m_nId = nId
         m_vtMove = Vector3d.NULL()
         m_vtDelta = Vector3d.NULL()
         m_dAngRotDeg = 0
         m_sCups = String.Empty
      End Sub
   End Class


   '-----------------------------------------------------------------------------------------------
   Friend Function GetVacuumType() As Integer
      Return m_nVacType
   End Function

   Friend Function GetVacuumId() As Integer
      Return m_nVacId
   End Function

   Friend Function LoadVacuumCups() As Boolean
      ' Leggo tipo manipolatore con ventosa
      m_nVacType = 0
      EgtGetInfo(EgtGetHeadId(VACUUM_HEAD), "VacType", m_nVacType)
      ' Cancello eventuali vecchie ventose
      RemoveVacuumCups()
      ' Identificativo ventose nella macchina
      Dim nLayId As Integer = EgtGetFirstNameInGroup(EgtGetHeadId(VACUUM_HEAD), VACUUM_HEAD_LAYOUT)
      If nLayId = GDB_ID.NULL Then Return False
      ' Identificativo riferimento della testa nella macchina
      Dim nT1Id As Integer = EgtGetFirstNameInGroup(EgtGetHeadId(VACUUM_HEAD), HEAD_FIRST_EXIT)
      If nT1Id = GDB_ID.NULL Then Return False
      ' Creo gruppo temporaneo in cui copiarli
      m_nTempId = EgtCreateGroup(GDB_ID.ROOT)
      If m_nTempId = GDB_ID.NULL Then Return False
      EgtSetName(m_nTempId, "VacTmp")
      EgtSetLevel(m_nTempId, GDB_LV.SYSTEM)
      ' Eseguo copia in globale
      m_nVacId = EgtCopyGlob(nLayId, m_nTempId)
      If m_nVacId = GDB_ID.NULL Then Return False
      ' Nascondo il gruppo ma rendo visibili le curve di contorno delle ventose
      EgtSetStatus(m_nVacId, GDB_ST.OFF)
      Dim nId As Integer = EgtGetFirstInGroup(m_nVacId)
      While nId <> GDB_ID.NULL
         Select Case EgtGetType(nId)
         Case GDB_TY.CRV_LINE, GDB_TY.CRV_ARC, GDB_TY.CRV_BEZ, GDB_TY.CRV_COMPO
            EgtSetStatus(nId, GDB_ST.ON_)
         Case Else
            EgtSetStatus(nId, GDB_ST.OFF)
         End Select
         nId = EgtGetNext(nId)
      End While
      m_nRefId = EgtCopyGlob(nT1Id, m_nVacId)
      If m_nRefId = GDB_ID.NULL Then Return False
      EgtSetStatus(m_nRefId, GDB_ST.ON_)
      Return True
   End Function

   Friend Function RemoveVacuumCups() As Boolean
      ' cancello eventuale gruppo per ventose
      EgtErase(m_nTempId)
      m_nTempId = GDB_ID.NULL
      m_nVacId = GDB_ID.NULL
      m_nRefId = GDB_ID.NULL
      Return True
   End Function

   Friend Function ResetVacuumCups() As Boolean
      ' Ripristino posizione e rotazione originali
      Dim frOriRef As New Frame3d
      EgtFrame(EgtGetFirstNameInGroup(EgtGetHeadId(VACUUM_HEAD), HEAD_FIRST_EXIT), GDB_ID.ROOT, frOriRef)
      Dim frCurrRef As New Frame3d
      EgtFrame(m_nRefId, GDB_ID.ROOT, frCurrRef)
      EgtMove(m_nVacId, frOriRef.Orig() - frCurrRef.Orig(), GDB_RT.GLOB)
      frCurrRef.ToLoc(frOriRef)
      Dim dLen, dAngVertDeg, dAngOrizzDeg As Double
      frCurrRef.VersX().ToSpherical(dLen, dAngVertDeg, dAngOrizzDeg)
      EgtRotate(m_nVacId, frOriRef.Orig(), frOriRef.VersZ(), -dAngOrizzDeg, GDB_RT.GLOB)
      ' Ripristino visualizzazione originale
      Dim nId As Integer = EgtGetFirstInGroup(m_nVacId)
      While nId <> GDB_ID.NULL
         Select Case EgtGetType(nId)
         Case GDB_TY.CRV_LINE, GDB_TY.CRV_ARC, GDB_TY.CRV_BEZ, GDB_TY.CRV_COMPO
            EgtSetStatus(nId, GDB_ST.ON_)
         Case Else
            EgtSetStatus(nId, GDB_ST.OFF)
         End Select
         nId = EgtGetNext(nId)
      End While
      EgtSetStatus(m_nRefId, GDB_ST.ON_)
      Return True
   End Function

   Friend Function PutVacuumCupsOnRaw(nRawId As Integer, ByRef rmData As RawMoveData) As Boolean
      ' Ripristino posizione originale ventose
      ResetVacuumCups()
      ' Box e baricentro del grezzo (riportato sopra al grezzo)
      Dim b3Raw As New BBox3d
      GetRawBox(nRawId, b3Raw)
      Dim ptRawCen As Point3d
      GetRawCenter(nRawId, ptRawCen)
      ptRawCen.z += b3Raw.DimZ() / 2
      ' Se non esiste, creo la regione del kerf del grezzo
      Dim nKerfId As Integer = EgtGetFirstNameInGroup(nRawId, NAME_KERF)
      Dim nRKerfId = EgtGetFirstNameInGroup(nRawId, NAME_KERF_REGION)
      If nRKerfId = GDB_ID.NULL Then
         nRKerfId = EgtCreateSurfFlatRegion(nRawId, nKerfId)
         ' se non sono riuscito a crearla, allora è nulla e posso uscire
         If nRKerfId = GDB_ID.NULL Then Return False
         EgtSetName(nRKerfId, NAME_KERF_REGION)
         EgtSetStatus(nRKerfId, GDB_ST.OFF)
      End If
      ' Box e baricentro della regione di kerf
      Dim b3Kerf As New BBox3d
      EgtGetBBoxGlob(nRKerfId, GDB_BB.STANDARD, b3Kerf)
      Dim ptKerfCen As Point3d
      EgtCentroid(nRKerfId, GDB_ID.ROOT, ptKerfCen)
      ' Eseguo ricerca
      If FindVacuumCupsOnRaw(nRawId, ptRawCen, b3Kerf, ptKerfCen, nKerfId, nRKerfId, rmData) Then
         Return True
      End If
      ' In caso di fallimento, provo riducendo con offset la regione di kerf
      ' (così si simula la proiezione del centro sul MAT - medial axis transform)
      Dim bOkFind As Boolean = False
      Dim vOffset() As Double = {-450, -350, -250, -150}
      For i As Integer = 0 To vOffset.Length() - 1
         Dim nRKerfOffsId = EgtCopy(nRKerfId, nRKerfId, GDB_POS.AFTER)
         If EgtSurfFrOffset(nRKerfOffsId, vOffset(i), OFF_TYPE.FILLET) AndAlso
            EgtCentroid(nRKerfOffsId, GDB_ID.ROOT, ptKerfCen) Then
            bOkFind = FindVacuumCupsOnRaw(nRawId, ptRawCen, b3Kerf, ptKerfCen, nRKerfOffsId, nRKerfId, rmData)
         End If
         EgtErase(nRKerfOffsId)
         If bOkFind Then Return True
      Next
      Return False
   End Function

   Private Function FindVacuumCupsOnRaw(nRawId As Integer, ptRawCen As Point3d,
                                        b3Kerf As BBox3d, ptKerfCen As Point3d, nOutlineId As Integer, nRKerfId As Integer,
                                        ByRef rmData As RawMoveData) As Boolean
      ' Cerco migliore configurazione di ventose per prendere il grezzo
      Const MAX_SEL As Integer = 20
      For nI As Integer = 1 To MAX_SEL
         ' Recupero la configurazione di ventose nI-esima
         Dim sCups() As String = Nothing
         Dim sCups2() As String = Nothing
         If Not GetVacuumCupSelection(nI, sCups, sCups2) Then Return False
         ' Determino validità soluzioni della configurazione
         Dim vtMove As New Vector3d
         Dim ptRotCen As New Point3d
         Dim dRotAngDeg As Double = 0
         Dim vtMove2 As New Vector3d
         Dim ptRotCen2 As New Point3d
         Dim dRotAngDeg2 As Double = 0
         Dim dDist = TestVacuumCupSelection(sCups, b3Kerf, ptKerfCen, nOutlineId, nRKerfId, vtMove, ptRotCen, dRotAngDeg)
         Dim dDist2 = TestVacuumCupSelection(sCups2, b3Kerf, ptKerfCen, nOutlineId, nRKerfId, vtMove2, ptRotCen2, dRotAngDeg2)
         If dDist > INFINITO - 1 And dDist2 > INFINITO - 1 Then Continue For
         If dDist2 < dDist Then
            sCups = sCups2
            vtMove = vtMove2
            ptRotCen = ptRotCen2
            dRotAngDeg = dRotAngDeg2
         End If
         ' Eseguo il movimento
         EgtMove(m_nVacId, vtMove, GDB_RT.GLOB)
         EgtRotate(m_nVacId, ptRotCen, Vector3d.Z_AX(), dRotAngDeg, GDB_RT.GLOB)
         ' Visualizzo le ventose
         For nJ As Integer = 0 To sCups.Length() - 1
            Dim nCupId = EgtGetFirstNameInGroup(m_nVacId, sCups(nJ))
            EgtSetStatus(nCupId, GDB_ST.ON_)
         Next
         ' Calcolo delta posizione
         Dim frCurrRef As New Frame3d
         EgtFrame(m_nRefId, GDB_ID.ROOT, frCurrRef)
         Dim vtDelta As Vector3d = frCurrRef.Orig() - ptRawCen
         ' Assegno dati noti al movimento del grezzo
         rmData.m_nId = nRawId
         rmData.m_vtMove = Vector3d.NULL()
         rmData.m_vtDelta = vtDelta
         rmData.m_dAngRotDeg = dRotAngDeg
         Dim sVal As String = String.Empty
         For Each sCup As String In sCups
            If String.IsNullOrEmpty(sVal) Then
               sVal &= sCup
            Else
               sVal &= "," & sCup
            End If
         Next
         rmData.m_sCups = sVal
         Return True
      Next
      Return False
   End Function

   Private Function GetVacuumCupSelection(nInd As Integer, ByRef sCups() As String, ByRef sCups2() As String) As Boolean
      ' Recupero elenco ventose nella soluzione
      Dim sSel As String = "Sel" & nInd.ToString()
      Dim sVal As String = String.Empty
      If Not EgtGetInfo(m_nVacId, sSel, sVal) Then Return False
      If sVal.IndexOf("/") >= 0 Then
         Dim sSplit() As String = sVal.Split("/".ToCharArray)
         If sSplit.Length() >= 2 Then
            sCups = sSplit(0).Split(",".ToCharArray)
            sCups2 = sSplit(1).Split(",".ToCharArray)
         ElseIf sSplit.Length() >= 1 Then
            sCups = sSplit(0).Split(",".ToCharArray)
            sCups2 = Nothing
         Else
            sCups = Nothing
            sCups2 = Nothing
         End If
      Else
         sCups = sVal.Split(",".ToCharArray)
         sCups2 = Nothing
      End If
      Return True
   End Function

   Private Function TestVacuumCupSelection(sCups() As String, b3Raw As BBox3d, ptRawCen As Point3d, nOutlineId As Integer, nRawRegId As Integer,
                                           ByRef vtMove As Vector3d, ByRef ptRotCen As Point3d, ByRef dRotAngDeg As Double) As Double
      ' Se definizione mancante, scarto soluzione
      If IsNothing(sCups) Then Return INFINITO
      ' Ne calcolo il box
      Dim b3Vac As New BBox3d
      For nJ As Integer = 0 To sCups.Length() - 1
         Dim nCupId = EgtGetFirstNameInGroup(m_nVacId, sCups(nJ))
         Dim b3Cup As New BBox3d
         If EgtGetBBoxGlob(nCupId, GDB_BB.STANDARD, b3Cup) Then b3Vac.Add(b3Cup)
      Next
      If b3Vac.IsEmpty() Then Return INFINITO
      ' Se box maggiore di quello del pezzo, scarto soluzione
      If Not ((b3Vac.DimX() < b3Raw.DimX() And b3Vac.DimY() < b3Raw.DimY()) Or
              (b3Vac.DimX() < b3Raw.DimY() And b3Vac.DimY() < b3Raw.DimX())) Then
         Return INFINITO
      End If
      ' Determino il movimento
      vtMove = ptRawCen - b3Vac.Center()
      b3Vac.Move(vtMove)
      ' Determino la rotazione, allineando il lato lungo delle ventose con quello del grezzo
      ptRotCen = b3Vac.Center()
      Dim frMinRect As New Frame3d
      If EgtCurveMinAreaRectangleXY(nOutlineId, nOutlineId, frMinRect) Then
         Dim dLen, dAngVertDeg, dAngOrizzDeg As Double
         frMinRect.VersX().ToSpherical(dLen, dAngVertDeg, dAngOrizzDeg)
         dRotAngDeg = dAngOrizzDeg
         If b3Vac.DimY() > b3Vac.DimX() Then dRotAngDeg += 90
         While dRotAngDeg > 90
            dRotAngDeg -= 180
         End While
         While dRotAngDeg < -90
            dRotAngDeg += 180
         End While
      Else
         dRotAngDeg = 0
         If (b3Vac.DimX() >= b3Vac.DimY() And b3Raw.DimX() < b3Raw.DimY()) Or
            (b3Vac.DimX() < b3Vac.DimY() And b3Raw.DimX() >= b3Raw.DimY()) Then
            dRotAngDeg = 90
         End If
      End If
      ' Identificativi delle ventose
      Dim nCups(sCups.Length() - 1) As Integer
      For nJ As Integer = 0 To sCups.Length() - 1
         nCups(nJ) = EgtGetFirstNameInGroup(m_nVacId, sCups(nJ))
      Next
      ' Eseguo verifica delle ventose rispetto al grezzo
      Dim bVacOk As Boolean = False
      Dim vAngRot() As Double = {0, +5, -5, +12.5, -12.5, +25, -25, +45, -45}
      For i As Integer = 0 To vAngRot.Length() - 1
         If TestVacuumCups(nCups, nRawRegId, vtMove, ptRotCen, dRotAngDeg + vAngRot(i)) Then
            dRotAngDeg += vAngRot(i)
            bVacOk = True
            Exit For
         End If
      Next
      If Not bVacOk Then Return INFINITO
      ' Recupero riferimento della testa ventose
      Dim frCurrRef As New Frame3d
      EgtFrame(m_nRefId, GDB_ID.ROOT, frCurrRef)
      Dim ptRef As Point3d = frCurrRef.Orig()
      ' Applico movimento e rotazione al punto
      ptRef.Move(vtMove)
      ptRef.Rotate(ptRotCen, Vector3d.Z_AX(), dRotAngDeg)
      ' Ne calcolo la distanza dal centro della tavola
      Dim b3Tab As New BBox3d
      EgtGetTableArea(1, b3Tab)
      Dim dDist As Double = Point3d.DistXY(ptRef, b3Tab.Center())
      Return dDist
   End Function

   Private Function TestVacuumCups(nCups() As Integer, nRawRegId As Integer,
                                   vtMove As Vector3d, ptRotCen As Point3d, dRotAngDeg As Double) As Boolean
      ' Eseguo verifica delle ventose rispetto al grezzo
      Dim bVacOk As Boolean = True
      For nJ As Integer = 0 To nCups.Length() - 1
         Dim nCupId = nCups(nJ)
         ' Eseguo rototraslazione delle ventose per verificare se sono contenute nel grezzo
         EgtMove(nCupId, vtMove, GDB_RT.GLOB)
         EgtRotate(nCupId, ptRotCen, Vector3d.Z_AX(), dRotAngDeg, GDB_RT.GLOB)
         ' Confronto le regioni
         If EgtSurfFrChunkSimpleClassify(nRawRegId, 0, nCupId, 0) <> REGC.IN2 Then bVacOk = False
         ' Annullo rototraslazione
         EgtRotate(nCupId, ptRotCen, Vector3d.Z_AX(), -dRotAngDeg, GDB_RT.GLOB)
         EgtMove(nCupId, -vtMove, GDB_RT.GLOB)
         ' Se verifica fallita, esco dal ciclo
         If Not bVacOk Then Exit For
      Next
      Return bVacOk
   End Function

   Friend Function SaveMoveInfoInDisposition(nDispId As Integer, rmData As RawMoveData) As Boolean
      ' Verifico DispId
      If EgtGetOperationType(nDispId) <> MCH_OY.DISP Then Return False
      ' Creo il gruppo
      Dim nRpmId As Integer = EgtCreateGroup(nDispId)
      If nRpmId = GDB_ID.NULL Then Return False
      EgtSetName(nRpmId, "Rpm" & rmData.m_nId.ToString())
      ' Assegno le informazioni
      Dim sInfo As String = String.Empty
      sInfo = rmData.m_nId.ToString()
      EgtSetInfo(nRpmId, "Id", sInfo)
      sInfo = DoubleToString(rmData.m_vtMove.x, 4) & "," &
              DoubleToString(rmData.m_vtMove.y, 4) & "," &
              DoubleToString(rmData.m_vtMove.z, 4)
      EgtSetInfo(nRpmId, "Mv", sInfo)
      sInfo = DoubleToString(rmData.m_vtDelta.x, 4) & "," &
              DoubleToString(rmData.m_vtDelta.y, 4) & "," &
              DoubleToString(rmData.m_vtDelta.z, 4)
      EgtSetInfo(nRpmId, "Dt", sInfo)
      sInfo = DoubleToString(rmData.m_dAngRotDeg, 4)
      EgtSetInfo(nRpmId, "Ad", sInfo)
      sInfo = rmData.m_sCups
      EgtSetInfo(nRpmId, "Vc", sInfo)
      sInfo = GetVacuumType().ToString()
      EgtSetInfo(nRpmId, "Vt", sInfo)
      Return True
   End Function

   Friend Function SaveRemoveByHandInDisposition(nDispId As Integer, bRemoveByHand As Boolean) As Boolean
      ' Verifico DispId
      If EgtGetOperationType(nDispId) <> MCH_OY.DISP Then Return False
      ' Imposto flag
      EgtSetInfo(nDispId, "Rbh", bRemoveByHand)
      Return True
   End Function

End Module