import os
import time
import smbus
import numpy as np
import csv
import serial
import pynmea2
import warnings
import pytz
import datetime
        
from pynput import keyboard
from imusensor.MPU9250 import MPU9250
from imusensor.filters import kalman

warnings.filterwarnings('ignore')
tz = pytz.timezone("Turkey")

result = 0
def on_press(key):
    if key.char == ('d'):
        global result
        result = 1
        
def on_release(key):
    if key.char == ('d'):
        global result
        result = 0
        
listener = keyboard.Listener(
    on_press=on_press,
    on_release=on_release)
listener.start()

def port_setup(port):
    ser = serial.Serial(port, baudrate=9600, timeout=2)
    return ser

def parseGPSdata(ser):
        keywords = ["$GPRMC","$GPGGA"]
        gps_data = ser.readline()
        gps_data = gps_data.decode("utf-8")  # transform data into plain string
        
        if len(gps_data) > 5:
            if gps_data[0:6] in keywords:
                gps_msg = pynmea2.parse(gps_data)
                lat = gps_msg.latitude
                lng = gps_msg.longitude
                today = datetime.datetime.now(tz=pytz.utc)
                if gps_msg.timestamp is not None:
                    localT = datetime.datetime(today.year, today.month, today.day, gps_msg.timestamp.hour, gps_msg.timestamp.minute, gps_msg.timestamp.second, tzinfo=pytz.utc)
                    date = localT.astimezone(tz).isoformat()
                else:
                    date = today
                return (date,lat,lng)
            else:
                return None
        else:
            return None

gps_port = "/dev/serial0"
ser = port_setup(gps_port)
    
address = 0x68
bus = smbus.SMBus(1)
imu = MPU9250.MPU9250(bus, address)
imu.begin()

sensorfusion = kalman.Kalman()

imu.readSensor()
imu.computeOrientation()
sensorfusion.roll = imu.roll
sensorfusion.pitch = imu.pitch
sensorfusion.yaw = imu.yaw

count = 0
currTime = time.time()

#os.remove('/home/pi/Desktop/sarsinti/data.csv')
f = open('/home/pi/Desktop/sarsinti/data.csv', 'w')
writer = csv.writer(f)

row = ['roll','pitch','date','lat','lon','result']
writer.writerow(row)
f.close()

while True:
    try:
        imu.readSensor()
        imu.computeOrientation()
        newTime = time.time()
        dt = newTime - currTime
        currTime = newTime
        sensorfusion.computeAndUpdateRollPitchYaw(imu.AccelVals[0], imu.AccelVals[1], imu.AccelVals[2], imu.GyroVals[0], imu.GyroVals[1], imu.GyroVals[2],\
                                                  imu.MagVals[0], imu.MagVals[1], imu.MagVals[2], dt)
        
        gps_coords = parseGPSdata(ser)
        
        if gps_coords is not None:
            row = [sensorfusion.roll, sensorfusion.pitch, gps_coords[0], gps_coords[1], gps_coords[2], result]
            f = open('/home/pi/Desktop/sarsinti/data.csv', 'a')
            writer = csv.writer(f)
            writer.writerow(row)
            f.close()
            print("Kalmanroll:{0} KalmanPitch:{1} DateUTC: {2} GPSLat: {3} GPSLon: {4} Result: {5}".format(sensorfusion.roll, sensorfusion.pitch, gps_coords[0], gps_coords[1], gps_coords[2], result))
        
        time.sleep(0.01)
        
    except KeyboardInterrupt as e:
           f.close()
           break