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)

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

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

FileName = 'Diffy_GPS_4.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 = chr(ord(X[i:i+1]))
        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[4] == '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, LatM, LonM, speed])
      
Navy = LCD.RGB(0,0,5)
White = LCD.RGB(200,200,200)
Pink = LCD.RGB(250,150,150)
LCD.Init()
LCD.Clear(Navy)


LCD.Title(FileName,White, Navy)
LCD.Text2('x0', 5, 50, White, Navy)
LCD.Text2('y0', 5, 90, White, Navy)
LCD.Text2('x1', 5, 130, White, Navy)
LCD.Text2('y1', 5, 170, White, Navy)
LCD.Text2('v0', 5, 210, White, Navy)
LCD.Text2('v1', 5, 250, 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
        [t0, x0, y0, v0] = GPS_Read(0)
        [t1, x1, y1, v1] = GPS_Read(1)
    if(Button15.value() == 0):
        xref = x0
        yref = y0
        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
        
    dx0 = (x0 - xref) * 111046.57 / 60  # meters
    dy0 = (y0 - yref) * 75976.33 / 60
    dx1 = (x1 - xref) * 111046.57 / 60  # meters
    dy1 = (y1 - yref) * 75976.33 / 60
    LCD.Number2(dx0, 11, 4, 100, 50, White, Navy)
    LCD.Number2(dy0, 11, 4, 100, 90, White, Navy)
    LCD.Number2(dx1, 11, 4, 100, 130, White, Navy)
    LCD.Number2(dy1, 11, 4, 100, 170, White, Navy)
    LCD.Number2(v0*0.5144, 11, 4, 100, 210, White, Navy)
    LCD.Number2(v1*0.5144, 11, 4, 100, 250, White, Navy)
 
    msg0 = str('{:9.4f}'.format(dx0) + ' ' + '{:9.4f}'.format(dy0) + ' ' )
    msg1 = str('{:9.4f}'.format(dx1) + ' ' + '{:9.4f}'.format(dy1) + ' ' )
    msg2 = str('{:9.4f}'.format(v0) + ' ' +  '{:9.4f}'.format(v1) + ' ')
    msg = msg0 + msg1 + msg2
    print(msg)
    if(Record_Flag):
        f.write(msg + '\n')