from machine import UART, Pin
from time import sleep
import LCD
from math import sin, cos, pi

uart0 = UART(0, 9600)
uart0.init(9600, bits=8, parity=None, stop=1, tx=0, rx=1)

Button14 = Pin(14, Pin.IN, Pin.PULL_UP)
Button15 = Pin(15, Pin.IN, Pin.PULL_UP)
Beeper = Pin(13, Pin.OUT)

FileName = 'WheresMyCar.txt'
Error_Flag = 0

def Beep():
    Beeper.value(1)
    sleep(0.1)
    Beeper.value(0)
    
def Str2Num(X):
    global Error_Flag
    n = len(X)
    y = 0
    flag = 0
    k = 0
    for i in range(0,n):
        z = X[i]
        if(z in {'0','1','2','3','4','5','6','7','8','9','0','.'}):
            if(z == '.'):
                flag = 1
            else:
                if(flag == 0):
                    y = 10*y + int(z)
                else:
                    k -= 1
                    y = y + int(z) * (10 ** k)
        else:
            Error_Flag = 1
    return(y)

def GPS_Read_Line(chan):
    flag = 0
    n = 0
    msg = ''
    while(flag == 0):
        if(chan == 0):
            x = uart0.read(1)
        else:
            x = uart1.read(1)
        if(x != None):
            x = ord(x)
            if(chr(x) == '$'):
                msg = ''
            if(x == 13):
                flag = 1
            else:
                msg = msg + chr(x)
    return(msg)

    
def GPS_Read(chan):
    flag = 0
    
    while(flag == 0):
        x = GPS_Read_Line(chan)
        if(len(x) > 52):
            if(x[3] == 'R'): # $GPRMC
                flag = 1   
                time = Str2Num(x[7:16])
                LatD = Str2Num(x[19:21])
                LatM = Str2Num(x[21:29])
                LonD = Str2Num(x[32:35])
                LonM = Str2Num(x[35:43])
                speed = Str2Num(x[46:51])
            if(x[0] == 'G'): # x[4] = 'G'$GPGGA
                flag = 1
                time = Str2Num(x[7:16])
                LatD = Str2Num(x[17:19])
                LatM = Str2Num(x[19:27])
                LonD = Str2Num(x[30:33])
                LonM = Str2Num(x[33:41])
                speed = 0
        
    return([time, LatD, LatM, LonD, LonM, speed])
        
        
while(0):
    msg = GPS_Read_Line(0)
    print(msg)
    
while(1):
    [t, Yd, Ym, Xd, Xm, v] = GPS_Read(0)
    print(t, Yd, Ym, Xd, Xm, v)
    

Navy = LCD.RGB(0,0,5)
White = LCD.RGB(250,250,250)
Pink = LCD.RGB(250,150,150)
LCD.Init()
LCD.Clear(Navy)


LCD.Title(FileName,White, Navy)
LCD.Text2('North', 5, 50, White, Navy)
LCD.Text2('West', 5, 90, White, Navy)
LCD.Text2('N(m)', 5, 130, White, Navy)
LCD.Text2('W(m)', 5, 170, White, Navy)

xref = 49.55054
yref = 52.11679

f = open(FileName, "a")
Record_Flag = 0
while(1):
    Error_Flag = 1
    while(Error_Flag == 1):
        Error_Flag = 0
        [t, yd, ym, xd, xm, v] = GPS_Read(0)
    if(Button15.value() == 0):
        xref = xm
        yref = ym
        print('Home = Current GPS Position')
    if(Button14.value() == 0):
        Record_Flag = not Record_Flag
        if(Record_Flag):
            Beep()
            f = open(FileName, "a")
            print('File Open')
            LCD.Text('Recording',5,5,Pink,Navy)
        else:
            Beep()
            sleep(0.1)
            Beep()
            f.close()
            print('File Closed')
            LCD.Text('         ',5,5,Pink,Navy)
        while(Button14.value() == 0):
            pass
        
    dx = (xm - xref) * 111046.57 / 60  # meters
    dy = (ym - yref) * 75976.33 / 60
 
    msg0 = str('{:3.0f}'.format(xd) + ' ' + '{:11.7f}'.format(xm) + ' ' )
    msg1 = str('{:3.0f}'.format(yd) + ' ' + '{:11.7f}'.format(ym) + ' ' )
    msg2 = str('{:9.4f}'.format(dx) + ' ')
    msg3 = str('{:9.4f}'.format(dy) + ' ' )
    msg = msg0 + msg1 + msg2 + msg3
    print(msg)
    
    LCD.Text2(msg0, 100, 50, White, Navy)
    LCD.Text2(msg1, 100, 90, White, Navy)
    LCD.Text2(msg2, 100, 130, White, Navy)
    LCD.Text2(msg3, 100, 170, White, Navy)
 
    if(Record_Flag):
        f.write(msg + '\n')