Option Base 0
Option Default None
Option Explicit On

Cls

Const VERSION$ = "0.25"

' **** Constants controlling hardware configuration and diagnostics ***********

' For testing purposes the code can run on the Colour Maximite 2.
Const IS_CMM2% = Left$(Mm.Device$, 17) = "Colour Maximite 2"
If Not IS_CMM2% And Mm.Device$ <> "PicoMite" Then Error "Unknown device"

If IS_CMM2% Then
 Option Console Serial
 Const WIDTH% = 96
 Const HEIGHT% = 64
Else
 Const WIDTH% = Mm.HRes
 Const HEIGHT% = Mm.VRes
EndIf

' Set these to 1 to interface with real hardware or 0 to use the keyboard alternative:
'  [A]     - accelerate
'  [Z]     - decelerate
'  [<]     - decrease battery voltage
'  [>]     - increase battery voltage
'  [Space] - toggle MPH/KPH
'  [X]     - toggle GPS valid/invalid
Const REAL_GPS% = 1
Const REAL_BATTERY% = 1
Const REAL_MPH_TOGGLE% = 1

' 1 to display diagnostics on the OLED, 0 for normal operation.
Const DIAGNOSTICS_ON% = 0

' If this is > 0 then generate debug output on this interval in milliseconds.
Const DEBUG_INTERVAL% = 6000 ' 6 seconds.

' *****************************************************************************

' If the speed is < 3 (MPH or KPH depending on toggle) for this duration then
' we transition from the DRIVING state to the STOPPING state.
' 10 seconds when REAL_GPS% = 0, otherwise 1 minute
Const DELAY% = Choice(REAL_GPS%, 60, 10) * 1000

' These are the states that the device can be in:
Const STATE_STARTING%   = 1
Const STATE_DRIVING%    = 2
Const STATE_STOPPING%   = 3
Const STATE_STOPPED%    = 4
Const STATE_WAKING%     = 5
Const STATE_RESTARTING% = 6

Const BATTERY_MIN! = 3.2     ' If the battery voltage falls below this the low
                            ' battery warning will be shown.

' State variables:
Dim gps_valid%               ' 1 for valid, 0 for invalid.
Dim mph_flag%                ' 1 for MPH, 0 for KPH.
Dim speed!                   ' Current speed in MPH or KPH as above.
Dim battery!                 ' Battery voltage.
Dim state% = STATE_STARTING% ' Current state, one of the STATE_XXXX% constants.

' When using the keyboard rather than the real hardware these are the variables
' that the keyboard manipulates.
Dim test_gps_valid% = 1      ' Default to GPS signal valid.
Dim test_mph_flag%  = 1      ' Default to MPH.
Dim test_speed!     = 0      ' Note this is the speed in knots.
Dim test_battery!   = 5      ' Default to 5V.

If REAL_GPS% Then
 SetPin GP0, GP1, COM1
 Open "COM1:460800" As Gps  ' Sets GPS pins
EndIf

If REAL_BATTERY% Then SetPin GP26, AIN  ' This pin is always 1/2 battery voltage

If REAL_MPH_TOGGLE% Then SetPin GP14, DIN, PULLUP  ' MPH/KPH Select - 3.3V = MPH, 0V = KPH

' Enable keyboard interrupt unless we are using entirely real hardware.
If Not REAL_GPS% Or Not REAL_BATTERY% Or Not REAL_MPH_TOGGLE% Then
 On Key on_key
EndIf

' Generate debug output on the given interval.
If DEBUG_INTERVAL% > 0 Then
 Print "PicoMite GPS Speedo v" VERSION$
 print_debug_ex("POWER ON")
 SetTick DEBUG_INTERVAL%, print_debug
EndIf

Timer = 0

clear_display()

Do
 read_inputs()
 update_state()
 update_display()

 If state% = STATE_STOPPED% And DIAGNOSTICS_ON% = 0 Then
   cpu_sleep(5) ' Sleep CPU for 5 seconds to save battery.
 EndIf

 ' Lewis you might consider adding an ELSE clause to the above, might make
 ' display marginally smoother and means you aren't constantly polling the GPS:
 ' Else
 '   Pause 100
 ' EndIf
Loop

End

Sub clear_display()
 If IS_CMM2% Then
   ' On the CMM2 we draw a box to simulate the OLED size.
   Box 0, 0, WIDTH%, HEIGHT%, 1, Rgb(Gray), Rgb(Black)
 Else
   Cls
 EndIf
End Sub

Sub read_inputs()
 ' Check if the GPS has a valid lock:
 gps_valid% = Choice(REAL_GPS%, Gps(Valid), test_gps_valid%)

 ' Read the M/K select pin:
 mph_flag% = Choice(REAL_MPH_TOGGLE%, Pin(GP14), test_mph_flag%)

 ' Read speed and adjust according to M/K select pin
 ' - note this may not actually be correct if gps_valid% = 0.
 speed! = Choice(REAL_GPS%, Gps(Speed), test_speed!)
 speed! = speed! * Choice(mph_flag%, 1.15077945, 1.852)

 ' Read battery voltage:
 battery! = Choice(REAL_BATTERY%, 2 * Pin(GP26), test_battery!)
End Sub

' Decides what the unit is doing.
Sub update_state()
 Local old_state% = state%

 Select Case state%
   Case STATE_STARTING%
     If Timer > 1000 Then state% = STATE_DRIVING%

   Case STATE_DRIVING%
     If speed! < 3 Then
       Timer = 0
       state% = STATE_STOPPING%
     EndIf

   Case STATE_STOPPING%
     If speed! >= 3 Then
       state% = STATE_DRIVING%
     ElseIf Timer > DELAY% Then
       state% = STATE_STOPPED%
     EndIf

   Case STATE_STOPPED%
     If speed! >= 3 Then
       Timer = 0
       state% = STATE_RESTARTING%
     Else
       Timer = 0
       state% = STATE_WAKING%
     EndIf

   Case STATE_WAKING%
     If speed! >= 3 Then
       Timer = 0        
       state% = STATE_RESTARTING%
     Else If Timer > 200 Then
       state% = STATE_STOPPED%
     EndIf

   Case STATE_RESTARTING%
     If Timer > 2000 Then state% = STATE_DRIVING%

   Case Else
     Error "Unknown state"
     
 End Select

 ' Always log state changes.
 If state% <> old_state% Then print_debug()
End Sub

Sub update_display()
 If DIAGNOSTICS_ON% Then
   update_diagnostics_display()
   Exit Sub
 EndIf

 ' Static variables are like Dim'd (global) variables except they are local to the
 ' Sub or Function, they are only initialised the first time they are encountered.
 Static old_state% = -1
 Static old_dvalue$ = ""
 Static old_dfont% = -1
 Static old_dcolour% = -1
 Static old_dindicator% = -1
 Static old_dunits$= ""

 ' If we are still STOPPED/WAKING then don't update the display.
 If stopped_or_waking%(state%) And stopped_or_waking%(old_state%) Then Exit Sub

 Local dvalue$ = display_value$()
 Local dfont% = display_font%()
 Local dcolour% = display_colour%()
 Local dindicator% = display_indicator%()
 Local dunits$ = display_units$()

 ' If nothing has changed then don't update the display
 ' - a single line containing 5 AND statements is too long to work with, so do it this way.
 Local changed%
 Inc changed%, old_dvalue$ <> dvalue$
 Inc changed%, old_dfont% <> dfont%
 Inc changed%, old_dcolour% <> dcolour%
 Inc changed%, old_dindicator% <> dindicator%
 Inc changed%, old_dunits$ <> dunits$
 If Not changed% Then Exit Sub

 clear_display()

 If Not stopped_or_waking%(state%) Then
   Text WIDTH%/2, HEIGHT%/2, dvalue$, CM, dfont%, 1, dcolour%
   Circle 89, 5, 4, 1, 1, dindicator%, dindicator%
   Text 85, 62, dunits$, LB, 4, 1, Rgb(White)

   ' Uncomment to show version on STARTING/RESTARTING displays.
   If state% = STATE_STARTING% Or state% = STATE_RESTARTING% Then
     Text 2, 58, "v" + VERSION$, LB, 7
   EndIf
 EndIf

 old_state% = state%
 old_dvalue$ = dvalue$
 old_dfont% = dfont%
 old_dcolour% = dcolour%
 old_dindicator% = dindicator%
 old_dunits$= dunits$
End Sub

Function stopped_or_waking%(st%)
 stopped_or_waking% = (st% = STATE_STOPPED% Or st% = STATE_WAKING%)
End Function

Function display_indicator%()
 display_indicator% = Choice(gps_valid%, Rgb(Green), Rgb(Black))
End Function

Function display_value$()
 If state% = STATE_STARTING% Or state% = STATE_RESTARTING% Then
   display_value$ = Str$(battery!, 1, 1) + "V"
 ElseIf Not gps_valid% Then
   display_value$ = "--"
 Else
   display_value$ = Str$(speed!, 0, 0)
 EndIf
End Function

Function display_font%()
 display_font% = Choice(state% = STATE_STARTING% Or state% = STATE_RESTARTING%, 3, 6)
End Function

' Returns colour to be used for speed display depending on current speed/state.
Function display_colour%()
 If state% = STATE_STARTING% Or state% = STATE_RESTARTING% Then
   display_colour% = Rgb(White)
 ElseIf stopped_or_waking%(state%) Then
   ' We should never actually get to this point as we don't show the display
   ' at all under these conditions.
   display_colour% = Rgb(Black)
 ElseIf state% = STATE_STOPPING% Or Not gps_valid% Then
   display_colour% = Rgb(0, 128, 0)
 ElseIf speed! > Choice(mph_flag%, 71, 121) Then ' RED if Speed is 71Mph/121Kph or more
   display_colour% = Rgb(Red)
 ElseIf speed! > Choice(mph_flag%, 61, 101) Then ' ORANGE if Speed is 61Mph/101Kph or more
   display_colour% = Rgb(Orange)
 Else
   display_colour% = Rgb(Green)
 EndIf
End Function

Function display_units$()
 If battery! < BATTERY_MIN! Then
   display_units$ = "B" ' If battery < 3.2V then replace K/M with B.
 ElseIf speed! > 99 Then
   display_units$ = ""  ' If speed > 99 then don't show K/M indicator.
 ElseIf mph_flag% Then
   display_units$ = "M"
 Else
   display_units$ = "K"
 EndIf
End Function

Sub update_diagnostics_display()
 Static count% = 1000
 Inc count%
 If count% < 1000 Then Exit Sub

 clear_display()

 Text 2, 2,  "GPS valid: " + Str$(gps_valid%), , 7
 Text 2, 12, "MPH flag:  " + Str$(mph_flag%), , 7
 Text 2, 22, "Speed: "     + Str$(speed!, 0, 3), , 7
 Text 2, 32, "Battery: "   + Str$(battery!, 0, 3), , 7
 Text 2, 42, "State: "     + Left$(state_as_string$(), 8), , 7

 count% = 0
End Sub

Sub cpu_sleep(duration_in_seconds%)
 print_debug_ex("*SLEEPING*")
 If REAL_GPS% Then
   Cpu Sleep duration_in_seconds%
 Else
   Pause duration_in_seconds% * 1000
 EndIf
End Sub

Sub on_key()
 Select Case Inkey$
   Case "a", "A" : test_speed! = Min(100, test_speed! + 0.5)
   Case "z", "Z" : test_speed! = Max(0, test_speed! - 0.5)
   Case ".", ">" : test_battery! = Min(5, test_battery! + 0.2)
   Case ",", "<" : test_battery! = Max(0, test_battery! - 0.2)
   Case "x", "X" : test_gps_valid% = Not test_gps_valid%
   Case " " : test_mph_flag% = Not test_mph_flag%
 End Select
End Sub

' Note that because of how SETTICK interrupts are implemented in MMBasic it is
' possible if unlikely that this will occasionally be called partway through
' a call to read_inputs() resulting in inconsistent data, e.g. MPH flag set true,
' but Speed reported in KPH.
Sub print_debug()
 print_debug_ex(state_as_string$())
End Sub

Sub print_debug_ex(s$)
 If DEBUG_INTERVAL% <= 0 Then Exit Sub

 ' Note that without a clock installed Time$ = 00:00:00 will be when the unit started up.
 Print Time$ " - GPS valid:" gps_valid% ", MPH flag:" mph_flag%;
 Print ", Spd: " Str$(speed!, 0, 3) ", Bat: " Str$(battery!, 0, 3) ", State: " s$
End Sub

Function state_as_string$()
 Select Case state%
   Case STATE_STARTING%:   state_as_string$ = "STARTING"
   Case STATE_DRIVING%:    state_as_string$ = "DRIVING"
   Case STATE_STOPPING%:   state_as_string$ = "STOPPING"
   Case STATE_STOPPED%:    state_as_string$ = "STOPPED"
   Case STATE_WAKING%:     state_as_string$ = "WAKING"
   Case STATE_RESTARTING%: state_as_string$ = "RESTARTING"
   Case Else:              state_as_string$ = "ERROR"
 End Select
End Function