#!/usr/bin/python3
# -*- coding: utf-8 -*-

import argparse
import re
from subprocess import run, PIPE
import sys
import asyncio,signal, traceback, gpsd, mgrs, zmq, os, time, logging
from datetime import datetime
from pytz import timezone, utc
from configobj import ConfigObj
from timezonefinder import TimezoneFinder
from logging.handlers import RotatingFileHandler

APP_VERSION = "4.1.8"

Changelog = """
4.1.5 - 2025-02-17
    - round up to 5 decimal places in GPS.data with ±1.1m accuracy
    - that GPS.data is sent to DMC daemon
4.1.6 - 2025-02-17
    # - count only if 3D fix is available
    - Compare prev lat/lon within ±1.1m accuracy
    - read position at each 0.5sek
4.1.7 - 2025-02-18
    - Updated version number to 4.1.7
4.1.8 - 2025-02-19
    - Cleaned GPS_parse() function, only 2 cases: <2 and >=2
    - count position_precision() > 10 as invalid and add @ to coordinates
4.1.9 - 2025-02-19
    - compare current and previous position with 5 decimal places (±1.1m)
    - Intro LOCATION_PRESISION = 20 for gps_data.position_precision() > 20m and add @ to coordinates
4.1.10 - 2025-02-20
    - add error simbol "@" to both coordinates if gps_data.position_precision() > 20m
"""

### Logging ###

rotate_handler = RotatingFileHandler("/var/log/gpsctl.log", mode='a', maxBytes=10*1024*1024, backupCount=3, encoding=None, delay=0)
log_formatter = logging.Formatter('%(name)s:%(asctime)s - %(levelname)s: %(message)s',"%Y-%m-%d %H:%M:%S")
rotate_handler.setFormatter(log_formatter)
log = logging.getLogger('GPSctl')
log.addHandler(rotate_handler)
log.propagate = False
log.setLevel(logging.INFO)

################

### Read global config
conf = ConfigObj('/etc/bdr.conf') # GPS_DAEMON_SLEEP variable and other global

# ovrl_cmd = '/usr/local/bin/gstd-client element_set cam_src_pipe location text '
# tz_obj=TimezoneFinder(in_memory=True)
#tz_obj=TimezoneFinder()

CMDDEBUG = False
prev_sats = 0
prev_longitude = 0
prev_latitude = 0
LOCATION_PRECISION = 20
GPS_SELF_LOG = "/home/user/bidentifier/gpslocation"
VAR_TRACK_LOG = '/var/log/gpstrack.log'

SELF_GPS = "6105"
DUMMY_GPS = "FE00"
NO_LOC_GPS = "FE01"


### ZMQ
context = zmq.Context()
socket = context.socket(zmq.PUB)
socket.connect("tcp://localhost:5555")
time.sleep(1)

websimulator = "/bdr/websimulator.py"

### Connect to the local gpsd ###
gpsd.connect() # gpsd.connect(host="127.0.0.1", port=2947)
location_current = None
m = mgrs.MGRS()

class CmdTool:
    def __init__(self, name, cmd):
        self.name = name
        self.cmd = cmd

    def to_arg_list(self, *argv):
        args = self.cmd.split()
        for arg in argv:
            args.append(arg)

        return args

    def run(self, *argv):
        cmd_args = self.to_arg_list(*argv)
        cmd_out = run(cmd_args, stdout=PIPE, stderr=PIPE, universal_newlines=True)
        rc = cmd_out.returncode
        try:
            output = cmd_out.stdout  # int(cmd_out.stdout, base=10)
        except Exception:
            output = None

        return rc, output

def parse_arguments():
    parser = argparse.ArgumentParser()
    parser.add_argument('--debug', action='store_true', help="DEBUG mode.")
    parser.add_argument('-V', '--version', action='store_true', help='Display the application version.')
    return parser.parse_args()

def deleteContent(fName):
    open(fName, "w").close()

def split_mgrs(mgrs_string):

    match = re.match(r'^(\d{1,2}[A-Z])([A-Z]{2})(\d+)$', mgrs_string)

    zone = match.group(1)
    square_id = match.group(2)
    coordinates = match.group(3)

    half_len = len(coordinates) // 2
    easting = coordinates[:half_len]
    northing = coordinates[half_len:]

    return "{}|{}|{}|{}".format(zone, square_id, easting, northing)


def cmd_to_mcu(opcode,data):

    send_buff = []
    send_buff.append("E6")
    send_buff.append("00")
    send_buff.append(opcode[:2])
    send_buff.append(opcode[2:])
    for ch in data:      #String
        # if CMDDEBUG:
        #     print(ch)
        send_buff.append(hex(ord(ch))[2:])
    send_buff.append('FF') #ChSum

    # send_buff[1]=str(hex(len(send_buff))).lower() #Replace lenght byte
    strlen = len(send_buff)
    send_buff[1]=str("{0:02x}".format(strlen))

    cmd=''.join(ch for ch in send_buff)
    if CMDDEBUG:
        print("Lenght {} cmd {}".format(strlen,cmd))
        log.debug("Lenght {} cmd {}".format(strlen,cmd))

    CmdTool("cmd_to_mcu",websimulator).run(cmd)
    send_buff.clear()

# def timezone_refresh (current_lat,current_lon,datenew):
#     log.debug('Update TZ')

#     # Get current TZ
#     tz_file=open("/etc/timezone","r")
#     current_tz=tz_file.read()
#     tz_file.close()
#     log.debug('System TZ is: %s', current_tz)

#     # Get GPS TZ
#     tz_obj=TimezoneFinder(in_memory=True)
#     gps_tz=tz_obj.certain_timezone_at(lng=current_lon, lat=current_lat)
#     log.debug('GPS TZ is: %s', gps_tz)

#     if gps_tz.strip() != current_tz.strip():
#         # os.system("timedatectl set-timezone " + str(gps_tz))
#         # os.system("timedatectl set-local-rtc 0")
#         newdate="date -s" + "\"" + str(datenew) + "\""
#         os.system(newdate)
#         os.system("hwclock -w")
#         log.info('Refresh TZ to %s Date is %s', gps_tz, datenew)
#     else:
#         log.debug('Current TZ is the same: %s Date is %s', current_tz, datenew)
#         newdate='date -s"' + str(datenew) + '"'
#         os.system(newdate)

def set_offset (current_lat,current_lon):

    log.debug('Get UTC offset')

    format = "%Y-%m-%d %H:%M:%S %Z%z"
    # today = datetime.now()
    now_utc = datetime.now(timezone('UTC'))

    # Get current TZ
    tz_file=open("/etc/timezone","r")
    current_tz=tz_file.read()
    tz_file.close()
    log.debug('System TZ is: %s', current_tz)

    # Get GPS TZ
    tf=TimezoneFinder(in_memory=True)
    gps_tz=tf.certain_timezone_at(lng=current_lon, lat=current_lat)

    if gps_tz.strip() != current_tz.strip():
        now_current_tz = now_utc.astimezone(timezone(gps_tz))
        newdate='date -s"' + str(now_current_tz) + '"'
        os.system(newdate)
        # log.debug("{}: UTC:{} GPSTZ:{}".format(now_current_tz.strftime(format),now_utc,gps_tz))


def gps_parse(configuration):

    global prev_longitude
    global prev_sats
    global prev_latitude

    try:
        gps_data = gpsd.get_current()

        if gps_data.mode < 2:

            log.debug('GPS mode {} sats {}'.format(gps_data.mode,gps_data.sats))
            os.system("echo 0 > /proc/bdr/gpslock")
            # deleteContent('/home/user/bidentifier/gpslocation')

            if prev_sats != gps_data.sats:
                cmd_to_mcu(NO_LOC_GPS,"No location @{}".format(gps_data.sats))
                prev_sats = gps_data.sats
                prev_latitude = 0
                prev_longitude = 0

            time.sleep(1.0)

        # elif gps_data.mode >= 2:
        else:

            os.system("echo 1 > /proc/bdr/gpslock")
            # time.sleep(int(str(configuration['GPS_DAEMON_SLEEP'])))
            time.sleep(3)

            # latitude = "{:.5f}".format(gps_data.lat)
            # longitude = "{:.5f}".format(gps_data.lon)
            # altitude = "{:.0f}".format(gps_data.alt)
            latitude = gps_data.lat
            longitude = gps_data.lon
            altitude = gps_data.alt
            location = str(latitude) + " " + str(longitude)
            # location = str(float(latitude))+" "+str(float(longitude))

            log.debug('GPS mode=%s location %s altitude %s err %s', str(gps_data.mode), str(location), str(altitude), gps_data.position_precision()[0])

            # if location_current != location:
            # if "{:.5f}".format(prev_latitude) != "{:.5f}".format(latitude) or "{:.5f}".format(prev_longitude) != "{:.5f}".format(longitude):
            # if prev_latitude != latitude or prev_longitude != longitude:
            if round(prev_latitude, 5) != round(latitude, 5) or round(prev_longitude, 5) != round(longitude, 5):

                ###     Loging data                     ###
                log.info('>>> Change in GPS sats %s\nposition %s\n      was %s %s',gps_data.sats,gps_data.position(),prev_latitude,prev_longitude)

                prev_latitude = latitude
                prev_longitude = longitude

                ###     Check&Set TimeZone
                # getdateandtime=str(packet.get_time(local_time=True))
                # getdate=getdateandtime.split(' ',1)[1]
                # timezone_refresh(latitude,longitude,getdate)
                set_offset(latitude,longitude)

                ###     Keep track to file "tracking"   ###
                with open (VAR_TRACK_LOG,'a') as file:
                    file.write(
                        "%s: %s  %s errs: %s\n"% (
                        str(gps_data.get_time(local_time=True)),
                        gps_data.position(),
                        gps_data.map_url(),
                        gps_data.position_precision()
                        )
                    )

                 ### Send to DMC deaemon gps data with ZMQ
                ## TODO - Should be string.encode('utf-8') ?
                log.debug("Send to ZMQ: {} {} {} {}".format("GPS",latitude,longitude,altitude))
                socket.send_string("%s %s %s" % ("GPS",latitude,longitude))

                possition_error, elevation_error = gps_data.position_precision()
                log.debug("Position error {}m > {}m".format(possition_error, LOCATION_PRECISION))

                if str(configuration['MILITARY']).strip().lower() == 'true':
                    # 12Q FJ 12345 67890
                    # 12QFJ1234567890
                    mgrs_string = m.toMGRS(float(latitude),float(longitude))
                    if possition_error > LOCATION_PRECISION:
                        data_string = "@" + mgrs_string + "|" + str(altitude)
                    else:
                        data_string = mgrs_string + "|" + str(altitude)
                    log.debug("Latitude {} longitude {} altitude {} to MGRS string: {}".format(latitude, longitude, altitude, data_string))
                else:
                    if possition_error > LOCATION_PRECISION:
                        data_string = "@{:.5f}|@{:.5f}|{}".format(latitude, longitude, int(altitude))
                    else:
                        data_string = "{:.5f}|{:.5f}|{}".format(latitude, longitude, int(altitude))
                    log.debug("Latitude {} longitude {} altitude {} to string: {}".format(latitude, longitude, altitude, data_string))


                cmd_to_mcu(DUMMY_GPS, data_string)
                log.debug('GPS coords to MCU {}'.format(data_string))

                ###     Write GPS Mode 2/3 and data     ###
                with open(GPS_SELF_LOG,'w') as gpslocation:
                    gpslocation.write(str(data_string))

                ###     Logging data                     ###
                # log.info('Change in GPS sats %s position %s',gps_data.sats,gps_data.position())

            #     if str(configuration['MILITARY']).strip().lower() == 'true':
            #         # 12Q FJ 12345 67890
            #         # 12QFJ1234567890
            #         mgrs_string = m.toMGRS(float(latitude),float(longitude))
            #         # data_string = split_mgrs(data_string)
            #         data_string = mgrs_string + "|" + str(altitude)
            #         log.debug("Latitude {} longitude {} to MGRS+alt: {}".format(latitude,longitude,data_string))
            #     else:
            #         data_string = str(latitude) + "|" + str(longitude) + "|" + str(altitude)
            #     cmd_to_mcu(DUMMY_GPS,str(data_string))

        # else:
        # #     # time.sleep(int(str(conf['GPS_DAEMON_SLEEP'])))
        #     os.system("echo 0 > /proc/bdr/gpslock")
        #     deleteContent('/home/user/bidentifier/gpslocation')
        #     prev_latitude = 0
        #     prev_longitude = 0
        #     log.warning("No location @{}".format(gps_data.sats))
        #     cmd_to_mcu(NO_LOC_GPS,"No location @{}".format(gps_data.sats))


    except Exception as e:
        os.system("echo 0 > /proc/bdr/gpslock")
        deleteContent('/home/user/bidentifier/gpslocation')
        prev_latitude = 0
        prev_longitude = 0
        cmd_to_mcu(NO_LOC_GPS,"")
        log.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))

async def shutdown(sig, loop):
    try:

        cmd_to_mcu(DUMMY_GPS,"                  ")
        # signal the remaining tasks in the loop that its time to stop
        tasks = [task for task in asyncio.Task.all_tasks() if task is not
                 asyncio.tasks.Task.current_task()]
        list(map(lambda task: task.cancel(), tasks))
        await asyncio.gather(*tasks, return_exceptions=True)
        # stop the loop
        loop.stop()

    except Exception as e:
        logging.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))

def main_loop(loop):

    while True:

        try:

            # set_offset(42.51,24.14)
            conf = ConfigObj('/etc/bdr.conf') # GPS_DAEMON_SLEEP variable and other global vars

            try:
                gps_parse(conf)
            except Exception as str_error:
                log.error('Error gps_parse {}'.format(str_error))
                continue

        except Exception as e:
            log.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))

if __name__ == "__main__":

    args = parse_arguments()

    if args.debug:
        log.setLevel(logging.DEBUG)
    elif args.version:
        print("Version: {}".format(APP_VERSION))
        sys.exit()

    loop = asyncio.get_event_loop()
    asyncio.ensure_future(main_loop(loop), loop=loop)

    try:
        loop.add_signal_handler(getattr(signal, 'SIGINT'), lambda: asyncio.ensure_future(shutdown('SIGINT', loop)))
        loop.add_signal_handler(getattr(signal, 'SIGTERM'),lambda: asyncio.ensure_future(shutdown('SIGTERM', loop)))
    except Exception as e:
        logging.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))

    try:
        loop.run_forever()
    finally:
        loop.close()
### reference
# https://stackoverflow.com/a/20985318/2147823
# https://stackoverflow.com/a/48726480/2147823
###