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

from subprocess import run, PIPE, Popen
import asyncio, time, os, sys, argparse, signal, logging, math, zmq.asyncio
import traceback
from datetime import datetime
import numpy as np
from geopy import Point
from geopy.distance import geodesic,distance,Distance
from tinkerforge.ip_connection import IPConnection
from tinkerforge.bricklet_imu_v3 import BrickletIMUV3


# logging.basicConfig(format='%(name)s:%(asctime)s-%(levelname)s:%(message)s',
#                     datefmt='%Y/%m/%d %H:%M:%S',
#                     filename='/var/log/dmcd.log')
# logging.getLogger().setLevel("DEBUG")

logging.basicConfig(filename='/var/log/dmcd.log',format='%(asctime)s: %(levelname)s - %(message)s',level=logging.INFO)
log = logging.getLogger("DMCD")
logging.getLogger('asyncio').setLevel(logging.WARNING)

FOV = 10 #Field of view in degrees
DEBUG=True
CMDDEBUG=False
global sensors_data
IMU_UID=None
IMU_CALIBRATION_STATUS=None
LOGLINELIMIT=200
APP_VERSION = "2.1.2"

#MCU cmds
SELF_HEADING_DMC = "6500"
TARGET_INCLINATION_DMC = "6501"
TARGET_GPS = "6104"
TARGET_BEARING = "6600"
TARGET_DISTANCE = "6601"
# SELF_WINDOW_GPS = "6501"
DMC_CALIBRATION_STATUS= "6503"


dirname = os.path.dirname(__file__)
tracking = os.path.join(dirname, 'tracking.log')
websimulator = os.path.join(dirname, 'websimulator.py')
#websimulator = "/home/user/bidentifier/websimulator.py"
parser = argparse.ArgumentParser()
parser.add_argument('-V', '--version', action='store_true', help='Display the application version')
args = parser.parse_args()
if args.version:
    print("Version: {}".format(APP_VERSION))
    sys.exit()

#IMU connect
HOST = "127.0.0.1"
PORT = 4223
# UID = "Zkf" #"2161" #"XYZ"
def cb_enumerate(uid, connected_uid, position, hardware_version, firmware_version,device_identifier, enumeration_type):

    global IMU_UID

    if enumeration_type == IPConnection.ENUMERATION_TYPE_DISCONNECTED:
        print(" Error IMU 3.0")
        return

    IMU_UID=uid
    print("IMU 3.0 UID: {}".format(IMU_UID))
try:
    ipcon = IPConnection() # Create IP connection
    ipcon.connect(HOST, PORT) # Connect to brickd
    ipcon.register_callback(IPConnection.CALLBACK_ENUMERATE, cb_enumerate) # Register Enumerate Callback
    ipcon.enumerate() # Trigger Enumerate
except Exception as e:
    log.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))
    print("Error: {}".format(e))
    sys.exit(1)


#ZMQ
context = zmq.asyncio.Context()
socket = context.socket(zmq.SUB) # type: ignore
socket.bind("tcp://*:5555")
socket.setsockopt(zmq.SUBSCRIBE, b'LRF')  # type: ignore
socket.setsockopt(zmq.SUBSCRIBE, b'GPS')  # type: ignore
time.sleep(1)

class sensors_data_class():
    def __init__(self):
        self.lrf_distance = 0
        self.gps_self_lat = 0.0
        self.gps_self_lon = 0.0
        self.dmc_heading = 0.0
        self.dmc_pitch = 0.0
        self.dmc_roll = 0.0
        self.dmc_quat_w = 0.0
        self.dmc_quat_x = 0.0
        self.dmc_quat_y = 0.0
        self.dmc_quat_z = 0.0
        self.dmc_calibrate_status = 3
        self.target_gps_lat = 0.0
        self.target_gps_lon = 0.0
        self.target_dmc_heading = 0.0
        self.target_distance = 0

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

sensors_data = sensors_data_class()


# Callback function for all data callback
def cb_all_data(acceleration, magnetic_field, angular_velocity, euler_angle, quaternion,
                linear_acceleration, gravity_vector, temperature, calibration_status):
    log.debug(" >>> DMC Calibration: {}".format(calibration_status))
    sensors_data.dmc_heading =  euler_angle[0]/16.0
    sensors_data.dmc_roll =     euler_angle[1]/16.0
    sensors_data.dmc_pitch =    euler_angle[2]/16.0
    sensors_data.dmc_quat_w = quaternion[0]/16383.0
    sensors_data.dmc_quat_x = quaternion[1]/16383.0
    sensors_data.dmc_quat_y = quaternion[2]/16383.0
    sensors_data.dmc_quat_z = quaternion[3]/16383.0
    sensors_data.dmc_calibrate_status = calibration_status

    return sensors_data

def cb_orientation (heading,roll,pitch):

    sensors_data.dmc_heading =  round(heading/16.0)
    sensors_data.dmc_roll =     round(roll/16.0)
    sensors_data.dmc_pitch =    round(pitch/16.0)

    return sensors_data

# def cb_quaternion2euler_data(x,y,z,w):
#     from math import atan2, asin
#     x = x / 16383.0
#     y = y / 16383.0
#     z = z / 16383.0
#     w = w / 16383.0

#     sensors_data.dmc_heading  = atan2(2*y*w - 2*x*z, 1 - 2*y*y - 2*z*z) # Pitch == Inclination
#     sensors_data.dmc_roll = atan2(2*x*w - 2*y*z, 1 - 2*x*x - 2*z*z) # Roll == Horizont
#     sensors_data.dmc_pitch = asin(2*x*y + 2*z*w) #Yaw == Heading

#     return sensors_data


def format_map_url(lat,lon):
    return "http://www.openstreetmap.org/?mlat={}&mlon={}&zoom=15".format(lat,lon)

def get_bearing(lat1,lon1,lat2,lon2):
    dLon = float(lon2 - lon1);
    y = math.sin(dLon) * math.cos(lat2);
    x = math.cos(lat1)*math.sin(lat2) - math.sin(lat1)*math.cos(lat2)*math.cos(dLon);
    brng = np.rad2deg(math.atan2(y, x));
    if brng < 0: brng+= 360
    return brng

def calculate_initial_compass_bearing(pointA, pointB):
    """
    Calculates the bearing between two points.
    The formulae used is the following:
        θ = atan2(sin(Δlong).cos(lat2),
                  cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong))
    :Parameters:
      - `pointA: The tuple representing the latitude/longitude for the
        first point. Latitude and longitude must be in decimal degrees
      - `pointB: The tuple representing the latitude/longitude for the
        second point. Latitude and longitude must be in decimal degrees
    :Returns:
      The bearing in degrees
    :Returns Type:
      float
    """
    if (type(pointA) != tuple) or (type(pointB) != tuple):
        raise TypeError("Only tuples are supported as arguments")

    lat1 = math.radians(pointA[0])
    lat2 = math.radians(pointB[0])

    diffLong = math.radians(pointB[1] - pointA[1])

    x = math.sin(diffLong) * math.cos(lat2)
    y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1)
            * math.cos(lat2) * math.cos(diffLong))

    initial_bearing = math.atan2(x, y)

    # Now we have the initial bearing but math.atan2 return values
    # from -180° to + 180° which is not what we want for a compass bearing
    # The solution is to normalize the initial bearing as shown below
    initial_bearing = math.degrees(initial_bearing)
    compass_bearing = (initial_bearing + 360) % 360

    return compass_bearing

def get_distance(self_coords,target_coords):
    return geodesic(self_coords, target_coords).meters

def increase_index(_file):
    # load data from file
    with open(_file, "r") as f:
        data = f.read().strip().split("\n")

    latest_row = data[-1]
    index = int(latest_row.split(":")[0])
    index += 1
    latest_row = "{0},{1}".format(index, ",".join(latest_row.split(',')[1:]))
    data[-1] = latest_row
    # reverse data
    data = data[::-1]
    with open(_file, "w") as f:
        f.write("\n".join(data))


def save_new_line(file,index,fields):
    # load data from file
    with open(file, "r") as f:
        data = f.read().strip().split("\n")

    # create new line
    new_line = "{0},{1}".format(index, ",".join(fields))

    # add new line to data
    data.insert(0, new_line)

    # save data back to file
    with open(file, "w") as f:
        f.write("\n".join(data))

def save_tracklog():
    data =[]
    now = datetime.now()
    date_str=now.strftime("%d/%m/%Y %H:%M:%S")

    #Read whole file and enumerate lines
    # If more than 1000 records are saved
    with open (tracking,'r') as trackingfile:

        data = trackingfile.read().strip().split("\n")
        if len(data) > LOGLINELIMIT:
            data = data[:LOGLINELIMIT]
        #Timestamp|BDR Lon,Lat|BDR URL|TRG Lon,Lat|TRG URL|Distance|Azimuth|Inclination
        template = "{0}|{1},{2}|{3}|{4},{5}|{6}|{7}m|{8}°|{9}°"

        new_line = template.format(date_str,
                                    sensors_data.gps_self_lat, sensors_data.gps_self_lon,
                                    format_map_url(sensors_data.gps_self_lat, sensors_data.gps_self_lon),
                                    sensors_data.target_gps_lat, sensors_data.target_gps_lon,
                                    format_map_url(sensors_data.target_gps_lat,sensors_data.target_gps_lon),
                                    sensors_data.lrf_distance,
                                    sensors_data.dmc_heading,
                                    sensors_data.dmc_pitch)

        # data[-1] = new_line
        data.insert(0,new_line)
        #Should we still reverse order ?
        #data = data[::-1]

    with open (tracking,'w') as trackingfile:
        for item in data:
            trackingfile.write(item+"\n")

    time.sleep(0.5)
    log.info("Calling targetlog_create.py")
    Popen(["python3", "/bdr/targetlog_create.py"])

async def shutdown(sig, loop):

    try:
        #TBD shutdown proc
        # await loop.run_in_executor(None, time.sleep, 2)

        # 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))
        results = await asyncio.gather(*tasks, return_exceptions=True)
        # stop the loop
        loop.stop()

    except Exception as e:
        raise e

async def zmq_loop(loop,handler):

    global sensors_data
    try:
        while True:
            # try:
            message = await socket.recv()
            time.sleep(0.1)
            data = message.split()

            if data[0] == b"LRF":

                sensors_data.lrf_distance=int(data[1])
                await asyncio.ensure_future(handler("LRF"))

            elif data[0] == b"GPS":

                sensors_data.gps_self_lat = float(data[1])
                sensors_data.gps_self_lon = float(data[2])
                await asyncio.ensure_future(handler("GPS"))
    except Exception as e:
        logging.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))

async def zmq_event_handler(signal):

        global sensors_data

        log.debug("Signal {} >>> Heading: {} Incl: {} LRF dist: {} GPS self: {}/{}"
                .format(signal,
                        sensors_data.dmc_heading,
                        sensors_data.dmc_pitch,
                        sensors_data.lrf_distance,
                        sensors_data.gps_self_lat,
                        sensors_data.gps_self_lon)
                )

        if (sensors_data.dmc_heading==0 or sensors_data.dmc_pitch==0):
            heading,roll,pitch=imu.get_orientation()
            cb_orientation(heading,roll,pitch)

        ## All data fields we need to triangulate TARGET
        ## Triger by LRF
        if signal=="LRF":

            if sensors_data.lrf_distance !=0:
                sensors_data.target_distance=sensors_data.lrf_distance
                # TARGET bearing == DMC heading
                sensors_data.target_dmc_heading=sensors_data.dmc_heading
                cmd_to_mcu(TARGET_DISTANCE,str(sensors_data.target_distance)+" ")

            if all(v != 0 for v in [ sensors_data.lrf_distance,
                                        sensors_data.dmc_heading,
                                        sensors_data.gps_self_lat,
                                        sensors_data.gps_self_lon]
                ):
                log.debug("By LRF triger: Target distance {}".format(sensors_data.target_distance))

                # given: lat1, lon1, bearing, distMiles
                #lat2, lon2 = VincentyDistance(miles=distMiles).destination(Point(lat1, lon1), bearing)
                if sensors_data.dmc_pitch != None:
                    #DMC Pitch a.k.a Inclination
                    data_string=str(round(sensors_data.dmc_pitch)) + "° "
                    cmd_to_mcu(TARGET_INCLINATION_DMC,data_string)

                #Triangualtion of Target
                distKm=float(sensors_data.lrf_distance/1000)
                self_gps_point=Point(sensors_data.gps_self_lat, sensors_data.gps_self_lon)
                bearing=float(sensors_data.dmc_heading) #Azimuth

                #Calculate TARGET geodesic point
                target_point = geodesic(kilometers=distKm).destination(self_gps_point,float(sensors_data.dmc_heading)).format_decimal()

                #Cleanup coords
                sensors_data.target_gps_lat,sensors_data.target_gps_lon = [x.strip() for x in target_point.split(',')]
                #Form round to 4 numbers TARGET GPS coordinates
                sensors_data.target_gps_lat=round(float(sensors_data.target_gps_lat),6)
                sensors_data.target_gps_lon=round(float(sensors_data.target_gps_lon),6)

                target_gps_coords = str(round(sensors_data.target_gps_lat,4)) + " |" + str(round(sensors_data.target_gps_lon,4)) + " "
                # target_gps_point = (sensors_data.target_gps_lat, sensors_data.target_gps_lon)
                # sensors_data.target_distance = geodesic(self_gps_point, target_gps_point).meters #get_distance(self_gps_point,target_gps_point)

                # To MCU
                cmd_to_mcu(TARGET_GPS,target_gps_coords)
                cmd_to_mcu(TARGET_DISTANCE,str(sensors_data.target_distance)+" ")
                save_tracklog()
                #Nulling LRF data after receive and proceed
                sensors_data.lrf_distance = 0
                log.info("By LRF event >>> Target: coords={} distance={} bearing={}".format(target_gps_coords,sensors_data.target_distance,round(bearing)))

        ## Trigered by GPS coords
        elif signal=="GPS":
            if all(v != 0 for v in [ sensors_data.dmc_heading,
                                    sensors_data.gps_self_lat,
                                    sensors_data.gps_self_lon,
                                    sensors_data.target_gps_lat,
                                    sensors_data.target_gps_lon]):

                target_gps_tuple = (sensors_data.target_gps_lat,sensors_data.target_gps_lon)
                self_gps_tuple = (sensors_data.gps_self_lat,sensors_data.gps_self_lon)

                # Calculation of the Target
                # Heading by GPS coords
                gps_bearing = round(get_bearing(sensors_data.target_gps_lat, sensors_data.target_gps_lon,sensors_data.gps_self_lat, sensors_data.gps_self_lon))
                log.debug("GPS bearing to target {}".format(gps_bearing))

                # Distance by GPS coords
                sensors_data.target_distance = round(geodesic(self_gps_tuple, target_gps_tuple).meters)
                log.debug("GPS distance to target {}".format(sensors_data.target_distance))

                target_gps_coords = str(round(float(sensors_data.target_gps_lat),4))+" |"+str(round(float(sensors_data.target_gps_lon),4))+" "

                #To MCU
                cmd_to_mcu(TARGET_BEARING,str(gps_bearing))
                cmd_to_mcu(TARGET_DISTANCE,str(sensors_data.target_distance)+" ")

                log.info("By GPS event >>> Target: coords={} distance={} bearing={}"
                        .format(target_gps_coords,sensors_data.target_distance,round(gps_bearing)))

def quadrant(angle):
    angle %= 360.0
    if (angle < 0):
        angle += 360.0;
    quadrant = int((angle/45) % 8 + 1)
    quadrantList = ["NE", "E", "SE", "S", "SW", "W", "NW", "N"]
    return str(quadrantList[quadrant-1])

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
        send_buff.append(hex(ord(ch))[2:])
    send_buff.append('00') #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(str(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 dmc_event_handler(heading,roll,pitch):
    try:


        cb_orientation(heading,roll,pitch)

        if sensors_data.dmc_heading != None:

            data_string=str(quadrant(sensors_data.dmc_heading))+":"+str(sensors_data.dmc_heading)+"°/"+str(sensors_data.dmc_pitch)+"° "

            #todo Calibration status
            if sensors_data.dmc_calibrate_status == 255:
                data_string=str(quadrant(sensors_data.dmc_heading))+":"+str(sensors_data.dmc_heading)+"°/"+str(sensors_data.dmc_pitch)+"° "
            else:
                # data_string="-"+str(sensors_data.dmc_calibrate_status)+"-"+str(quadrant(sensors_data.dmc_heading))+":"+str(sensors_data.dmc_heading)+"°/"+str(sensors_data.dmc_pitch)+"° "
                data_string="@"+str(quadrant(sensors_data.dmc_heading))+":"+str(sensors_data.dmc_heading)+"°/"+str(sensors_data.dmc_pitch)+"° "
                # cmd_to_mcu(DMC_CALIBRATION_STATUS,str(sensors_data.dmc_calibrate_status))

            cmd_to_mcu(SELF_HEADING_DMC,data_string)

        deviation_loop()
        log.debug("Heading: {} Horizont: {} Incl: {}".format(sensors_data.dmc_heading,sensors_data.dmc_roll,sensors_data.dmc_pitch))
    except Exception as e:
        logging.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))

# def dmc_alldata_handler(acceleration, magnetic_field, angular_velocity, euler_angle, quaternion,
#                 linear_acceleration, gravity_vector, temperature, calibration_status):
#     global IMU_CALIBRATION_STATUS
#     IMU_CALIBRATION_STATUS=calibration_status
#     if not IMU_CALIBRATION_STATUS:
#         log.info("DMC: Calibration status: " + format(calibration_status, "08b"))
#         data_string="DMC status: " + format(calibration_status, "08b")
#         cmd_to_mcu(CALIBRATION_STATUS,data_string)


def stringify_target_bearing(deviation):

    pre_dev_bearing=""
    post_dev_bearing=""

    if abs(deviation) > FOV:

        if deviation > 0:
            if deviation > 90: pre_dev_bearing="## "
            else:              pre_dev_bearing="# "
        else:
            if deviation < -90: post_dev_bearing=" $$"
            else:               post_dev_bearing=" $"

    target_dev_bearing=pre_dev_bearing + str(deviation) + "°" + post_dev_bearing + " "
    cmd_to_mcu(TARGET_BEARING,target_dev_bearing)
    log.debug(">>> Target bearing: {}".format(target_dev_bearing))


def deviation_loop():

    if all(v != 0 for v in [sensors_data.target_gps_lat,
                            sensors_data.target_gps_lon,
                            sensors_data.gps_self_lat,
                            sensors_data.gps_self_lon]):

        target_gps_tuple = (sensors_data.target_gps_lat,sensors_data.target_gps_lon)
        self_gps_tuple = (sensors_data.gps_self_lat,sensors_data.gps_self_lon)

        # compass_bearing = calculate_initial_compass_bearing(target_gps_tuple,self_gps_tuple)
        # if DEBUG:   log.debug("Compass bearing to target {}".format(compass_bearing))

        gps_bearing = get_bearing(sensors_data.target_gps_lat, sensors_data.target_gps_lon,
                                sensors_data.gps_self_lat, sensors_data.gps_self_lon)
        log.debug("GPS bearing to target {}".format(gps_bearing))

        gps_distance = round(geodesic(self_gps_tuple, target_gps_tuple).meters)
        log.debug("GPS distance to target {}".format(gps_distance))

        if (sensors_data.target_dmc_heading and sensors_data.dmc_heading):

            deviation_bearing = (int(int(sensors_data.dmc_heading) - int(sensors_data.target_dmc_heading)) + 180) % 360 - 180

            # log.debug(">>> Target azmth: {} Own azmth: {} Deviation: {}".format(sensors_data.target_dmc_heading,sensors_data.dmc_heading,deviation_bearing))

            # Target bearing deviation simbols
            stringify_target_bearing(deviation_bearing)

            # Target re-calculate by GPS deviation
            # stringify_target_bearing(gps_bearing)

        # Target distance re-calculate
        if ( round(gps_distance) != sensors_data.target_distance ):
            log.debug("Diff between LRF {} and GPS distance {}".format(sensors_data.target_distance,gps_distance))
            cmd_to_mcu(TARGET_DISTANCE,str(gps_distance)+" ")


async def main_loop(loop):
    try:
        if os.path.exists("/home/user/bidentifier/gpslocation") and os.path.getsize("/home/user/bidentifier/gpslocation") > 0:
            with open ('/home/user/bidentifier/gpslocation','r') as gpslocation:
                # content = gpslocation.readline()
                sensors_data.gps_self_lat, sensors_data.gps_self_lon = map(float, gpslocation.readline().split("|"))
                logging.info("GPS: {} {}".format(str(sensors_data.gps_self_lat), str(sensors_data.gps_self_lon)))

        tasks = [zmq_loop(loop,zmq_event_handler)]
        await asyncio.gather(*tasks)
    except Exception as e:
        log.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))

if __name__ == "__main__":

    # Don't use device before ipcon is connected
    # try:
    #     id = imu.get_identity()
    #     # print(id)
    # except Exception as e:
    #     # raise e
    #     pass

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

        imu = BrickletIMUV3(IMU_UID, ipcon) # Create device object

        imu.register_callback(imu.CALLBACK_ORIENTATION,dmc_event_handler)
        imu.set_orientation_callback_configuration(500,True) # Interval in ms, on event or not

        imu.register_callback(imu.CALLBACK_ALL_DATA,cb_all_data)
        imu.set_all_data_callback_configuration(10000,True)

        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:
            log.error('add_signal_handler error')

        try:
            loop.run_forever()
        finally:
            loop.close()
            log.error("{} {} ".format(traceback.extract_tb(e.__traceback__)[-1],e))

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