#!/usr/bin/python3
import logging
import signal
import subprocess
import sys
import threading
import time
from queue import Queue
import os
import psutil

import flask
from flask import jsonify, request

from dronekit_server_client import DroneKitComms
from vision_cargo_approach import VisionCargoApproacher
from pallet_hole_alignment_c import HoleAligner


# def reinit_networking():
#     logging.info('Resetting dhcpcd service, wait 5 seconds...')
#     subprocess.run(["sudo", "service", "dhcpcd", "restart"])
#     time.sleep(5)
#     subprocess.run(["route", "-n"])

DO_ZERO = True
DO_GPS_THERE = True
DO_GOTO_TARGET_HEADING = False
DO_VISION_APPROACH = True
DO_FORK_ALIGN = True
DO_PICKUP_CARGO = True
DO_REVERSE_TURN = False
DO_GPS_RETURN = False
DO_DROPOFF_CARGO = True
DO_RETURN_TO_NEUTRAL = False
DO_LOOP = True


JOB_QUEUE = Queue(maxsize=10)
KILL_SIGNAL = False

def exit_handler(*_):
    global KILL_SIGNAL
    print('\nROOT: CTRL-C pressed, killing everything')
    KILL_SIGNAL = True
    sys.exit(0)
signal.signal(signal.SIGINT, exit_handler)

logging.basicConfig()
logging.root.setLevel(logging.NOTSET)
logging.basicConfig(level=logging.NOTSET)

executor_logger = logging.getLogger("LOGISTICS EXECUTOR")
executor_logger.setLevel(logging.DEBUG)


#--- <Pseudoprivate methods

def __go_to_gps_waypoints(vehicle_ctrl, waypoints: list):
    if len(waypoints) > 0:
        for ix, waypoint in enumerate(waypoints):
            target_lat = waypoint['lat']
            target_lon = waypoint['lon']
            executor_logger.info(f"Going to waypoint {ix} at lat: {target_lat}, lon: {target_lon}")
            success = __go_to_gps(vehicle_ctrl, lat=target_lat, lon=target_lon)
            if not success:
                executor_logger.error("Navigate to gps watpoint func returned error, chack logs, aborting")
                return False
    return True

def __go_to_gps(vehicle_ctrl, lat: float, lon: float):
    executor_logger.info(f"Starting GPS navigation to lat: {lat}, lon: {lon}")
    timeout_time = 300 + time.time()  # 5 minutes to get to target
    vehicle_in_target = False
    vehicle_ctrl.go_to_gps(lat=lat, lon=lon)
    while vehicle_in_target == False and time.time() < timeout_time:
        time.sleep(0.2)  # Don't DDoS the Pi!
        vehicle_in_target, distance = vehicle_ctrl.check_gps_in_radius((lat, lon))
    if vehicle_in_target:
        executor_logger.info(f'We are in radius ({distance})')
        return True
    else:
        executor_logger.critical(f"5 minuted have elapsed and we are not in the target area ({distance}), aborting")
        return False

def __go_to_cargo_vision(vision_system):
    executor_logger.info(f"Starting vision to cargo navigation")
    success = vision_system.navigate_to_cargo_vision()
    return success

def __align_forks_with_holes(hole_alignment_system):
    executor_logger.info(f"Starting pallet hole alignment")
    success = hole_alignment_system.align_fork()
    return success

def reverse_turn(vehicle_ctrl):
    executor_logger.info(f"Picking up cargo")
    __reverse_turn(vehicle_ctrl)
    return True

def __pick_up_cargo(vehicle_ctrl):
    vehicle_ctrl.approach_pallet()
    time.sleep(1)
    vehicle_ctrl.zero_fork_vert_movement()
    return True

def __put_down_cargo(vehicle_ctrl):
    vehicle_ctrl.zero_fork_vert_pickup()
    time.sleep(1)
    vehicle_ctrl.reverse_from_pallet()
    return True


def __reverse_turn(vehicle_ctrl):
    executor_logger.info(f"performing reverse turn")
    vehicle_ctrl.nudge_drive(False, 5, 2000)
    vehicle_ctrl.nudge_drive(False, 6, 2000)
    vehicle_ctrl.nudge_drive(True, 5, 1000)
    vehicle_ctrl.nudge_drive(True, 6, 1000)

    return True

#--- <Pseudoprivate methods


def queue_executor_thread():
    executor_logger.info("Starting task executor...")
    executor_logger.info("Reiniting networking...")
    # reinit_networking() # Reinits dhcpcd to enable Pi communications ---We fixed this by tweaking /etc/network/interfaces
    executor_logger.info("Init vehicle control...")
    FORKLIFT = DroneKitComms()
    executor_logger.info("Importing Vision system...")
    VISION_CARGO_APPROACH = VisionCargoApproacher(FORKLIFT)
    executor_logger.info("Importing Pallet hole aligner...")
    HOLE_ALIGNMENT_SYSTEM = HoleAligner(FORKLIFT)
    executor_logger.info("ARMING VEHICLE...")
    FORKLIFT.set_arm()
    FORKLIFT.set_state("READY_FOR_NEXT_TASK")
    executor_logger.info("Ready.")

    while not KILL_SIGNAL:
        if not JOB_QUEUE.empty():
            current_job = JOB_QUEUE.get()
            executor_logger.info(f"Executing new job: {current_job}")
            #execute job.....

            if DO_ZERO:
                executor_logger.info("Zeroing fork...")
                FORKLIFT.zero_fork_vert_movement()
                FORKLIFT.zero_fork_horiz()
            else:
                executor_logger.warning("DO_ZERO flag is false, skipping zeroing")

            if DO_GPS_THERE:
                FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
                gps_navigation_succcess = __go_to_gps_waypoints(FORKLIFT, current_job['pickup_waypoints'])
                if not gps_navigation_succcess:
                    executor_logger.critical("Go to GPS was not successful, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_GPS_THERE flag is false, skipping gps navigation to cargo")

            if DO_GOTO_TARGET_HEADING:
                FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
                heading_set_success = FORKLIFT.go_to_heading(current_job['pickup_heading'])
                if not heading_set_success:
                    executor_logger.critical("Go to GPS HEADING was not successful, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_GOTO_TARGET_HEADING flag is false, skipping GOTO target heading")

            if DO_VISION_APPROACH:
                FORKLIFT.set_state("NAVIGATING_TO_POINT_VISION")
                vision_to_cargo_succcess = __go_to_cargo_vision(VISION_CARGO_APPROACH)
                if not vision_to_cargo_succcess:
                    executor_logger.critical("Go to cargo with vision was not successful, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_VISION_APPROACH flag is false, skipping vision navigation to cargo")

            if DO_FORK_ALIGN:
                FORKLIFT.set_state("MANIPULATING_CARGO")
                FORKLIFT.zero_fork_vert_pickup()
                executor_logger.info("Aligning forks with holes...")
                align_forks_sucess = __align_forks_with_holes(HOLE_ALIGNMENT_SYSTEM)
                if not align_forks_sucess:
                    executor_logger.critical("Fork hole alignment was not successful, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_FORK_ALIGN flag is false, skipping fork alignment")

            if DO_PICKUP_CARGO:
                FORKLIFT.set_state("MANIPULATING_CARGO")
                cargo_pickupe = __pick_up_cargo(FORKLIFT)
                if not cargo_pickupe:
                    executor_logger.critical("Problem with cargo pickup, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_PICKUP_CARGO flag is false, skipping cargo pickup")

            if DO_REVERSE_TURN:
                reverse_turn = reverse_turn(FORKLIFT)
                if not reverse_turn:
                    executor_logger.critical("Problem with reverse turn, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_REVERSE_TURN flag is false, skipping reverse turn")


            if DO_GPS_RETURN:
                FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
                gps_dropoff_succcess = __go_to_gps_waypoints(FORKLIFT, current_job['dropoff_waypoints'])
                if not gps_dropoff_succcess:
                    executor_logger.critical("Go to GPS was not successful, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_GPS_RETURN flag is false, skipping navigationg to dropoff with gps")


            if DO_DROPOFF_CARGO:
                FORKLIFT.set_state("MANIPULATING_CARGO")
                cargo_down_success = __put_down_cargo(FORKLIFT)
                if not cargo_down_success:
                    executor_logger.critical("Problem with cargo put down, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_DROPOFF_CARGO flag is false, skipping dropping off cargo")

            if DO_REVERSE_TURN:
                reverse_turn = reverse_turn(FORKLIFT)
                if not reverse_turn:
                    executor_logger.critical("Problem with reverse turn, check logs !")
                    FORKLIFT.set_state("OTHER_ERROR")
                    return False
            else:
                executor_logger.warning("DO_REVERSE_TURN flag is false, skipping reverse turn")

            if current_job['on_finish']['goto'] == True:
                if DO_RETURN_TO_NEUTRAL:
                    FORKLIFT.set_state("NAVIGATING_TO_POINT_GPS")
                    gps_return_success = __go_to_gps_waypoints(FORKLIFT, current_job['on_finish']['waypoints'])
                    if not gps_return_success:
                        executor_logger.critical("Go to GPS was not successful, check logs !")
                        FORKLIFT.set_state("OTHER_ERROR")
                        return False
                else:
                    executor_logger.warning("DO_RETURN_TO_NEUTRAL flag is false, skipping returning to neutral point over GPS")

            if DO_LOOP:
                FORKLIFT.set_state("READY_FOR_NEXT_TASK")
                logging.info("Task complete... Sleeping for 5 and Waiting for next task")
                time.sleep(5)
            else:
                executor_logger.warning("DO_LOOP flag is false, exiting...")
                exit(0)

    executor_logger.info("Kill signal activated, exiting")


job_executor = threading.Thread(target=queue_executor_thread)
job_executor.start()


app = flask.Flask("task_upload_server")
app.logger.setLevel("DEBUG")

DEFULT_JOB_TEMPLATE = {
    "job_id": 0,
    "cargo_id": 0,
    "pickup_waypoints": [{"lat": 0, "lon": 0}, {"lat": 1, "lon": 1}, {"lat": 2, "lon": 2}],
    "dropoff_waypoints": [{"lat": 2, "lon": 2}, {"lat": 3, "lon": 3}, {"lat": 4, "lon": 4}],
    "on_finish": {"goto": True, "waypoints": [{"lat": 5, "lon": 5}, {"lat": 1, "lon": 1}, {"lat": 0, "lon": 0}]},
}

DEFAULT_JOB_TEMPLATE_KEYS = DEFULT_JOB_TEMPLATE.keys()

def verify_json_data(uploaded_data):
    data_keys = uploaded_data.keys()
    for item in DEFAULT_JOB_TEMPLATE_KEYS:
        if item not in data_keys:
            app.logger.error(f"Item  {item} not in uploaded job uploaded_data keys")
            app.logger.error(f"uploaded job dump: {uploaded_data}")
            return False
    return True

@app.route('/', methods=['GET'])
def home():
    return "<h1>Logistics executionner is running...</h1>"

@app.route('/alive', methods=['POST'])
def alive():
    app.logger.info(f"Returning alive")
    return jsonify({'Success': True})

@app.route('/add_job', methods=['POST'])
def add_job():
    if request.is_json:
        job_request = request.get_json()
        if not verify_json_data(job_request):
            app.logger.error("Error processing job request: check logs...")
            return jsonify({'Error': 'Error processing job request: check logs...'})
        app.logger.info(f"Got valid job request: {job_request}")
        JOB_QUEUE.put(job_request)
        return jsonify({'Success': True})
    else:
        return jsonify({'Error': 'Not JSON'})


app.run(host='0.0.0.0', port=8080)