Imports System.Threading
Imports System.Text

Module GetScopePos

    Public Resp As String
    Dim tickBegin As Long
    Dim tickEnd As Long
    Public RespGood As Boolean
    Dim Deg As Single
    Dim DegSign As String = ""
    Dim M As Single
    Dim S As Single
    Dim SS As Single
    Dim LAT As Double
    Dim LON As Double
    Dim ST As Double
    Dim Alt As Double
    Dim Az As Double
    Dim AltGoto As Double
    Dim AZGoto As Double
    Dim GotoAZL As Boolean = False
    Dim Ascom_Rate As Double = 0.0416666        ' default of 10 x sidereal
    Dim Sid_Deg_Sec As Double = 0.004166666665  ' sidereal rate in degrees / second
    Dim AscomSetOnly As Boolean = False

    Public Sub SendReceiveAP(ByVal Command As String, ByVal ErrorMessage As String, ByVal WaitResp As Boolean, ByVal RespLen As Integer)
        Resp = ""
        If Form1.AscomConnect = True Then
            AscomSetOnly = False
            Try
                Select Case Command
                    Case ":GR#"
                        Dim RA As Double = Form1.driver.RightAscension
                        Dim ts As TimeSpan = TimeSpan.FromHours(CDbl(RA))
                        Resp = Format(ts.Hours, "00:") & Format(ts.Minutes, "00:") & Format(ts.Seconds, "00") & "." & _
                                Microsoft.VisualBasic.Left(ts.Milliseconds, 1) & "#"

                    Case ":GD#"
                        Dim DEC As Double = Form1.driver.Declination
                        DegToDMS(DEC)
                        Resp = DegSign & Format(Deg, "00") & "*" & Format(M, "00") & ":" & Format(S, "00") & "#"

                    Case ":GA#"
                        If Form1.driver.CanSlewAltAzAsync = True Then
                            Alt = Form1.driver.Altitude
                            DegToDMS(Alt)
                            'CalcALTfromRADEC()
                        Else
                            CalcALTfromRADEC()
                        End If
                        Resp = DegSign & Format(Deg, "00") & "*" & Format(M, "00") & ":" & Format(S, "00") & "#"

                    Case ":GZ#"
                        If Form1.driver.CanSlewAltAzAsync = True Then
                            Az = Form1.driver.Azimuth
                            DegToDMS(Az)
                            'CalcAZfromRADEC()
                        Else
                            CalcAZfromRADEC()
                        End If
                        Resp = DegSign & Format(Deg, "000") & "*" & Format(M, "00") & ":" & Format(S, "00") & "#"

                    Case ":Gt#"
                        LAT = Form1.driver.SiteLatitude
                        DegToDMS(LAT)
                        Resp = DegSign & Format(Deg, "00") & "*" & Format(M, "00") & ":" & Format(S, "00") & "#"

                    Case ":Gg#"
                        LON = Form1.driver.SiteLongitude
                        DegToDMS(LON)
                        Resp = DegSign & Format(Deg, "00") & "*" & Format(M, "00") & ":" & Format(S, "00") & "#"

                    Case ":GS#"
                        ST = Form1.driver.SiderealTime
                        Dim ts As TimeSpan = TimeSpan.FromHours(CDbl(ST))
                        Resp = Format(ts.Hours, "00:") & Format(ts.Minutes, "00:") & Format(ts.Seconds, "00") & "." & _
                            Microsoft.VisualBasic.Left(ts.Milliseconds, 1) & "#"

                    Case ":pS#"
                        Dim Side As Integer
                        Side = Form1.driver.SideOfPier
                        If Side = 0 Then
                            Resp = "East#"
                        End If
                        If Side = 1 Then
                            Resp = "West#"
                        End If

                    Case ":MS#"
                        If GotoAZL = False Then
                            Form1.driver.SlewToTargetAsync()
                        Else
                            If Form1.driver.CanSlewAltAzAsync = False Then
                                TransRADECfromALTAZ()
                                Form1.driver.SlewToTargetAsync()
                            Else
                                'TransRADECfromALTAZ()
                                'Form1.driver.SlewToTargetAsync()
                                Form1.driver.SlewToAltAzAsync(AZGoto, AltGoto)
                            End If
                        End If
                        Resp = 0

                    Case ":RG0#"
                        Ascom_Rate = Sid_Deg_Sec * 0.25
                        AscomSetOnly = True

                    Case ":RG1#"
                        Ascom_Rate = Sid_Deg_Sec * 0.5
                        AscomSetOnly = True

                    Case ":RG2#"
                        Ascom_Rate = Sid_Deg_Sec * 1.0
                        AscomSetOnly = True

                    Case ":RC0#"
                        Ascom_Rate = Sid_Deg_Sec * 12.0
                        AscomSetOnly = True

                    Case ":RC1#"
                        Ascom_Rate = Sid_Deg_Sec * 64.0
                        AscomSetOnly = True

                    Case ":RC2#"
                        Ascom_Rate = Sid_Deg_Sec * 200.0
                        AscomSetOnly = True

                    Case ":Mn#"
                        Form1.driver.MoveAxis(ASCOM.DeviceInterface.TelescopeAxes.axisSecondary, Ascom_Rate)

                    Case ":Ms#"
                        Form1.driver.MoveAxis(ASCOM.DeviceInterface.TelescopeAxes.axisSecondary, Ascom_Rate * -1)

                    Case ":Me#"
                        Form1.driver.MoveAxis(ASCOM.DeviceInterface.TelescopeAxes.axisPrimary, Ascom_Rate)

                    Case ":Mw#"
                        Form1.driver.MoveAxis(ASCOM.DeviceInterface.TelescopeAxes.axisPrimary, Ascom_Rate * -1)

                    Case ":Q#"
                        Form1.driver.MoveAxis(ASCOM.DeviceInterface.TelescopeAxes.axisPrimary, 0.0)
                        Form1.driver.MoveAxis(ASCOM.DeviceInterface.TelescopeAxes.axisSecondary, 0.0)
                        Form1.driver.AbortSlew()

                    Case ":Qs#", ":Qn#"
                        Form1.driver.MoveAxis(ASCOM.DeviceInterface.TelescopeAxes.axisSecondary, 0.0)

                    Case ":Qe#", ":Qw#"
                        Form1.driver.MoveAxis(ASCOM.DeviceInterface.TelescopeAxes.axisPrimary, 0.0)

                    Case ":V#"
                        ' AP GTOCP version return V2 for CP3
                        Resp = "V2#"

                    Case ":RT9#"
                        ' Stop tracking
                        Form1.driver.Tracking = False

                    Case ":RT2#", ":RT1#", ":RT0#"
                        ' Start tracking, sidereal, solar, lunar
                        Form1.driver.Tracking = True

                    Case ":CMR#"
                        Form1.driver.SyncToTarget()

                End Select

                If Mid(Command, 1, 3) = ":Rc" Then
                    Dim VarRate As String = Mid(Command, 4, 3)
                    Ascom_Rate = Sid_Deg_Sec * VarRate
                End If

                Dim GotoCmd As String = Mid(Command, 1, 3)
                Dim Cmd As String = Command
                Dim D, R As Double
                Dim DegH As Integer
                Dim Mins As Integer
                Dim Secs As Single
                Select Case GotoCmd
                    Case ":Sr"
                        DegH = Mid(Cmd, 5, 2)
                        Mins = Mid(Cmd, 8, 2)
                        If Cmd.Length > 14 Then
                            Secs = Mid(Cmd, 11, 4)
                        Else
                            Secs = Mid(Cmd, 11, 2)
                        End If
                        R = DegH + (Mins / 60) + (Secs / 3600)
                        GotoAZL = False
                        Form1.driver.TargetRightAscension = R
                        Resp = "1"

                    Case ":Sd"
                        Dim SignDec As Integer = 1
                        If Mid(Cmd, 5, 1) = "-" Then
                            SignDec = -1
                        End If
                        DegH = Mid(Cmd, 6, 2)
                        Mins = Mid(Cmd, 9, 2)
                        Secs = Mid(Cmd, 12, 2)
                        D = (DegH + (Mins / 60) + (Secs / 3600)) * SignDec
                        GotoAZL = False
                        Form1.driver.TargetDeclination = D
                        Resp = "1"

                    Case ":Sa"
                        Dim SignDec As Integer = 1
                        If Mid(Cmd, 5, 1) = "-" Then
                            SignDec = -1
                        End If
                        DegH = Mid(Cmd, 6, 2)
                        Mins = Mid(Cmd, 9, 2)
                        Secs = Mid(Cmd, 12, 2)
                        AltGoto = (DegH + (Mins / 60) + (Secs / 3600)) * SignDec
                        GotoAZL = True
                        Resp = "1"

                    Case ":Sz"
                        DegH = Mid(Cmd, 5, 3)
                        Mins = Mid(Cmd, 9, 2)
                        Secs = Mid(Cmd, 12, 2)
                        AZGoto = DegH + (Mins / 60) + (Secs / 3600)
                        GotoAZL = True
                        Resp = "1"

                End Select

            Catch ex As Exception
                Form1.LogScope("ASCOM > ", Color.DarkRed, ex.Message)
                Form1.UpdateStatusLine("ASCOM communications - ERROR, see log", Color.DarkRed)
                Exit Sub
            End Try
        Else
            If Form1.APnetwork = False Then
                If Form1.SerialPort1.IsOpen = True Then
                    Form1.SerialPort1.Write(Command)
                Else
                    Form1.UpdateStatusLine("Not connected to mount", Color.DarkRed)
                    Form1.Rsp = "Not connected to mount"
                    Exit Sub
                End If
            Else
                Dim ApRec(50) As Byte
                Try
                    ApRec = Encoding.ASCII.GetBytes(Command)
                    Form1.networkStream.Write(ApRec, 0, Command.Length)
                Catch ex As Exception
                    Form1.LogScope("TCP > ", Color.DarkRed, ex.Message)
                    Form1.UpdateStatusLine("AP mount network write - ERROR, see log", Color.DarkRed)
                    Exit Sub
                End Try
            End If
        End If

        If AscomSetOnly = False Then
            Form1.LogScope("Sent > ", Color.DarkBlue, Command)
        End If

        If WaitResp = True Then
            If Form1.AscomConnect = False Then
                If Form1.APnetwork = False Then
                    tickBegin = Int(Date.Now.Ticks / 10000000)
                    If RespLen = 0 Then
                        ' Read response until # is received or timeout
                        Do Until Microsoft.VisualBasic.Right(Resp, 1) = "#"
                            Resp = Resp & Form1.SerialPort1.ReadExisting()
                            tickEnd = Int(Date.Now.Ticks / 10000000)
                            If tickEnd - tickBegin > Form1.ReadTimeout Then
                                Form1.SerialPort1.DiscardInBuffer()
                                MountReadTimeout(ErrorMessage)
                                Exit Do
                            End If
                        Loop
                    Else
                        ' Read response until 'RespLen' bytes are received
                        Do Until Resp.Length = RespLen
                            Resp = Resp & Form1.SerialPort1.ReadExisting()
                            tickEnd = Int(Date.Now.Ticks / 10000000)
                            If tickEnd - tickBegin > Form1.ReadTimeout Then
                                Form1.SerialPort1.DiscardInBuffer()
                                MountReadTimeout(ErrorMessage)
                                Exit Do
                            End If
                        Loop

                    End If
                Else
                    Try
                        If RespLen = 0 Then
                            Dim TermChr As String = "#"
                            If Form1.CP4ip = True Then
                                TermChr = Chr(10)
                            End If
                            Do
                                Form1.NumBytesRead = Form1.networkStream.Read(Form1.MountRec, 0, Form1.MountRec.Length)
                                Resp = Resp & Encoding.ASCII.GetString(Form1.MountRec, 0, Form1.NumBytesRead)
                            Loop Until Microsoft.VisualBasic.Right(Resp, 1) = TermChr
                            If Form1.CP4ip = True Then
                                Resp = Mid(Resp, 1, Resp.Length - 1)
                            End If
                            ' remove any mount power up data
                            If Form_ControlP.CheckBoxDebugLogging.Checked = True Then
                                Form1.LogScope("Debug TCP < (", Color.SaddleBrown, Resp.Length & ") " & Resp)
                            End If
                            Dim Resp2 As String = Resp
                            Do While Mid(Resp2, 1, 2) = "Y1"
                                Resp2 = Resp2.Replace("Y1", "")
                            Loop
                            Resp = Resp2
                        Else
                            Do
                                Form1.NumBytesRead = Form1.networkStream.Read(Form1.MountRec, 0, Form1.MountRec.Length)
                                Resp = Resp & Encoding.ASCII.GetString(Form1.MountRec, 0, Form1.NumBytesRead)
                            Loop Until Form1.NumBytesRead = RespLen
                            If Form_ControlP.CheckBoxDebugLogging.Checked = True Then
                                Form1.LogScope("Debug TCP [" & RespLen & "] < (", Color.SaddleBrown, Resp.Length & ") " & Resp)
                            End If
                        End If
                    Catch ex As Exception
                        MountReadTimeout(ErrorMessage)
                    End Try
                End If

            End If

            Form1.LogScope("Resp < ", Color.DarkGreen, Resp)
            Form1.Rsp = Resp

        End If

    End Sub

    Private Sub MountReadTimeout(ByVal ErrorMessage As String)
        RespGood = False
        Form1.UpdateStatusLine("Timeout " & Form1.ReadTimeout & " sec. getting " _
            & ErrorMessage, Color.DarkRed)
        If Form_Dome.StopTrackingOnTimeout.Checked = True And Form_Dome.CheckBoxTrack.Checked = True Then
            Form_Dome.CheckBoxTrack.Checked = False
        End If

    End Sub

    Public Sub GetScopeRaDec()
        If Form1.SerialPort1.IsOpen = True Or Form1.SocketIsOpen = True Or Form1.IsConnected = True Then

            RespGood = True
            ' Get RA
            Form1.AP_RA = ""
            SendReceiveAP(":GR#", "RA", True, 0)
            If RespGood = False Then Exit Sub
            Form1.AP_RA = Resp

            ' Get DEC
            Form1.AP_DEC = ""
            SendReceiveAP(":GD#", "DEC", True, 0)
            If RespGood = False Then Exit Sub
            Form1.AP_DEC = Resp

            ' Get Sidereal time
            SendReceiveAP(":GS#", "Sid. Time", True, 0)
            If RespGood = False Then Exit Sub
            Form1.TelescopeST = Mid(Resp, 1, Resp.Length - 1)

            ' Get Side of mount
            SendReceiveAP(":pS#", "Side of mount", True, 0)
            If RespGood = False Then Exit Sub
            Form1.TelescopeSide = Mid(Resp, 1, Resp.Length - 1)

        End If

    End Sub

    Public Sub GetScopeAzEl()
        If Form1.SerialPort1.IsOpen = True Or Form1.SocketIsOpen = True Or Form1.IsConnected = True Then
            RespGood = True

            ' Get AZ
            Form1.AP_AZ = ""
            SendReceiveAP(":GZ#", "AZ", True, 0)
            If RespGood = False Then Exit Sub
            Form1.AP_AZ = Resp
            Dim I As Integer
            I = InStr(Resp, "*") - 2
            If I > 0 Then
                Form1.Az_Value = Mid(Resp, 2, I)
            End If

            ' Get AL
            Form1.AP_AL = ""
            SendReceiveAP(":GA#", "AL", True, 0)
            If RespGood = False Then Exit Sub
            Form1.AP_AL = Resp

        End If

    End Sub

    Public Sub GetMeridianDist()
        If Form1.SerialPort1.IsOpen Or Form1.SocketIsOpen = True Or Form1.IsConnected = True Then
            Try
                GetScopeRaDec()
                Dim ScopeRA As Double = Mid(Form1.AP_RA, 1, 2) + (Mid(Form1.AP_RA, 4, 2) / 60) + (Mid(Form1.AP_RA, 7, 4) / 3600)
                Dim ST As Double = Mid(Form1.TelescopeST, 1, 2) + (Mid(Form1.TelescopeST, 4, 2) / 60) + (Mid(Form1.TelescopeST, 7, 2) / 3600)

                Dim HA As Double = Form_Dome.HAScale(ST - ScopeRA)
                Dim MerDist As Double = HA * 15.0
                Form1.AP_MeridianDeg = MerDist

                Dim HM As String = DegToHourMin(Math.Abs(MerDist))
                Form1.AP_MeridianTimeToStop = HM

                If MerDist < 0.0 Then
                    Form1.AP_MeridianDist = Format(Math.Abs(MerDist), "0.0") & "° To Meridian (" & HM & ")"
                Else
                    Form1.AP_MeridianDist = Format(MerDist, "0.0") & "° Past Meridian (" & HM & ")"
                End If

                Form1.LogScope("Status = ", Color.Black, Form1.AP_MeridianDist)

            Catch ex As Exception
                Form1.LogScope("Status = ", Color.DarkRed, "Error in GetMeridianDist = " & ex.Message)
                Form1.StatusLabel.Text = "Error in GetMeridianDist"
            End Try

        End If

    End Sub

    Private Sub DegToDMS(ByVal D As Double)
        Deg = Math.Truncate(D)
        If D >= 0.0 Then
            DegSign = "+"
        End If
        If D < 0.0 And D > -1.0 Then
            DegSign = "-"
        End If
        If D <= -1.0 Then
            DegSign = ""
        End If
        M = (D - Deg) * 60.0
        M = Math.Truncate(M)
        S = (D - Deg) * 3600 - (M * 60)
        M = Math.Abs(M)
        S = Math.Abs(S)
        SS = S - Math.Truncate(S)
        SS = SS * 10
        If S > 59 Then S = 59

    End Sub

    Private Function DegToHourMin(ByVal deg As Single) As String
        ' convert degrees decimal to hours minutes time
        Dim H As Single = deg / 15.0
        Dim M As Single = Math.Round((H - Int(deg / 15)) * 60)
        Dim str As String = Format(Int(H), "00h:") & Format(M, "00m")
        Return str
    End Function

    Private Sub TransRADECfromALTAZ()
        Dim RA As Double
        Dim DEC As Double
        Dim nowUTC As DateTime
        Dim transform As New ASCOM.Astrometry.Transform.Transform
        Dim utl As New ASCOM.Utilities.Util

        nowUTC = DateTime.UtcNow
        LAT = Form1.driver.SiteLatitude
        LON = Form1.driver.SiteLongitude

        transform.SiteElevation = 0.0
        transform.SiteLatitude = LAT
        transform.SiteLongitude = LON
        transform.JulianDateUTC = utl.DateUTCToJulian(nowUTC)
        transform.SetAzimuthElevation(AZGoto, AltGoto)

        RA = transform.RATopocentric
        DEC = transform.DECTopocentric
        Form1.driver.TargetDeclination = DEC
        Form1.driver.TargetRightAscension = RA

        Dim RAfmt = utl.DegreesToDMS(RA, ":", ":", "", 1)
        Dim DECfmt = utl.DegreesToDMS(DEC, "*", ":", "", 0)

        Form1.LogScope("ALT/AZ to RA > ", Color.Black, RAfmt)
        Form1.LogScope("ALT/AZ to DEC > ", Color.Black, DECfmt)

    End Sub

    'Private Sub CalcRADECfromATLAZ()
    '    ' This routine calculates RA and DEC from Alt, Az & Latitude

    '    Const RAD_DEG As Double = 57.2957795

    '    LAT = Form1.driver.SiteLatitude
    '    ST = Form1.driver.SiderealTime

    '    Dim LatR As Double = LAT / RAD_DEG
    '    Dim AltR As Double = AltGoto / RAD_DEG
    '    Dim AzR As Double = AZGoto / RAD_DEG

    '    Dim SinDec As Double = (Math.Sin(AltR) * Math.Sin(LatR)) + (Math.Cos(AltR) * Math.Cos(LatR) * Math.Cos(AzR))
    '    Dim DecRad As Double = Math.Asin(SinDec)
    '    Dim DecDeg As Double = DecRad * RAD_DEG

    '    DegToDMS(DecDeg)
    '    Dim DECfmt = DegSign & Format(Deg, "00") & "*" & Format(M, "00") & ":" & Format(S, "00") & "#"

    '    Dim y As Double = -Math.Cos(AltR) * Math.Cos(LatR) * Math.Sin(AzR)
    '    Dim x As Double = Math.Sin(AltR) - (Math.Sin(LatR) * SinDec)
    '    Dim A As Double = Math.Atan2(y, x)
    '    Dim B As Double = A * RAD_DEG
    '    Dim Hours As Double = B / 15

    '    Dim STdeg As Double = ST * 15
    '    Dim RaHoursR As Double = Hours - ST
    '    Dim RaHours As Double = Math.Abs(Hours - ST)

    '    If RaHours > 24 Then RaHours = RaHours - 24

    '    DegToDMS(RaHours)
    '    Dim RAfmt = Format(Deg, "00") & "*" & Format(M, "00") & ":" & Format(S, "00") & "." & Format(SS, "0") & "#"

    '    Form1.LogScope("ALT/AZ to RaH > ", Color.Black, RaHoursR)
    '    Form1.LogScope("ALT/AZ to RA > ", Color.Black, RAfmt)
    '    Form1.LogScope("ALT/AZ to DEC > ", Color.Black, DECfmt)
    '    'RichTextBoxCmds.SelectionColor = CmdColor
    '    'RichTextBoxCmds.SelectedText = NL & T & "ASCOM.TargetRightAscension = " & Format(RaHours, "0.000")
    '    'RichTextBoxCmds.SelectedText = NL & T & "Alt/Az to Ra = " & RAfmt

    '    'RichTextBoxCmds.SelectedText = NL & T & "ASCOM.TargetDeclination = " & Format(DecDeg, "0.000")
    '    'RichTextBoxCmds.SelectedText = NL & T & "Alt/Az to Dec = " & DECfmt

    '    Form1.driver.TargetDeclination = DecDeg
    '    Form1.driver.TargetRightAscension = RaHours

    'End Sub

    Private Sub CalcALTfromRADEC()
        ' This routine calculates Alt from RA, DEC, LATITUDE & SIDEREAL TIME

        Dim RA As Double = Form1.driver.RightAscension
        Dim LST As Double = Form1.driver.SiderealTime
        Dim DEC As Double = Form1.driver.Declination
        Dim LAT As Double = Form1.driver.SiteLatitude

        Dim RAdeg As Double = RA * 15
        Dim LSTdeg As Double = LST * 15
        Dim HA As Double
        HA = LSTdeg - RAdeg

        If HA < 0 Then HA = HA + 360

        Dim x As Double = Math.Cos(HA * (Math.PI / 180)) * Math.Cos(DEC * (Math.PI / 180))
        Dim y As Double = Math.Sin(HA * (Math.PI / 180)) * Math.Cos(DEC * (Math.PI / 180))
        Dim z As Double = Math.Sin(DEC * (Math.PI / 180))

        Dim xhor As Double = x * Math.Cos((90 - LAT) * (Math.PI / 180)) - z * Math.Sin((90 - LAT) * (Math.PI / 180))
        Dim yhor As Double = y
        Dim zhor As Double = x * Math.Sin((90 - LAT) * (Math.PI / 180)) + z * Math.Cos((90 - LAT) * (Math.PI / 180))

        Az = Math.Atan2(yhor, xhor) * (180 / Math.PI) + 180
        Alt = Math.Asin(zhor) * (180 / Math.PI)

        DegToDMS(Alt)

    End Sub

    Private Sub CalcAZfromRADEC()
        ' This routine calculates Az from RA, DEC, LATITUDE & SIDEREAL TIME

        Dim RA As Double = Form1.driver.RightAscension
        Dim LST As Double = Form1.driver.SiderealTime
        Dim DEC As Double = Form1.driver.Declination
        Dim LAT As Double = Form1.driver.SiteLatitude

        Dim RAdeg As Double = RA * 15
        Dim LSTdeg As Double = LST * 15
        Dim HA As Double
        HA = LSTdeg - RAdeg

        If HA < 0 Then HA = HA + 360

        Dim x As Double = Math.Cos(HA * (Math.PI / 180)) * Math.Cos(DEC * (Math.PI / 180))
        Dim y As Double = Math.Sin(HA * (Math.PI / 180)) * Math.Cos(DEC * (Math.PI / 180))
        Dim z As Double = Math.Sin(DEC * (Math.PI / 180))

        Dim xhor As Double = x * Math.Cos((90 - LAT) * (Math.PI / 180)) - z * Math.Sin((90 - LAT) * (Math.PI / 180))
        Dim yhor As Double = y
        Dim zhor As Double = x * Math.Sin((90 - LAT) * (Math.PI / 180)) + z * Math.Cos((90 - LAT) * (Math.PI / 180))

        Az = Math.Atan2(yhor, xhor) * (180 / Math.PI) + 180
        Alt = Math.Asin(zhor) * (180 / Math.PI)

        DegToDMS(Az)

    End Sub

End Module
