#!/usr/bin/python
# -*- coding:utf-8 -*-
import RPi.GPIO as GPIO
import serial
import time
from datetime import datetime
import os
import csv
import signal
import sys

# GPIO configuration
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

ser = serial.Serial('/dev/ttyUSB3', 115200)
ser.flushInput()

myhost = os.uname()[1]

# CSV file settings
csv_filename = f"gps_data_{myhost}.csv"
csv_headers = ["Timestamp", "Latitude", "Longitude", "Date", "Time", "Altitude", "Speed"]

# Global flag for graceful exit
running = True

def signal_handler(sig, frame):
    global running
    print('You pressed Ctrl+C! Stopping GPS tracking...')
    running = False

signal.signal(signal.SIGINT, signal_handler)

def parse_gps_data(gps_string):
    parts = gps_string.split(',')
    if len(parts) < 8 or all(part == '' for part in parts):
        return None
    
    try:
        lat = float(parts[0][:2]) + float(parts[0][2:]) / 60
        if parts[1] == 'S':
            lat = -lat
        
        lon = float(parts[2][:3]) + float(parts[2][3:]) / 60
        if parts[3] == 'W':
            lon = -lon
        
        date = parts[4]
        gps_time = parts[5]
        altitude = parts[6]
        speed = parts[7]
        
        return f"{lat:.6f}", f"{lon:.6f}", date, gps_time, altitude, speed
    except ValueError as e:
        print(f"Error parsing GPS data: {e}")
        return None

def write_to_csv(lat, lon, date, gps_time, altitude, speed):
    timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
    with open(csv_filename, mode='a', newline='') as file:
        writer = csv.writer(file)
        writer.writerow([timestamp, lat, lon, date, gps_time, altitude, speed])

def send_at(command, back, timeout):
    print(f"Sending command: {command}")
    ser.write((command + '\r\n').encode())
    time.sleep(timeout)
    if ser.inWaiting():
        time.sleep(0.01)
        rec_buff = ser.read(ser.inWaiting()).decode()
        print(f"Received data: {rec_buff}")
        if back not in rec_buff:
            print(command + ' ERROR')
            print(command + ' back:\t' + rec_buff)
            return 0
        else:
            try:
                gps_data = rec_buff.split('+CGPSINFO: ')[1].split('\r\n')[0]
                print(f"Extracted GPS data: {gps_data}")
                parsed_data = parse_gps_data(gps_data)
                if parsed_data:
                    lat, lon, date, gps_time, altitude, speed = parsed_data
                    print(f"Parsed GPS data: Lat: {lat}, Lon: {lon}, Date: {date}, Time: {gps_time}, Altitude: {altitude}, Speed: {speed}")
                    write_to_csv(lat, lon, date, gps_time, altitude, speed)
                    return 1
                else:
                    print("No valid GPS data found in this response")
                    return 0
            except IndexError:
                print("Unexpected response format")
                return 0
    else:
        print('No data received from GPS')
        return 0

def get_gps_position():
    answer = send_at('AT+CGPSINFO', '+CGPSINFO:', 1)
    if answer == 1:
        print("Valid GPS data received and processed")
    else:
        print('Waiting for GPS fix...')

def initialize_gps():
    print('Starting GPS')
    send_at('AT+CGPS=1,1', 'OK', 1)
    time.sleep(2)

    attempts = 0
    max_attempts = 10
    while attempts < max_attempts:
        print(f"Initialization attempt {attempts + 1} of {max_attempts}")
        if get_gps_position() == 1:
            print("GPS initialized successfully")
            return True
        attempts += 1
        time.sleep(2)
    
    print("Failed to initialize GPS after maximum attempts")
    return False

# Create CSV file with headers if it doesn't exist
if not os.path.exists(csv_filename):
    with open(csv_filename, mode='w', newline='') as file:
        writer = csv.writer(file)
        writer.writerow(csv_headers)

try:
    if initialize_gps():
        print("Starting continuous GPS tracking. Press Ctrl+C to stop.")
        while running:
            get_gps_position()
            time.sleep(10)  # Wait for 10 seconds before the next reading
    else:
        print("GPS initialization failed. Exiting.")
except Exception as e:
    print(f"Error: {e}")
finally:
    if ser:
        ser.close()
    GPIO.cleanup()
    print("GPS tracking stopped. Goodbye!")