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 = 'GPS_Vel_2.txt'
Error_Flag = 0

def Big_Display(N, x, y):
    T = 15
    c1 = LCD.RGB(250,250,250)
    c0 = 0
    if(N == 0):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,c1)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,c1)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,c1)
        LCD.Solid_Box(x,y+100,x+T,y+200,c1)
        LCD.Solid_Box(x,y,x+T,y+100,c1)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,0)
    if(N == 1):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,0)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,c1)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,0)
        LCD.Solid_Box(x,y+100,x+T,y+200,0)
        LCD.Solid_Box(x,y,x+T,y+100,0)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,0)
    if(N == 2):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,c1)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,c1)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,0)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,c1)
        LCD.Solid_Box(x,y+100,x+T,y+200,c1)
        LCD.Solid_Box(x,y,x+T,y+100,0)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,c1)
    if(N == 3):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,c1)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,c1)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,c1)
        LCD.Solid_Box(x,y+100,x+T,y+200,0)
        LCD.Solid_Box(x,y,x+T,y+100,0)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,c1)
    if(N == 4):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,0)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,c1)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,0)
        LCD.Solid_Box(x,y+100,x+T,y+200,0)
        LCD.Solid_Box(x,y,x+T,y+100,c1)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,c1)
    if(N == 5):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,c1)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,0)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,c1)
        LCD.Solid_Box(x,y+100,x+T,y+200,0)
        LCD.Solid_Box(x,y,x+T,y+100,c1)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,c1)
    if(N == 6):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,c1)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,0)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,c1)
        LCD.Solid_Box(x,y+100,x+T,y+200,c1)
        LCD.Solid_Box(x,y,x+T,y+100,c1)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,c1)
    if(N == 7):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,c1)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,c1)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,0)
        LCD.Solid_Box(x,y+100,x+T,y+200,0)
        LCD.Solid_Box(x,y,x+T,y+100,0)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,0)
    if(N == 8):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,c1)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,c1)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,c1)
        LCD.Solid_Box(x,y+100,x+T,y+200,c1)
        LCD.Solid_Box(x,y,x+T,y+100,c1)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,c1)
    if(N == 9):
        LCD.Solid_Box(x+T,y,x+100-T,y+T,c1)    
        LCD.Solid_Box(x+100-T,y,x+100,y+100,c1)
        LCD.Solid_Box(x+100-T,y+100,x+100,y+200,c1)
        LCD.Solid_Box(x+T,y+200-T,x+100-T,y+200,c1)
        LCD.Solid_Box(x,y+100,x+T,y+200,0)
        LCD.Solid_Box(x,y,x+T,y+100,c1)
        LCD.Solid_Box(x+T,y+100-T,x+100-T,y+100,c1)

def Display(Speed):
    X = int(Speed*10)
    A0 = X % 10
    X = X // 10
    A1 = X % 10
    X = X // 10
    A2 = X % 10
    Big_Display(A2, 50, 50)
    Big_Display(A1, 170, 50)
    Big_Display(A0, 300, 50)
    LCD.Solid_Box(280,235,295,250,LCD.RGB(250,250,250))
   
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]))
        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])
                lat = Str2Num(x[21:29])
                lon = Str2Num(x[35:43])
                speed = Str2Num(x[46:51])
    
    return([time, lat, lon, speed])
        
Navy = LCD.RGB(0,0,5)
White = LCD.RGB(200,200,200)
Pink = LCD.RGB(250,150,150)
LCD.Init()
LCD.Clear(Navy)

xref = 49.55054
yref = 52.11679
Record_Flag = 0

while(Button15.value() == 1):
    Error_Flag = 1
    while(Error_Flag == 1):
        Error_Flag = 0
        [t, x, y, v] = GPS_Read(0)
    if(Button14.value() == 0):
        Record_Flag = not Record_Flag
        if(Record_Flag):
            Beep()
            f = open(FileName, "a")
            print('Recording')
            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

    Display(v*1.15078)

    if(Record_Flag):
        msg = str('{:9.4f}'.format(v*1.15077) + ' ' )
        f.write(msg + '\n')

Beep()
Beep()
Beep()
print('stop')