Imports System.IO.Ports
Imports System.Text
Imports System.IO
Imports System.IO.IsolatedStorage
Imports System.Reflection
Imports System.Xml
Imports System.Xml.Serialization

Public Class Main

    Const FOC_FREQUENCY As Single = 20                  '[kHz] 
    Const USERSTORE_TOOLDIRECTORY As String = "NXP FOC tool"    'Directoryname for settingsfile in userstore
    Const USERSTORE_TOOLCONFIGFILE As String = "settings.xml"   'Filename for settings in userstore

    Private scopeCollection As New List(Of ScopeForm)   ' Collection of scope forms
    Private logForm As DataLogForm                      ' From displaying the logging options
    Private logStream As StreamWriter                   ' Used for logging data
    Private doImport As Boolean = True                  ' When COM connected, import settings from the demoboard 
    Private doExport As Boolean = False                 ' When COM connected, export settings to the demoboard  
    Private myParam As New MotorParam                   ' XML serializable class containing the parameters of the motor
    Private myProgramSettings As New ProgramSettings    ' XML serializable class containing the settings of the program
    Private dataFields As String()                      ' String containing all datafileds
    Private boardID(1) As Integer                       ' Board ID
    Private boardIDRead As Boolean = False              ' Boolean if a board ID has been read after startup
    Private suspendIDCheck As Boolean = False           ' Suspend ID check temporarily
    Private writeQueue As New Queue(Of Byte())          ' Queue of parameters needed to be written to the board
    Private wrongIDcnt As Integer = 0                   ' Filter uart exceptions by requirement for multiple wrong ID's 
    Private buttonCOMIgnoreOnce As Boolean = False

    Private serialBuffReady As Boolean = False
    Private syncBytes() As Boolean = {False, False, False, False}
    Private buffSize As Integer
    Private buff(2) As Byte

    Public boardVar As New BoardVarStruct

#Region "Form and UI Events"

    Private Sub Form1_Load(ByVal sender As Object, ByVal e As System.EventArgs) Handles Me.Load
        If Not ReadSettings() Then
            DefaultSettings()
            SaveSettings()
        End If

        If Not String.IsNullOrEmpty(myProgramSettings.PortName) Then SerialPort1.PortName = myProgramSettings.PortName

        SVPWMControl1.SetVectorXY(75 / 2, 0)

        boardVar.deviceID = New Integer(1) {}
        boardVar.adcPoint = New Integer(2) {}
        boardVar.adcVal = New Integer(2) {}
        boardVar.adcSeq = New Integer(2) {}
        boardVar.pwm = New Integer(2) {}
        boardVar.mACT = New Integer(5) {}
        boardVar.mDACT = New Integer(5) {}

        Dim vectorList1 As New List(Of VectorViewer.Vector)
        vectorList1.Add(New VectorViewer.Vector(0, 0, 0, 0, Pens.Red))
        vectorList1.Add(New VectorViewer.Vector(0, 0, 0, 0, Pens.Blue))
        vectorList1.Add(New VectorViewer.Vector(0, 0, 0, 0, Pens.Purple))
        VectorViewer1.Vectors = vectorList1.ToArray

        'Dim vectorList2 As New List(Of VectorViewer.Vector)
        'vectorList2.Add(New VectorViewer.Vector(0, 0, 0, 0, Pens.Red))
        'vectorList2.Add(New VectorViewer.Vector(0, 0, 0, 0, Pens.Blue))
        'vectorList2.Add(New VectorViewer.Vector(0, 0, 0, 0, Pens.Purple))
        'VectorViewer2.Vectors = vectorList2.ToArray

        EnumerateDataFields()

        LabelPeriodTime.Text = "Control period (T) = " & 1000 / FOC_FREQUENCY & " us"

        EnableTabs(False)

        RecursiveLabelTextErase(Me)

        If myProgramSettings.ScopeSettings IsNot Nothing Then
            For Each sc As ScopeSettings In myProgramSettings.ScopeSettings
                CreateScope(sc)
            Next
        End If
    End Sub


    Public Sub RecursiveLabelTextErase(ByVal parent As Control)
        For Each child As Control In parent.Controls
            If child.Controls.Count > 0 Then RecursiveLabelTextErase(child)
            If TypeOf child Is Label AndAlso child.Text = "value" Then child.Text = String.Empty
        Next
    End Sub

    Private Sub Main_FormClosing(ByVal sender As Object, ByVal e As System.Windows.Forms.FormClosingEventArgs) Handles Me.FormClosing
        Dim settingsList As New List(Of ScopeSettings)

        For Each scope As ScopeForm In scopeCollection
            settingsList.Add(scope.GetSettings)

            If Not scope Is Nothing AndAlso Not scope.IsDisposed Then
                scope.ScrollControl1.StopGraph()
                scope.ScrollControl1.Dispose()
            End If
        Next

        myProgramSettings.ScopeSettings = settingsList.ToArray

        If myProgramSettings.MotorParamFilePath IsNot Nothing AndAlso _
            File.Exists(myProgramSettings.MotorParamFilePath) AndAlso _
            myProgramSettings.MotorParamChanged Then

            Dim result As DialogResult = MessageBox.Show("Save motor changes?", "", MessageBoxButtons.YesNo, MessageBoxIcon.Question)
            If result = Windows.Forms.DialogResult.Yes Then
                Try
                    Using fs As New FileStream(myProgramSettings.MotorParamFilePath, FileMode.Open)
                        SaveParametersToStream(fs)
                        myProgramSettings.MotorParamChanged = False
                    End Using
                Catch ex As Exception
                    MessageBox.Show("Cannot save file." & vbNewLine & ex.Message)
                End Try
            End If
        End If

        SaveSettings()

        Try
            If Me.SerialPort1.IsOpen Then Me.SerialPort1.Close()
        Catch ex As Exception
            ' Do nothing 
        End Try
    End Sub

    Private Sub ButtonCOM_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles ButtonCOM.Click
        If buttonCOMIgnoreOnce Then
            buttonCOMIgnoreOnce = False
            Exit Sub
        End If

        If Not SerialPort1.IsOpen Then
            If Not OpenCom() Then
                buttonCOMIgnoreOnce = True
                EnableTabs(False)
            Else
                EnableTabs(True)
            End If

        Else
            CloseCom()
            EnableTabs(False)
        End If
    End Sub

    Public Sub EnableTabs(ByVal value As Boolean)
        For Each t As TabPage In TabControlFOC.TabPages
            t.Enabled = value
        Next
    End Sub

    Private Sub Vector_NewValue(ByVal sender As Object, ByVal e As SVPWMControl.NewValueEventArgs) Handles SVPWMControl1.NewValue
        Const PWM_MAX As Single = 100
        LabelA.Text = String.Concat("A: ", FormatNumber((e.A2 - e.A1) * PWM_MAX, 1), " %")
        LabelB.Text = String.Concat("B: ", FormatNumber((e.B2 - e.B1) * PWM_MAX, 1), " %")
        LabelC.Text = String.Concat("C: ", FormatNumber((e.C2 - e.C1) * PWM_MAX, 1), " %")
        PWM1_virtual.X1 = e.A1
        PWM1_virtual.X2 = e.A2
        PWM2_virtual.X1 = e.B1
        PWM2_virtual.X2 = e.B2
        PWM3_virtual.X1 = e.C1
        PWM3_virtual.X2 = e.C2

        PWM1_virtual.ADCpoint1 = e.ADCPoint1
        PWM2_virtual.ADCpoint1 = e.ADCPoint1
        PWM3_virtual.ADCpoint1 = e.ADCPoint1
        PWM1_virtual.ADCpoint2 = e.ADCPoint2
        PWM2_virtual.ADCpoint2 = e.ADCPoint2
        PWM3_virtual.ADCpoint2 = e.ADCPoint2

        If CheckBoxTransmit.Checked Then
            Const DEADT As Integer = 20
            Dim offset As Integer = 1
            Dim tx(24) As Byte
            tx(0) = Asc("M"c)

            'ACT
            CopyWORDToByteArray(e.A1 * 6000 + DEADT, tx, offset)
            CopyWORDToByteArray(e.A1 * 6000 - DEADT, tx, offset)
            CopyWORDToByteArray(e.B1 * 6000 + DEADT, tx, offset)
            CopyWORDToByteArray(e.B1 * 6000 - DEADT, tx, offset)
            CopyWORDToByteArray(e.C1 * 6000 + DEADT, tx, offset)
            CopyWORDToByteArray(e.C1 * 6000 - DEADT, tx, offset)

            'DACT
            CopyWORDToByteArray(e.A2 * 6000 - DEADT, tx, offset)
            CopyWORDToByteArray(e.A2 * 6000 + DEADT, tx, offset)
            CopyWORDToByteArray(e.B2 * 6000 - DEADT, tx, offset)
            CopyWORDToByteArray(e.B2 * 6000 + DEADT, tx, offset)
            CopyWORDToByteArray(e.C2 * 6000 - DEADT, tx, offset)
            CopyWORDToByteArray(e.C2 * 6000 + DEADT, tx, offset)

            If SerialPort1.IsOpen Then SerialPort1.Write(tx, 0, 25)
        End If
    End Sub

    Private Sub RadioButtonQEI_CheckedChanged(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles RadioButtonQEI.CheckedChanged
        If RadioButtonQEI.Checked Then : EnqueueWriteParam(Param.Mode, New Byte() {OperationMode.MODE_QEI})
        Else : EnqueueWriteParam(Param.Mode, New Byte() {OperationMode.MODE_SL})
        End If
    End Sub

    Private Sub MenuItemOpen_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MenuItemOpen.Click
        Using ofd As New OpenFileDialog()
            ofd.InitialDirectory = Microsoft.VisualBasic.FileIO.SpecialDirectories.Desktop
            ofd.Filter = "Motor file (*.xml)|*.xml|All files (*.*)|*.*"
            ofd.FilterIndex = 1
            ofd.RestoreDirectory = True
            If ofd.ShowDialog() = System.Windows.Forms.DialogResult.OK Then
                Using myStream As Stream = ofd.OpenFile()
                    Try
                        If (myStream IsNot Nothing) Then
                            LoadParametersFromStream(myStream)
                            myProgramSettings.MotorParamFilePath = ofd.FileName
                        End If
                    Catch Ex As Exception
                        MessageBox.Show("Cannot read file." & vbNewLine & Ex.Message)
                    Finally
                        ' Check this again, since we need to make sure we didn't throw an exception on open.
                        If (myStream IsNot Nothing) Then
                            myStream.Close()
                        End If
                    End Try
                End Using
            End If
        End Using
    End Sub

    Private Sub MenuItemSave_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MenuItemSave.Click
        SaveMotorParameter()
    End Sub

    Private Sub MenuItemSettings_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MenuItemSettings.Click
        If SerialPort1.IsOpen Then
            CloseCom()
        End If
        Using f As New SettingsForm(myProgramSettings)
            If f.ShowDialog() = Windows.Forms.DialogResult.Yes Then
                Me.SerialPort1.PortName = myProgramSettings.PortName
                SaveSettings()
            End If
        End Using
        If SerialPort1.IsOpen Then
            OpenCom()
        End If
    End Sub

    Private Sub MenuItemExit_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MenuItemExit.Click
        Me.Close()
    End Sub

    Private Sub ButtonScope_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles ButtonScope.Click
        AddScope()
    End Sub

    Private Sub AddScopeToolStripMenuItem_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles AddScopeToolStripMenuItem.Click
        AddScope()
    End Sub

    Private Sub DataloggerToolStripMenuItem_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles DataloggerToolStripMenuItem.Click
        ShowDatalogger()
    End Sub

    Private Sub ButtonLog_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles ButtonLog.Click
        ShowDatalogger()
    End Sub

    Private Sub FixedPointToolToolStripMenuItem_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles FixedPointToolToolStripMenuItem.Click
        Dim fpForm As New FixedPointForm
        fpForm.Show()
    End Sub

    Private Sub AboutToolStripMenuItem_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles AboutToolStripMenuItem.Click
        Dim about As New AboutForm
        AboutForm.ShowDialog()
    End Sub

#End Region

#Region "TabQDPIEvents"

    Private Sub RadioButtonAutoSpeed_CheckedChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles RadioButtonAutoSpeed.CheckedChanged
        TrackBarQ_SP.Enabled = Not RadioButtonAutoSpeed.Checked
        EnqueueWriteParam(Param.SpeedControlEnabled, New Byte() {RadioButtonAutoSpeed.Checked})
    End Sub

    Private Sub TrackBarQ_SP_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarQ_SP.ValueChanged
        EnqueueWriteParam(Param.QSP, WORDToBytes(FromSingleToFixedPointConst(TrackBarQ_SP.Value / 10, 6, 10)))
    End Sub

    Private Sub TrackBarD_SP_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarD_SP.ValueChanged
        EnqueueWriteParam(Param.DSP, WORDToBytes(FromSingleToFixedPointConst(TrackBarD_SP.Value / 10, 6, 10)))
    End Sub

    Private Sub CheckBoxSync_CheckedChanged(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles CheckBoxSync.CheckedChanged
        If CheckBoxSync.Checked Then CheckBoxTransmit.Checked = False
        CheckBoxAutoRotate.Enabled = Not CheckBoxSync.Checked
        CheckBoxTransmit.Enabled = Not CheckBoxSync.Checked
    End Sub

    Private Sub CheckBoxTransmit_CheckedChanged(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles CheckBoxTransmit.CheckedChanged
        If CheckBoxTransmit.Checked Then
            If Not MessageBox.Show("Are you sure you want to manually override the PWM outputs of the demoboard?" & vbNewLine & _
                                   "A current limited power supply is recommended for large voltage vectors.", _
                                   "Manual overide demoboard PWM output?", MessageBoxButtons.OKCancel, MessageBoxIcon.Exclamation) = Windows.Forms.DialogResult.OK Then
                CheckBoxTransmit.Checked = False
                Exit Sub
            Else
                EnqueueWriteParam(Param.FOCEnabled, New Byte() {False})
            End If
        Else
            EnqueueWriteParam(Param.FOCEnabled, New Byte() {True})
        End If
    End Sub


    Private Sub TrackBarQKp_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarQKp.ValueChanged
        EnqueueWriteParam(Param.QKp, WORDToBytes(FromSingleToFixedPointConst(TrackBarQKp.Value / 100, 6, 10)))
    End Sub

    Private Sub TrackBarQKi_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarQKi.ValueChanged
        EnqueueWriteParam(Param.QKi, WORDToBytes(FromSingleToFixedPointConst(TrackBarQKi.Value / 100, 6, 10)))
    End Sub

    Private Sub TrackBarDKp_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarDKp.ValueChanged
        EnqueueWriteParam(Param.DKp, WORDToBytes(FromSingleToFixedPointConst(TrackBarDKp.Value / 100, 6, 10)))
    End Sub

    Private Sub TrackBarDKi_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarDKi.ValueChanged
        EnqueueWriteParam(Param.DKi, WORDToBytes(FromSingleToFixedPointConst(TrackBarDKi.Value / 100, 6, 10)))
    End Sub

#End Region

#Region "TabSpeedPIDEvents"

    Private Sub TrackBarSpeedSP_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarSpeedSP.ValueChanged
        EnqueueWriteParam(Param.SpeedSP, WORDToBytes(FromSingleToFixedPointConst(TrackBarSpeedSP.Value / 100, 6, 10)))
    End Sub

    Private Sub TrackBarSpeedKp_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarSpeedKp.ValueChanged
        EnqueueWriteParam(Param.SpeedKp, WORDToBytes(FromSingleToFixedPointConst(TrackBarSpeedKp.Value / 1000, 6, 10)))
    End Sub

    Private Sub TrackBarSpeedKi_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarSpeedKi.ValueChanged
        EnqueueWriteParam(Param.SpeedKi, WORDToBytes(FromSingleToFixedPointConst(TrackBarSpeedKi.Value / 1000, 6, 10)))
    End Sub

    Private Sub TrackBarSpeedKd_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarSpeedKd.ValueChanged
        EnqueueWriteParam(Param.SpeedKd, WORDToBytes(FromSingleToFixedPointConst(TrackBarSpeedKd.Value / 1000, 6, 10)))
    End Sub

#End Region

#Region "TabVectorControlEvents"

    Private Sub VectorControlTimer_Tick(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles VectorControlTimer.Tick
        If CheckBoxAutoRotate.Checked Then
            SVPWMControl1.Alpha += 1
        End If
    End Sub

#End Region

#Region "TabParametersEvents"

    Private Sub NumericTextBoxKSlide_TextChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TextBoxSMCgain.TextChanged
        If Not IsNumeric(TextBoxSMCgain.Text) Then Exit Sub
        If doImport Then Exit Sub
        EnqueueWriteParam(Param.kSlide, DWORDToBytes(FromSingleToFixedPointConst(TextBoxSMCgain.Text, 16, 16)))
    End Sub

    Private Sub NumericTextBoxMaxError_TextChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TextBoxSMCMaxError.TextChanged
        If Not IsNumeric(TextBoxSMCMaxError.Text) Then Exit Sub
        If doImport Then Exit Sub
        EnqueueWriteParam(Param.maxError, DWORDToBytes(FromSingleToFixedPointConst(TextBoxSMCMaxError.Text, 16, 16)))
    End Sub

    Private Sub NumericTextBoxOmegaFltrCoef_TextChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TextBoxOmegaFltrCoef.TextChanged
        If Not IsNumeric(TextBoxOmegaFltrCoef.Text) Then Exit Sub
        If doImport Then Exit Sub
        EnqueueWriteParam(Param.omegaFltrCoef, DWORDToBytes(FromSingleToFixedPointConst(TextBoxOmegaFltrCoef.Text, 16, 16)))
    End Sub

    Private Sub TextBoxmOhm_TextChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TextBoxmOhm.TextChanged
        CalcAndSetFG()
    End Sub

    Private Sub TextBoxmH_TextChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TextBoxmH.TextChanged
        CalcAndSetFG()
    End Sub

    Private Sub ButtonDivR_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles ButtonDivR.Click
        If IsNumeric(TextBoxmOhm.Text) Then TextBoxmOhm.Text /= 2
    End Sub

    Private Sub ButtonDivL_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles ButtonDivL.Click
        If IsNumeric(TextBoxmH.Text) Then TextBoxmH.Text /= 2
    End Sub

    Private Sub CalcAndSetFG()
        If Not IsNumeric(TextBoxmOhm.Text) Then Exit Sub
        If Not IsNumeric(TextBoxmH.Text) Then Exit Sub

        LabelF.Text = 1 - (1 / (1000 * FOC_FREQUENCY)) * (TextBoxmOhm.Text / TextBoxmH.Text)
        LabelG.Text = (1 / (1000 * FOC_FREQUENCY)) / (TextBoxmH.Text / 1000)
        If doImport Then Exit Sub
        EnqueueWriteParam(Param.Fsmo, DWORDToBytes(FromSingleToFixedPointConst(LabelF.Text, 16, 16)))
        EnqueueWriteParam(Param.Gsmo, DWORDToBytes(FromSingleToFixedPointConst(LabelG.Text, 16, 16)))
    End Sub

    Private Sub TrackBarTest1_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarTest1.ValueChanged
        EnqueueWriteParam(Param.Test1, WORDToBytes(FromSingleToFixedPointConst(TrackBarTest1.Value / 100, 6, 10)))
    End Sub

    Private Sub TrackBarTest2_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarTest2.ValueChanged
        EnqueueWriteParam(Param.Test2, WORDToBytes(FromSingleToFixedPointConst(TrackBarTest2.Value / 100, 6, 10)))
    End Sub

    Private Sub TrackBarTest3_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarTest3.ValueChanged
        EnqueueWriteParam(Param.Test3, WORDToBytes(FromSingleToFixedPointConst(TrackBarTest3.Value / 100, 6, 10)))
    End Sub

    Private Sub TrackBarTest4_ValueChanged(ByVal sender As Object, ByVal e As System.EventArgs) Handles TrackBarTest4.ValueChanged
        EnqueueWriteParam(Param.Test4, WORDToBytes(FromSingleToFixedPointConst(TrackBarTest4.Value / 100, 6, 10)))
    End Sub

#End Region

#Region "COM"

    Private Function OpenCom() As Boolean
        If String.IsNullOrEmpty(myProgramSettings.PortName) Then
            MessageBox.Show("Configure COM settings in the settings form under Edit->Settings", "Configure COM settings", MessageBoxButtons.OK, MessageBoxIcon.Exclamation)
            Return False
        End If

        If Not SerialPort1.IsOpen Then
            Try
                SerialPort1.Open()
                ButtonCOM.BackgroundImage = My.Resources.NoSerial
                For Each scope As ScopeForm In scopeCollection
                    If Not scope Is Nothing AndAlso Not scope.IsDisposed Then
                        scope.ScrollControl1.Start()
                    End If
                Next
                If logForm IsNot Nothing AndAlso Not logForm.IsDisposed Then
                    logForm.ButtonStart.Enabled = True
                End If
            Catch ex As Exception
                MessageBox.Show(ex.Message, "COM error", MessageBoxButtons.OK, MessageBoxIcon.Exclamation)
                Return False
            End Try
        End If
        Return True
    End Function

    Private Sub CloseCom()
        If Me.SerialPort1.IsOpen Then
            Try
                Me.SerialPort1.DiscardInBuffer()
                Me.SerialPort1.Close()
            Catch ex As Exception
                MessageBox.Show(ex.Message, "COM error", MessageBoxButtons.OK, MessageBoxIcon.Exclamation)
            End Try
        End If

        For Each scope As ScopeForm In scopeCollection
            If Not scope Is Nothing AndAlso Not scope.IsDisposed Then
                scope.ScrollControl1.StopGraph()
            End If
        Next
        If logForm IsNot Nothing AndAlso Not logForm.IsDisposed Then
            logForm.ButtonStart.Enabled = False
        End If

        ButtonCOM.BackgroundImage = My.Resources.Serial
    End Sub

    Private Sub DataReceived() Handles SerialPort1.DataReceived
        Const SYNCBYTE0 As Byte = 241 '239,241,17,19
        Const SYNCBYTE1 As Byte = 239
        Const SYNCBYTE2 As Byte = 17
        Const SYNCBYTE3 As Byte = 19

        Try
            While SerialPort1.BytesToRead
                If Not syncBytes(0) Then
                    Do
                        Dim b As Byte = SerialPort1.ReadByte
                        If b = SYNCBYTE0 Then syncBytes(0) = True
                    Loop While SerialPort1.BytesToRead And Not syncBytes(0)         ' 1 sync byte detected
                ElseIf syncBytes(0) And Not syncBytes(1) Then
                    If SerialPort1.ReadByte = SYNCBYTE1 Then : syncBytes(1) = True  ' 2 sync bytes detected
                    Else : syncBytes(0) = False                                     ' Not in sync
                    End If
                ElseIf syncBytes(0) And syncBytes(1) And Not syncBytes(2) Then
                    If SerialPort1.ReadByte = SYNCBYTE2 Then : syncBytes(2) = True  ' 3 sync bytes detected
                    Else : syncBytes(0) = False : syncBytes(1) = False              ' Not in sync
                    End If
                ElseIf syncBytes(0) And syncBytes(1) And syncBytes(2) And Not syncBytes(3) Then
                    If SerialPort1.ReadByte = SYNCBYTE3 Then : syncBytes(3) = True  ' 4 sync bytes detected
                    Else : syncBytes(0) = False : syncBytes(1) = False : syncBytes(2) = False
                    End If
                ElseIf syncBytes(0) And syncBytes(1) And syncBytes(2) And syncBytes(3) And Not serialBuffReady Then
                    If SerialPort1.BytesToRead > 2 Then
                        Dim lo As Byte = SerialPort1.ReadByte
                        Dim hi As Byte = SerialPort1.ReadByte
                        buffSize = hi * 256 + lo - 6
                        If buff.Length <> buffSize Then
                            ReDim buff(buffSize - 1)
                            For i = 0 To 3 : syncBytes(i) = False : Next
                            SerialPort1.ReceivedBytesThreshold = buffSize + 6
                        Else
                            serialBuffReady = True
                        End If
                    End If
                ElseIf serialBuffReady And SerialPort1.BytesToRead >= buffSize Then
                    SerialPort1.Read(buff, 0, buffSize)
                    Try
                        Me.BeginInvoke(New ProcessDataDelegate(AddressOf ProcessData), buff)
                    Catch ex As Exception
                        ' Do nothing 
                    End Try
                    For i = 0 To 3 : syncBytes(i) = False : Next
                    serialBuffReady = False
                End If
            End While
        Catch ex As Exception
            ' Do nothing
        End Try
    End Sub

#End Region

#Region "Public subs and methods"

    Private Delegate Sub ProcessDataDelegate(ByRef buff() As Byte)
    Private Sub ProcessData(ByRef buff() As Byte)

        Const filterCoefficient As Single = 0.01
        Static Power As Single

        Try
            ExtractDataToFOCStruct(boardVar, buff)
            CheckBoardID()
            If doImport Then ImportParam()
            If doExport Then EnqueExportParam()
            If Not boardVar.uartBusy AndAlso writeQueue.Count > 0 Then SerialPort1.Write(writeQueue.Dequeue, 0, 5)

            If logForm IsNot Nothing AndAlso Not logForm.IsDisposed AndAlso logForm.DoLogData Then LogData()

            For Each scope As ScopeForm In scopeCollection
                If scope IsNot Nothing AndAlso Not scope.IsDisposed Then
                    If scope.ListBoxSignal2.SelectedItem = "No signal" Then
                        scope.ScrollControl1.AddValue( _
                            boardVar.GetType.GetField(scope.ListBoxSignal1.SelectedItem).GetValue(boardVar))
                    Else
                        scope.ScrollControl1.AddValue( _
                            boardVar.GetType.GetField(scope.ListBoxSignal1.SelectedItem).GetValue(boardVar), _
                            boardVar.GetType.GetField(scope.ListBoxSignal2.SelectedItem).GetValue(boardVar))
                    End If
                End If
            Next

            If boardVar.mode = OperationMode.MODE_QEI_RAMP And LabelMode.Text <> "QEI ramp" Then
                LabelMode.Text = "QEI ramp"
            ElseIf boardVar.mode = OperationMode.MODE_SL_RAMP And LabelMode.Text <> "Sensorless ramp" Then
                LabelMode.Text = "Sensorless ramp"
            ElseIf boardVar.mode = OperationMode.MODE_QEI And LabelMode.Text <> "QEI" Then
                LabelMode.Text = "QEI"
            ElseIf boardVar.mode = OperationMode.MODE_HALL And LabelMode.Text <> "Hall" Then
                LabelMode.Text = "Hall"
            ElseIf boardVar.mode = OperationMode.MODE_SL And LabelMode.Text <> "Sensorless" Then
                LabelMode.Text = "Sensorless"
            End If

            If TabControlFOC.SelectedTab.Equals(TabQDPI) Then
                ProgressControl1.Value = boardVar.Q_err
                ProgressControl2.Value = boardVar.D_err
                ProgressControl3.Value = boardVar.Q_int
                ProgressControl4.Value = boardVar.D_int

                LabelQKp.Text = boardVar.Q_Kp
                LabelQKi.Text = boardVar.Q_Ki
                LabelDKp.Text = boardVar.D_Kp
                LabelDKi.Text = boardVar.D_Ki

                'FOC.Valpha = FOC.Vd * cos(FOC.alpha) - FOC.Vq * sin(FOC.alpha);  
                'FOC.Vbeta = FOC.Vd * sin(FOC.alpha) + FOC.Vq * cos(FOC.alpha);
                VectorViewer1.Vectors(0).X2 = boardVar.Ialpha   ' Red
                VectorViewer1.Vectors(0).Y2 = boardVar.Ibeta    ' Red
                VectorViewer1.Vectors(1).X2 = boardVar.Id       ' Blue
                VectorViewer1.Vectors(1).Y2 = boardVar.Iq       ' Blue
                VectorViewer1.Vectors(2).X2 = boardVar.Vd       ' Purple 
                VectorViewer1.Vectors(2).Y2 = boardVar.Vq       ' Purple
                VectorViewer1.Alpha = Deg2Rad(boardVar.alpha * 9 / 50)
                VectorViewer1.Invalidate()

                'VectorViewer2.Vectors(2).X2 = D
                'VectorViewer2.Vectors(2).Y2 = Q

                'VectorViewer2.Invalidate()

                'If FOC.Id = 0 Then FOC.Id = 0.00001
                'If FOC.Vd = 0 Then FOC.Vd = 0.00001

                'VectorViewer2.Alpha = VectorViewer1.Alpha ' Deg2Rad(BoardVar.alpha * 9 / 50)
                'Dim _x1 As Single = 0
                'Dim _y1 As Single = 0
                'Dim _x2 As Single = BoardVar.Vr1 * 10
                'Dim _y2 As Single = 0
                'VectorViewer2.Vectors(0).X1 = _x1
                'VectorViewer2.Vectors(0).Y1 = _y1
                'VectorViewer2.Vectors(0).X2 = _x2
                'VectorViewer2.Vectors(0).Y2 = _y2
                '_x1 = _x2
                '_y1 = _y2
                '_x2 = _x1 + BoardVar.Vr2 * 10 * Math.Cos(Deg2Rad(120))
                '_y2 = _y1 - BoardVar.Vr2 * 10 * Math.Sin(Deg2Rad(120))
                'VectorViewer2.Vectors(1).X1 = _x1
                'VectorViewer2.Vectors(1).Y1 = _y1
                'VectorViewer2.Vectors(1).X2 = _x2
                'VectorViewer2.Vectors(1).Y2 = _y2
                '_x1 = _x2
                '_y1 = _y2
                '_x2 = _x1 + BoardVar.Vr3 * 10 * Math.Cos(Deg2Rad(240))
                '_y2 = _y1 - BoardVar.Vr3 * 10 * Math.Sin(Deg2Rad(240))
                'VectorViewer2.Vectors(2).X1 = _x1
                'VectorViewer2.Vectors(2).Y1 = _y1
                'VectorViewer2.Vectors(2).X2 = _x2
                'VectorViewer2.Vectors(2).Y2 = _y2
                'VectorViewer2.Invalidate()

                'mean1(cnt) = FOC.adcVal(0)
                'mean2(cnt) = FOC.adcVal(1)
                'cnt += 1
                'If cnt = 10 Then cnt = 0

                'Dim av1, av2 As Single
                'For cnt2 As Integer = 0 To 9
                '    av1 += mean1(cnt2)
                '    av2 += mean2(cnt2)
                'Next

                LabelQ_SP.Text = boardVar.Q_SP
                LabelD_SP.Text = boardVar.D_SP

            ElseIf TabControlFOC.SelectedTab.Equals(TabSpeedPID) Then
                LabelSpeedKp.Text = boardVar.SpeedKp
                LabelSpeedKi.Text = boardVar.SpeedKi
                LabelSpeedKd.Text = boardVar.SpeedKd
                ProgressControlSpeedErr.Value = boardVar.speed_err
                ProgressControlSpeedInt.Value = boardVar.speed_int
                ProgressControlSpeedDer.Value = boardVar.speed_der
                ValueLabelSpeedSP.Text = boardVar.SpeedSP * 1024
            ElseIf TabControlFOC.SelectedTab.Equals(TabVectorConrol) Then
                If CheckBoxSync.Checked Then SVPWMControl1.Alpha = boardVar.alpha * 9 / 50

                PWM1_actual.X1 = CSng(boardVar.mACT(0)) / 6000
                PWM2_actual.X1 = CSng(boardVar.mACT(2)) / 6000
                PWM3_actual.X1 = CSng(boardVar.mACT(4)) / 6000
                PWM1_actual.X2 = CSng(boardVar.mDACT(0)) / 6000
                PWM2_actual.X2 = CSng(boardVar.mDACT(2)) / 6000
                PWM3_actual.X2 = CSng(boardVar.mDACT(4)) / 6000
                PWM1_actual.ADCpoint1 = CSng(boardVar.adcPoint(0)) / 6000
                PWM1_actual.ADCpoint2 = CSng(boardVar.adcPoint(1)) / 6000
                PWM2_actual.ADCpoint1 = CSng(boardVar.adcPoint(0)) / 6000
                PWM2_actual.ADCpoint2 = CSng(boardVar.adcPoint(1)) / 6000
                PWM3_actual.ADCpoint1 = CSng(boardVar.adcPoint(0)) / 6000
                PWM3_actual.ADCpoint2 = CSng(boardVar.adcPoint(1)) / 6000

            ElseIf TabControlFOC.SelectedTab.Equals(TabParameters) Then
                'LabelTest1.Text = BoardVar.test1
                'LabelTest2.Text = BoardVar.test2
                'LabelTest3.Text = BoardVar.test3
                'LabelTest4.Text = BoardVar.test4

                Dim sb As New StringBuilder
                sb.AppendLine("F16_16_Valpha  = " & boardVar.F16_16_Valpha)
                sb.AppendLine("F16_16_Vbeta   = " & boardVar.F16_16_Vbeta)
                sb.AppendLine("F16_16_Ialpha  = " & boardVar.F16_16_Ialpha)
                sb.AppendLine("F16_16_Ibeta   = " & boardVar.F16_16_Ibeta)
                sb.AppendLine("bemf_alpha     = " & boardVar.bemf_alpha)
                sb.AppendLine("bemf_alpha_flt = " & boardVar.bemf_alpha_flt)
                sb.AppendLine("bemf_beta      = " & boardVar.bemf_beta)
                sb.AppendLine("bemf_beta_flt  = " & boardVar.bemf_beta_flt)
                sb.AppendLine("z_alpha        = " & boardVar.z_alpha)
                sb.AppendLine("z_beta         = " & boardVar.z_beta)
                sb.AppendLine("Ialpha_est     = " & boardVar.Ialpha_est)
                sb.AppendLine("Ibeta_est      = " & boardVar.Ibeta_est)
                sb.AppendLine("Ialpha_err     = " & boardVar.Ialpha_err)
                sb.AppendLine("Ibeta_err      = " & boardVar.Ibeta_err)
                sb.AppendLine("K_smc          = " & boardVar.K_smc)
                sb.AppendLine("smc_max_err    = " & boardVar.smc_max_err)
                sb.AppendLine("G              = " & boardVar.G)
                sb.AppendLine("F              = " & boardVar.F)
                sb.AppendLine("Cflt_smc       = " & boardVar.Cflt_smc)
                sb.AppendLine("Cflt_omega     = " & boardVar.Cflt_omega)
                sb.AppendLine("omega          = " & boardVar.omega)
                sb.AppendLine("omega_flt      = " & boardVar.omega_flt)
                sb.AppendLine("theta          = " & boardVar.theta)
                sb.AppendLine("theta_offset   = " & boardVar.theta_offset)
                TextBox1.Text = sb.ToString
            End If

            LabelTemp.Text = boardVar.temp & "°C"
            LabelRPM.Text = boardVar.speed * 1024
            Dim newPower As Single = (Math.Abs(boardVar.Ia) + Math.Abs(boardVar.Ib) + Math.Abs(boardVar.Ic))
            Power = Power + filterCoefficient * newPower - filterCoefficient * Power
            LabelPower.Text = FormatNumber(Power / 6.9, 2) & " Amp"
        Catch ex As Exception
            ' Do nothing
        End Try
    End Sub

    Public Sub ImportParam()
        myParam.FocEnabled = boardVar.FocEnabled
        myParam.SpeedEnabled = boardVar.speedEnabled
        myParam.Q_SP = boardVar.Q_SP
        myParam.Q_Kp = boardVar.Q_Kp
        myParam.D_Kp = boardVar.D_Kp
        myParam.Q_Ki = boardVar.Q_Ki
        myParam.D_Ki = boardVar.D_Ki
        myParam.Speed_SP = boardVar.SpeedSP
        myParam.Speed_Kp = boardVar.SpeedKp
        myParam.Speed_Ki = boardVar.SpeedKi
        myParam.Speed_Kd = boardVar.SpeedKd
        myParam.SMC_K = boardVar.K_smc
        myParam.SMC_MaxError = boardVar.smc_max_err
        myParam.CfltOmega = boardVar.Cflt_omega
        myParam.PhaseInd = ((1 / (1000 * FOC_FREQUENCY)) / boardVar.G) * 1000
        myParam.PhaseRes = (((1 - boardVar.F) * myParam.PhaseInd) / (1 / (1000 * FOC_FREQUENCY)))
        myParam.F = boardVar.F
        myParam.G = boardVar.G

        InitControlsFromParameters(myParam)
        doImport = False
    End Sub

    Public Sub EnqueExportParam()
        EnqueueWriteParam(Param.FOCEnabled, New Byte() {myParam.FocEnabled})
        EnqueueWriteParam(Param.SpeedControlEnabled, New Byte() {myParam.SpeedEnabled})
        EnqueueWriteParam(Param.QSP, WORDToBytes(FromSingleToFixedPointConst(myParam.Q_SP, 6, 10)))
        EnqueueWriteParam(Param.DSP, WORDToBytes(FromSingleToFixedPointConst(myParam.D_SP, 6, 10))) ' funny noise
        EnqueueWriteParam(Param.DKp, WORDToBytes(FromSingleToFixedPointConst(myParam.D_Kp, 6, 10)))
        EnqueueWriteParam(Param.DKi, WORDToBytes(FromSingleToFixedPointConst(myParam.D_Ki, 6, 10)))
        EnqueueWriteParam(Param.QKp, WORDToBytes(FromSingleToFixedPointConst(myParam.Q_Kp, 6, 10)))
        EnqueueWriteParam(Param.QKi, WORDToBytes(FromSingleToFixedPointConst(myParam.Q_Ki, 6, 10)))
        EnqueueWriteParam(Param.SpeedSP, WORDToBytes(FromSingleToFixedPointConst(myParam.Speed_SP, 6, 10)))
        EnqueueWriteParam(Param.SpeedKp, WORDToBytes(FromSingleToFixedPointConst(myParam.Speed_Kp, 6, 10)))
        EnqueueWriteParam(Param.SpeedKi, WORDToBytes(FromSingleToFixedPointConst(myParam.Speed_Ki, 6, 10)))
        EnqueueWriteParam(Param.SpeedKd, WORDToBytes(FromSingleToFixedPointConst(myParam.Speed_Kd, 6, 10)))
        EnqueueWriteParam(Param.Fsmo, DWORDToBytes(FromSingleToFixedPointConst(myParam.F, 16, 16)))
        EnqueueWriteParam(Param.Gsmo, DWORDToBytes(FromSingleToFixedPointConst(myParam.G, 16, 16)))

        doExport = False
    End Sub

    Public Sub InitControlsFromParameters(ByVal p As MotorParam)
        ' TwoWay controls : controls able to alter settings like Kp, Ki, Kd
        ' OneWay controls : controls that can only view data

        CheckBoxSync.Checked = p.FocEnabled
        RadioButtonAutoSpeed.Checked = p.SpeedEnabled
        RadioButtonManualSpeed.Checked = Not p.SpeedEnabled
        TrackBarQ_SP.Value = p.Q_SP
        TrackBarD_SP.Value = p.D_SP
        TrackBarQKp.Value = p.Q_Kp * 100
        TrackBarDKp.Value = p.D_Kp * 100
        TrackBarQKi.Value = p.Q_Ki * 100
        TrackBarDKi.Value = p.D_Ki * 100

        LabelQ_SP.Text = p.Q_SP
        LabelQKp.Text = p.Q_Kp
        LabelDKp.Text = p.D_Kp
        LabelQKi.Text = p.Q_Ki
        LabelDKi.Text = p.D_Ki

        TrackBarSpeedSP.Value = p.Speed_SP * 100
        TrackBarSpeedKp.Value = p.Speed_Kp * 1000
        TrackBarSpeedKi.Value = p.Speed_Ki * 1000
        TrackBarSpeedKd.Value = p.Speed_Kd * 1000

        LabelSpeedKp.Text = p.Speed_Kp
        LabelSpeedKi.Text = p.Speed_Ki
        LabelSpeedKd.Text = p.Speed_Kd

        TextBoxSMCgain.Text = p.SMC_K
        TextBoxSMCMaxError.Text = p.SMC_MaxError
        TextBoxOmegaFltrCoef.Text = p.CfltOmega

        TextBoxmOhm.Text = p.PhaseRes
        TextBoxmH.Text = p.PhaseInd
        LabelF.Text = p.F
        LabelG.Text = p.G
    End Sub

    Public Sub DeleteSettings()
        Try
            Using isf As IsolatedStorageFile = IsolatedStorageFile.GetUserStoreForAssembly()
                isf.DeleteFile(USERSTORE_TOOLDIRECTORY & Path.DirectorySeparatorChar & USERSTORE_TOOLCONFIGFILE)
                isf.DeleteDirectory(USERSTORE_TOOLDIRECTORY)
            End Using
        Catch ex As Exception
            ' Do nothing
        End Try
    End Sub

    Public Function ReadSettings() As Boolean
        Try
            ' Get the isolated store for this assembly
            Using isf As IsolatedStorageFile = IsolatedStorageFile.GetUserStoreForAssembly()
                If Not isf.GetDirectoryNames(USERSTORE_TOOLDIRECTORY).Contains(USERSTORE_TOOLDIRECTORY) Then
                    ' Create a directory at the top level of the store
                    isf.CreateDirectory(USERSTORE_TOOLDIRECTORY)
                    Return False
                End If

                If Not isf.GetFileNames(USERSTORE_TOOLDIRECTORY & Path.DirectorySeparatorChar & USERSTORE_TOOLCONFIGFILE).Contains(USERSTORE_TOOLCONFIGFILE) Then
                    ' File does not exist
                    Return False
                End If

                ' Open file in USERSTORE_TOOLDIRECTORY
                Using fs As IsolatedStorageFileStream = New IsolatedStorageFileStream( _
                                                                    USERSTORE_TOOLDIRECTORY & _
                                                                    Path.DirectorySeparatorChar & _
                                                                    USERSTORE_TOOLCONFIGFILE, _
                                                                    FileMode.Open, FileAccess.Read, isf)
                    ' Read content : deserialize the XML to an object
                    Dim xtr As XmlTextReader = New XmlTextReader(fs)
                    Dim xs As XmlSerializer = New XmlSerializer(GetType(ProgramSettings))
                    myProgramSettings = CType(xs.Deserialize(xtr), ProgramSettings)
                    fs.Close()
                End Using
                isf.Close()
            End Using
        Catch ex As Exception
            Return False
        End Try

        Return True
    End Function

    Public Sub SaveSettings()
        If Me.InvokeRequired Then
            Me.Invoke(New MethodInvoker(AddressOf SaveSettings))
            Exit Sub
        End If

        ' Get the isolated store for this assembly
        Using isf As IsolatedStorageFile = IsolatedStorageFile.GetUserStoreForAssembly() ' Open file in USERSTORE_TOOLDIRECTORY
            Using fs As IsolatedStorageFileStream = New IsolatedStorageFileStream( _
                                                                USERSTORE_TOOLDIRECTORY & _
                                                                Path.DirectorySeparatorChar & _
                                                                USERSTORE_TOOLCONFIGFILE, _
                                                                FileMode.Create, FileAccess.Write, isf)
                ' Serialize the object to the file
                Dim xs As XmlSerializer = New XmlSerializer(GetType(ProgramSettings))
                xs.Serialize(fs, myProgramSettings)
                fs.Close()
            End Using
            isf.Close()
        End Using
    End Sub

    Public Sub DefaultSettings()
        'Settings.DoPopupLoadAtStart = True
        'Settings.MotorFilePath = String.Empty
        'Settings.MotorSettingsChanged = False
        'myProgramSettings.LogFilePath = String.Empty
        'myProgramSettings.LogSeparator = ","
        'myProgramSettings.PortName = "COM1"

        myProgramSettings.MotorParamFilePath = String.Empty
        myProgramSettings.MotorParamChanged = False
        myProgramSettings.LogFilePath = String.Empty
        myProgramSettings.LogItems = Nothing
        myProgramSettings.LogSeparator = ", "
        myProgramSettings.PortName = "COM1"
        myProgramSettings.ScopeSettings = Nothing
    End Sub

    Private Sub ExtractDataToFOCStruct(ByRef f As BoardVarStruct, ByRef buff() As Byte)
        Dim offset As Integer = 0
        f.FocEnabled = CBool(buff(offset) And (1 << 0))
        f.speedEnabled = CBool(buff(offset) And (1 << 1))
        f.uartBusy = CBool(buff(offset) And (1 << 2))
        offset += 1

        f.mode = ToBYTE(buff, offset)
        f.deviceID(0) = ToDWORD(buff, offset)
        f.deviceID(1) = ToDWORD(buff, offset)
        f.alpha = ToWORD(buff, offset)
        f.adcVal(0) = ToWORD(buff, offset)
        f.adcVal(1) = ToWORD(buff, offset)
        f.currentOffset = ToWORD(buff, offset)
        f.adcPoint(0) = ToWORD(buff, offset)
        f.adcPoint(1) = ToWORD(buff, offset)
        f.adcSeq(0) = ToBYTE(buff, offset)
        f.adcSeq(1) = ToBYTE(buff, offset)
        f.adcSeq(2) = ToBYTE(buff, offset)
        f.temp = ToBYTE(buff, offset)
        f.busy = CBool(ToBYTE(buff, offset))
        f.sector = ToBYTE(buff, offset)
        f.pwm(0) = ToWORD(buff, offset)
        f.pwm(1) = ToWORD(buff, offset)
        f.pwm(2) = ToWORD(buff, offset)
        f.mACT(0) = ToWORD(buff, offset)
        f.mACT(1) = ToWORD(buff, offset)
        f.mACT(2) = ToWORD(buff, offset)
        f.mACT(3) = ToWORD(buff, offset)
        f.mACT(4) = ToWORD(buff, offset)
        f.mACT(5) = ToWORD(buff, offset)
        f.mDACT(0) = ToWORD(buff, offset)
        f.mDACT(1) = ToWORD(buff, offset)
        f.mDACT(2) = ToWORD(buff, offset)
        f.mDACT(3) = ToWORD(buff, offset)
        f.mDACT(4) = ToWORD(buff, offset)
        f.mDACT(5) = ToWORD(buff, offset)
        f.Ia = FromFixedPointToSigned(buff, offset, 6, 10) ' * 3.3 / 1024 '57.295779513082323
        f.Ib = FromFixedPointToSigned(buff, offset, 6, 10) '* 3.3 / 1024 ' * 57.295779513082323
        f.Ic = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Ialpha = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Ibeta = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Id = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Iq = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Vd = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Vq = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Valpha = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Vbeta = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Vr1 = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Vr2 = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Vr3 = FromFixedPointToSigned(buff, offset, 6, 10)

        f.Q_err = FromFixedPointToSigned(buff, offset, 22, 10)
        f.Q_int = FromFixedPointToSigned(buff, offset, 22, 10)
        f.D_err = FromFixedPointToSigned(buff, offset, 22, 10)
        f.D_int = FromFixedPointToSigned(buff, offset, 22, 10)

        f.Q_SP = FromFixedPointToSigned(buff, offset, 6, 10)
        f.D_SP = FromFixedPointToSigned(buff, offset, 6, 10)

        f.Q_Kp = FromFixedPointToSigned(buff, offset, 6, 10)
        f.Q_Ki = FromFixedPointToSigned(buff, offset, 6, 10)

        f.D_Kp = FromFixedPointToSigned(buff, offset, 6, 10)
        f.D_Ki = FromFixedPointToSigned(buff, offset, 6, 10)

        f.speed_err = FromFixedPointToSigned(buff, offset, 22, 10)
        f.speed_int = FromFixedPointToSigned(buff, offset, 22, 10)
        f.speed_der = FromFixedPointToSigned(buff, offset, 22, 10)
        f.speed_prev_err = FromFixedPointToSigned(buff, offset, 22, 10)

        f.empty = FromFixedPointToSigned(buff, offset, 6, 10)
        f.speed = FromFixedPointToSigned(buff, offset, 6, 10)

        f.SpeedKp = FromFixedPointToSigned(buff, offset, 6, 10)
        f.SpeedKi = FromFixedPointToSigned(buff, offset, 6, 10)
        f.SpeedKd = FromFixedPointToSigned(buff, offset, 6, 10)
        f.SpeedSP = FromFixedPointToSigned(buff, offset, 6, 10)

        f.alphaCalc = FromFixedPointToSigned(buff, offset, 6, 10)
        f.alphaEst = FromFixedPointToSigned(buff, offset, 6, 10)

        f.F16_16_Valpha = FromFixedPointToSigned(buff, offset, 16, 16)      ' Voltage on alpha axis
        f.F16_16_Vbeta = FromFixedPointToSigned(buff, offset, 16, 16)       ' Input: Stationary beta-axis stator voltage 
        f.F16_16_Ialpha = FromFixedPointToSigned(buff, offset, 16, 16)      ' Current on alpha axis 
        f.F16_16_Ibeta = FromFixedPointToSigned(buff, offset, 16, 16)       ' Current on beta axis  
        f.bemf_alpha = FromFixedPointToSigned(buff, offset, 16, 16)         ' BEMF voltage on alpha axis 
        f.bemf_alpha_flt = FromFixedPointToSigned(buff, offset, 16, 16)     ' Filtered BEMF voltage on alpha axis
        f.bemf_beta = FromFixedPointToSigned(buff, offset, 16, 16)          ' BEMF voltage on beta axis 
        f.bemf_beta_flt = FromFixedPointToSigned(buff, offset, 16, 16)      ' Filtered BEMF voltage on beta axis 
        f.z_alpha = FromFixedPointToSigned(buff, offset, 16, 16)            ' SMC correction factor z on alpha axis 
        f.z_beta = FromFixedPointToSigned(buff, offset, 16, 16)             ' SMC correction factor z on beta axis
        f.Ialpha_est = FromFixedPointToSigned(buff, offset, 16, 16)         ' Estimated current on alpha axis
        f.Ibeta_est = FromFixedPointToSigned(buff, offset, 16, 16)          ' Estimated current on beta axis 
        f.Ialpha_err = FromFixedPointToSigned(buff, offset, 16, 16)         ' Current estimation error on alpha axis
        f.Ibeta_err = FromFixedPointToSigned(buff, offset, 16, 16)          ' Current estimation error on beta axis               
        f.K_smc = FromFixedPointToSigned(buff, offset, 16, 16)              ' Sliding Mode Controller gain 
        f.smc_max_err = FromFixedPointToSigned(buff, offset, 16, 16)        ' Maximum error of SMC 
        f.G = FromFixedPointToSigned(buff, offset, 16, 16)                  ' Virtual motor algorithm constant
        f.F = FromFixedPointToSigned(buff, offset, 16, 16)                  ' Virtual motor algorithm constant
        f.Cflt_smc = FromFixedPointToSigned(buff, offset, 16, 16)           ' Slide mode controller filter coefficientficient 
        f.Cflt_omega = FromFixedPointToSigned(buff, offset, 16, 16)         ' Filter coefficientficient for Omega filtered calc
        f.omega = FromFixedPointToSigned(buff, offset, 16, 16)              ' Estimated rotor speed
        f.omega_flt = FromFixedPointToSigned(buff, offset, 16, 16)          ' Filtered estimated rotor speed
        f.theta = FromFixedPointToSigned(buff, offset, 16, 16)              ' Estimated rotor angle 
        f.theta_offset = FromFixedPointToSigned(buff, offset, 16, 16)       ' Theta compensation 
        f.test1 = FromFixedPointToSigned(buff, offset, 16, 16)              ' test1&2 can be used for debugging F16_16 fixed point
        f.test2 = FromFixedPointToSigned(buff, offset, 16, 16)
        f.test3 = FromFixedPointToSigned(buff, offset, 22, 10)              ' test3&4 can be used for debugging F22_10 fixed point
        f.test4 = FromFixedPointToSigned(buff, offset, 22, 10)

        Dim err As Single = f.alphaEst - f.alphaCalc
        'If err < 0 Then
        '    err = 2 * Math.PI + err
        'End If
        If err >= Math.PI Then
            err -= 2 * Math.PI ' 2 * Math.PI - d
        End If
        If err <= -Math.PI Then
            err += 2 * Math.PI ' 2 * Math.PI - d
        End If
        'If d < -Math.PI Then d += 2 * Math.PI
        f.alphaError = err
        f.Itot = Math.Abs(f.Ia) + Math.Abs(f.Ib) + Math.Abs(f.Ic)
        f.Itot_fltr = f.Itot_fltr * 0.95 + f.Itot * 0.05
    End Sub

    Public Sub LogData()
        If logStream Is Nothing Then InitLogStream()
        If logStream IsNot Nothing Then logStream.WriteLine(logForm.GetLogString(boardVar))
    End Sub

    Public Sub InitLogStream()
        If logStream IsNot Nothing Then
            Try
                logStream.Close()
            Catch ex As Exception
                ' Do nothing
            End Try
        End If
        logStream = New StreamWriter(myProgramSettings.LogFilePath)
    End Sub

    Public Enum Param
        FOCEnabled = 0
        SpeedControlEnabled = 1
        Mode = 2
        QSP = 3
        DSP = 4
        DKp = 5
        DKi = 6
        QKp = 7
        QKi = 8
        SpeedSP = 9
        SpeedKp = 10
        SpeedKi = 11
        SpeedKd = 12
        Fsmo = 13
        Gsmo = 14
        kSlide = 15
        maxError = 16
        omegaFltrCoef = 17
        Test1 = 18
        Test2 = 19
        Test3 = 20
        Test4 = 21
    End Enum

    Public Sub EnqueueWriteParam(ByVal cmd As Integer, ByVal data() As Byte)
        ' Dont enqueue write parameters if import is ongoing
        If doImport Then Exit Sub
        myProgramSettings.MotorParamChanged = True
        Dim tx(4) As Byte   ' 5 bytes
        tx(0) = cmd
        data.CopyTo(tx, 1)   ' coppy all bytes from data to tx starting at index 1
        writeQueue.Enqueue(tx)
    End Sub

    Public Sub LoadParametersFromStream(ByVal s As Stream)
        ' Read motor data : deserialize the XML to an object
        Dim xtr As XmlTextReader = New XmlTextReader(s)
        Dim xs As XmlSerializer = New XmlSerializer(GetType(MotorParam))
        myParam = CType(xs.Deserialize(xtr), MotorParam)

        ' Initialise controls from loaded parameters
        InitControlsFromParameters(myParam)

        ' At first connection with the demonstration board load the paramaters into the board
        doExport = True
    End Sub

    Public Sub SaveParametersToStream(ByVal s As Stream)
        ' Serialize the motor to a XML file
        Dim xs As XmlSerializer = New XmlSerializer(GetType(MotorParam))
        xs.Serialize(s, myParam)
    End Sub

    Public Sub SaveMotorParameter()
        Using sfd As New SaveFileDialog()
            sfd.Filter = "Motor files (*.xml)|*.xml|All files (*.*)|*.*"
            sfd.FilterIndex = 1
            sfd.RestoreDirectory = True
            sfd.InitialDirectory = Microsoft.VisualBasic.FileIO.SpecialDirectories.Desktop

            If sfd.ShowDialog() = DialogResult.OK Then
                Using myStream As Stream = sfd.OpenFile()
                    If (myStream IsNot Nothing) Then
                        Try
                            SaveParametersToStream(myStream)
                            myProgramSettings.MotorParamFilePath = sfd.FileName
                            myStream.Close()
                        Catch ex As Exception
                            MessageBox.Show("Cannot save file." & vbNewLine & ex.Message)
                        End Try
                    End If
                End Using
            End If
        End Using
    End Sub

    Public Sub ShowDatalogger()
        If logForm Is Nothing OrElse logForm.IsDisposed Then
            logForm = New DataLogForm(myProgramSettings, Me.dataFields)
            AddHandler logForm.FormClosing, AddressOf SaveSettings

            logForm.TextBoxLogFilePath.Text = myProgramSettings.LogFilePath
            If Not String.IsNullOrEmpty(myProgramSettings.LogFilePath) AndAlso _
                Directory.Exists(Path.GetDirectoryName(myProgramSettings.LogFilePath)) AndAlso _
                SerialPort1.IsOpen Then
                logForm.ButtonStart.Enabled = True
            End If
            logForm.Show()
            If SerialPort1.IsOpen Then logForm.ButtonStart.Enabled = True
        Else
            logForm.WindowState = FormWindowState.Normal
            logForm.BringToFront()
        End If
    End Sub

    Public Sub AddScope()
        ' Get position
        Dim newLeft As Integer
        Dim newTop As Integer
        If scopeCollection.Count = 0 Then
            newLeft = 0
            newTop = 0
        Else
            newLeft = scopeCollection.Item(scopeCollection.Count - 1).Left
            newTop = scopeCollection.Item(scopeCollection.Count - 1).Bottom
            If newLeft > Screen.PrimaryScreen.WorkingArea.Width - scopeCollection.Item(scopeCollection.Count - 1).Width Then
                newLeft = 0
            End If
            If newTop > Screen.PrimaryScreen.WorkingArea.Height - scopeCollection.Item(scopeCollection.Count - 1).Height Then
                newTop = 0
            End If
        End If

        ' Get next free name
        Dim newNumber As Integer = scopeCollection.Count + 1
        Dim names As New List(Of String)
        For Each scope As ScopeForm In scopeCollection
            names.Add(scope.Text.Replace("Scope ", String.Empty))
        Next
        For i As Integer = 1 To scopeCollection.Count
            If Not names.ToArray.Contains(i.ToString) Then
                newNumber = i
                Exit For
            End If
        Next

        ' Add scope
        Dim newSettings As New ScopeSettings
        newSettings.Number = newNumber
        newSettings.Top = newTop
        newSettings.Left = newLeft
        newSettings.Signal1Index = 0
        newSettings.Signal2Index = 0
        newSettings.Min = 0
        newSettings.Max = 0
        newSettings.Auto = True

        CreateScope(newSettings)
    End Sub

    Private Sub CreateScope(ByVal settings As ScopeSettings)
        Dim newScope As New ScopeForm(Me.dataFields, settings)
        scopeCollection.Add(newScope)
        AddHandler newScope.FormClosing, AddressOf ScopeFormClosing
        newScope.Show()
        If SerialPort1.IsOpen Then
            newScope.ScrollControl1.Start()
        End If
    End Sub

    Private Sub ScopeFormClosing(ByVal source As Object, ByVal e As EventArgs)
        scopeCollection.Remove(source)
        RemoveHandler CType(source, ScopeForm).FormClosing, AddressOf ScopeFormClosing
    End Sub

    Public Sub EnumerateDataFields()
        Dim dataFieldList As New List(Of String)
        For Each fi As FieldInfo In boardVar.GetType.GetFields()
            If fi.FieldType.Name = "Single" Or fi.FieldType.Name = "Int32" Then
                dataFieldList.Add(fi.Name)
            End If
        Next
        dataFields = dataFieldList.ToArray
    End Sub

    Public Sub CheckBoardID()
        If suspendIDCheck Then Exit Sub

        If Not boardIDRead Then
            boardIDRead = True
            ' First connection of a board after startup
            boardID(0) = boardVar.deviceID(0)
            boardID(1) = boardVar.deviceID(1)
            wrongIDcnt = 0
        Else
            If boardID(0) <> boardVar.deviceID(0) Or boardID(1) <> boardVar.deviceID(1) Then wrongIDcnt += 1
            If wrongIDcnt > 10 Then
                suspendIDCheck = True
                If MessageBox.Show("Connection of a new board has been detected. Write tool parameters to the demoboard?", "New board detected", MessageBoxButtons.YesNo) = Windows.Forms.DialogResult.Yes Then
                    doExport = True
                Else
                    doImport = True
                End If
                suspendIDCheck = False
                boardID(0) = boardVar.deviceID(0)
                boardID(1) = boardVar.deviceID(1)
                wrongIDcnt = 0
            End If
        End If
    End Sub

#End Region

End Class

#Region "Classes and Enums"

Public Enum OperationMode
    MODE_QEI_RAMP = 0   ' Ramp for QEI operation
    MODE_SL_RAMP = 1    ' Ramp for sensorless operation
    MODE_QEI = 2        ' QEI operation
    MODE_HALL = 3       ' HALL operation
    MODE_SL = 4         ' Sensorless operation
End Enum

<Serializable()> _
Public Class ProgramSettings
    Public MotorParamFilePath As String
    Public MotorParamChanged As Boolean
    Public LogFilePath As String
    Public LogItems() As String
    Public LogSeparator As String
    Public PortName As String
    Public ScopeSettings() As ScopeSettings
End Class

<Serializable()> _
Public Class ScopeSettings
    Public Number As Integer
    Public Top As Integer
    Public Left As Integer
    Public Signal1Index As Integer
    Public Signal2Index As Integer
    Public Min As Single
    Public Max As Single
    Public Auto As Boolean
End Class

<Serializable()> _
Public Class MotorParam
    Public FocEnabled As Boolean
    Public SpeedEnabled As Boolean
    Public Q_SP As Single
    Public D_SP As Single
    Public Q_Kp As Single
    Public D_Kp As Single
    Public Q_Ki As Single
    Public D_Ki As Single
    Public Speed_SP As Single
    Public Speed_Kp As Single
    Public Speed_Ki As Single
    Public Speed_Kd As Single
    Public SMC_K As Single
    Public SMC_MaxError As Single
    Public CfltOmega As Single
    Public PhaseRes As Single
    Public PhaseInd As Single
    Public F As Single
    Public G As Single
End Class

Public Class BoardVarStruct
    Public FocEnabled As Boolean   ' FOC enabled/disabled
    Public speedEnabled As Boolean ' Speed PID controller enabled/disabled
    Public uartBusy As Boolean
    Public mode As Byte
    Public deviceID As Integer() ' DeviceID of microcontroller on demoboard
    Public alpha As Integer     ' Measured / calculated stator angle
    Public adcVal As Integer()  ' ADC values measured
    Public currentOffset As Integer  ' Current offset
    Public adcPoint As Integer() ' ADC start offset from t=0
    Public adcSeq As Integer()  ' ADC sequence
    Public temp As Integer
    Public busy As Boolean      ' FOC busy
    Public sector As Integer    ' Current SVWPWM sector
    Public pwm As Integer()     ' PWM values from space vector
    Public mACT As Integer()    ' Match activate registers for PWM 
    Public mDACT As Integer()   ' Match de-activate registers for PWM  
    Public Ia As Single         ' Reconstructed Ia	
    Public Ib As Single         ' Reconstructed Ib	
    Public Ic As Single         ' Reconstructed Ic
    Public Ialpha As Single     ' Calculated Ia in dynamic reference frame
    Public Ibeta As Single      ' Calculated Ib in dynamic reference frame
    Public Id As Single         ' Calculated Id in static reference frame
    Public Iq As Single         ' Calculated Iq in static reference frame
    Public Vd As Single         ' Regulated PI output d in static reference frame
    Public Vq As Single         ' Regulated PI output q in static reference frame
    Public Valpha As Single     ' Regulated PI output alpha in dynamic reference frame
    Public Vbeta As Single      ' Regulated PI output beta in dynamic reference frame
    Public Vr1 As Single        ' Vref1 from inverse park
    Public Vr2 As Single        ' Vref2 from inverse park
    Public Vr3 As Single        ' Vref3 from inverse park
    Public Q_err As Single      ' Q error
    Public Q_int As Single      ' Q integrated error
    Public D_err As Single      ' D error
    Public D_int As Single      ' D integrated error

    Public Q_SP As Single       '  Q SetPoint
    Public D_SP As Single       '  D SetPoint 

    Public Q_Kp As Single       ' VQ Kp
    Public Q_Ki As Single       ' VQ Ki

    Public D_Kp As Single       ' VD PI const Kp
    Public D_Ki As Single       ' VD PI const Ki	 	

    Public speed_err As Single  ' Speed error
    Public speed_int As Single  ' Speed integrated error
    Public speed_der As Single  ' Speed derrivated error
    Public speed_prev_err As Single ' Speed previous error value

    Public empty As Single
    Public speed As Single      ' Speed : 32 <> 10000 RPM fixed point

    Public SpeedKp As Single    ' Speed Kp
    Public SpeedKi As Single    ' Speed Ki
    Public SpeedKd As Single    ' Speed Kd
    Public SpeedSP As Single    ' Speed setpoint
    Public alphaCalc As Single
    Public alphaEst As Single

    Public F16_16_Valpha As Single      ' Voltage on alpha axis
    Public F16_16_Vbeta As Single       ' Input: Stationary beta-axis stator voltage 
    Public F16_16_Ialpha As Single      ' Current on alpha axis 
    Public F16_16_Ibeta As Single       ' Current on beta axis  
    Public bemf_alpha As Single         ' BEMF voltage on alpha axis 
    Public bemf_alpha_flt As Single     ' Filtered BEMF voltage on alpha axis
    Public bemf_beta As Single          ' BEMF voltage on beta axis 
    Public bemf_beta_flt As Single      ' Filtered BEMF voltage on beta axis 
    Public z_alpha As Single            ' SMC correction factor z on alpha axis 
    Public z_beta As Single             ' SMC correction factor z on beta axis
    Public Ialpha_est As Single         ' Estimated current on alpha axis
    Public Ibeta_est As Single          ' Estimated current on beta axis 
    Public Ialpha_err As Single         ' Current estimation error on alpha axis
    Public Ibeta_err As Single          ' Current estimation error on beta axis               
    Public K_smc As Single              ' Sliding Mode Controller gain 
    Public smc_max_err As Single        ' Maximum error of SMC 
    Public G As Single                  ' Virtual motor algorithm constant
    Public F As Single                  ' Virtual motor algorithm constant
    Public Cflt_smc As Single           ' Slide mode controller filter coefficientficient 
    Public Cflt_omega As Single         ' Filter coefficientficient for Omega filtered calc
    Public omega As Single              ' Estimated rotor speed
    Public omega_flt As Single          ' Filtered estimated rotor speed
    Public theta As Single              ' Estimated rotor angle 
    Public theta_offset As Single       ' Theta compensation 
    Public test1 As Single              ' test1&2 can be used for debugging F16_16 fixed point
    Public test2 As Single
    Public test3 As Single              ' test3&4 can be used for debugging F22_10 fixed point
    Public test4 As Single

    Public sVbeta As Single
    Public sValpha As Single
    Public sIbeta As Single
    Public sIalpha As Single

    Public alphaError As Single
    Public Itot As Single
    Public Itot_fltr As Single
End Class

Public Class NumericTextBox
    Inherits TextBox
    Private SpaceOK As Boolean = False
    Private NegativeOK As Boolean = True

    ' Restricts the entry of characters to digits (including hex),
    ' the negative sign, the e decimal point, and editing keystrokes (backspace).
    Protected Overrides Sub OnKeyPress(ByVal e As KeyPressEventArgs)
        MyBase.OnKeyPress(e)

        Dim numberFormatInfo As Globalization.NumberFormatInfo = System.Globalization.CultureInfo.CurrentCulture.NumberFormat
        Dim decimalSeparator As String = numberFormatInfo.NumberDecimalSeparator
        Dim groupSeparator As String = numberFormatInfo.NumberGroupSeparator
        Dim negativeSign As String = numberFormatInfo.NegativeSign

        Dim keyInput As String = e.KeyChar.ToString()

        If [Char].IsDigit(e.KeyChar) Then
            ' Digits are OK
        ElseIf keyInput.Equals(decimalSeparator) OrElse keyInput.Equals(groupSeparator) Then
            ' Decimal separator is OK
        ElseIf keyInput.Equals(negativeSign) And NegativeOK Then
            ' Only allow negative sign if property is set
        ElseIf e.KeyChar = vbBack Then
            ' Backspace key is OK
            '    else if ((ModifierKeys & (Keys.Control | Keys.Alt)) != 0)
            '    {
            '     // Let the edit control handle control and alt key combinations
            '    }
        ElseIf Me.SpaceOK AndAlso e.KeyChar = " "c Then
            ' Do nothing
        Else
            ' Consume this invalid key and beep.
            e.Handled = True
        End If

    End Sub

    Public ReadOnly Property IntValue() As Integer
        Get
            Return Int32.Parse(Me.Text)
        End Get
    End Property

    Public ReadOnly Property DecimalValue() As Decimal
        Get
            Return [Decimal].Parse(Me.Text)
        End Get
    End Property

    Public Property AllowSpace() As Boolean
        Get
            Return Me.SpaceOK
        End Get
        Set(ByVal value As Boolean)
            Me.SpaceOK = value
        End Set
    End Property

    Public Property AllowNegative() As Boolean
        Get
            Return Me.NegativeOK
        End Get
        Set(ByVal value As Boolean)
            Me.NegativeOK = value
        End Set
    End Property

End Class

#End Region