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

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"])


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):
    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.nudge_drive(False, 9.9)
    return True


def __put_cargo_down_and_reverse_turn(vehicle_ctrl):
    executor_logger.info(f"Dropping cargo")
    __put_down_cargo(vehicle_ctrl)
    __reverse_turn(vehicle_ctrl)
    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()
    executor_logger.info("Zeroing fork...")
    FORKLIFT.zero_fork_vert_movement()
    FORKLIFT.zero_fork_horiz()
    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.....

            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

            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

            FORKLIFT.zero_fork_vert_pickup()

            FORKLIFT.set_state("MANIPULATING_CARGO")
            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


            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


            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


            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

            FORKLIFT.set_state("MANIPULATING_CARGO")
            cargo_down_success = __put_cargo_down_and_reverse_turn(FORKLIFT)
            if not cargo_down_success:
                executor_logger.critical("Problem with cargo put down, check logs !")
                FORKLIFT.set_state("OTHER_ERROR")
                return False

            if current_job['on_finish']['goto'] == True:
                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

            FORKLIFT.set_state("READY_FOR_NEXT_TASK")
            logging.info("Task complete... Sleeping for 5 and Waiting for next task")
            time.sleep(5)

    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)