import paho.mqtt.client as mqtt
import json
import threading
import os
import logging
import raspy_sensors as Sensors
from defines import Topics, SENSORDATA, ALLSENSORDATA

import time

def measure_send_data(plantID, actionID, client: mqtt.Client):
    """
    Reads all sensors and publishes data via MQTT in form of SENSORDATA

    Args:
        plantID (_type_): plant to measure
        actionID (_type_): current ID of driving action
        client (mqtt.Client): current mqtt client for publishing
    """
    sensordata = {}
    sensordata |= SENSORDATA

    try:
        sensorData = Sensors.readSensors()
    except Exception as e:
        logging.error(str(e))
        client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)

    sensorData["PlantID"] = plantID
    sensorData["ActionID"] = actionID
    client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4), qos=1)


def drive_plant_thread(plantID, actionID, client: mqtt.Client):
    """
    Function to drive to plant according to number from MQTT message in thread
    Meassure and publish data via MQTT
    Drive home to starting point

    Args:
        plantID (_type_): plant to measure
        actionID (_type_): current ID of driving action 
        client (mqtt.Client): current mqtt client for publishing
    """
    # FIXME Change to color code driving
    errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')
    # errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}')

    if errorCode != 0:
        logging.error(f"Robot Error {errorCode} occurred")
        logging.info("Drive Plant aborted, Robot at starting position")
        client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
        client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)

        return
   
    logging.info("Measuring Sensors")
    measure_send_data(plantID, actionID, client)

    # TODO How to Handle Pictures and PlantID
    logging.info("Taking Picture")
    sendPicture(client)

    logging.info("Robot driving home")
    errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}')

    if errorCode != 0:
        logging.error(f"Robot Error {errorCode} occurred")
        logging.info(f"Drive Home aborted, Robot at plant {plantID}")
        client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plantID}", qos=1)
        client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
        return

    logging.info("Robot home")
    client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)


def drive_plant_all_thread(plantIDs: list, actionID, client: mqtt.Client):
    """
    Function to drive to plants according to numbers from MQTT message in thread
    Meassure and publish data for all plants via MQTT
    Drive home to starting point

    Args:
        plantIDs (list): Plants to drive to
        actionID (_type_): ActionID from Backend for whole drive action
        client (mqtt.Client): current MQTT client
    """

    allPlantData = {
        "SensorData": [],
        "ActionID": ""
    }
        
    allPlantData["ActionID"] = actionID

    for plant in plantIDs:
        # FIXME Only possible with color codes
        logging.info(f"Robot Driving to plant {plant}")
        errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plant}')
        if errorCode != 0:
            logging.error(f"Robot Error {errorCode} occurred")
            logging.info("Drive Plant aborted, Robot at starting position")
            client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
            client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
            return
    

        # TODO Test error handling
        logging.info("Measuring Sensors")
        try:
            sensordata = {}
            sensordata |= SENSORDATA
            sensordata = Sensors.readSensors()
            sensordata["PlantID"] = plant
            allPlantData["SensorData"].append(sensordata)
        except Exception as e:
            logging.error(str(e))
            client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)

        time.sleep(3)
        

        # TODO How to Handle Pictures and PlantID
        logging.info("Taking Picture")
        sendPicture(client)

        errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}')
        if errorCode != 0:
            logging.error(f"Robot Error {errorCode} occurred")
            logging.info("Drive Plant aborted, Robot at starting position")
            client.publish(Topics["ROBOT_DATA_ERROR"], f"Rotate arm aborted, Robot at plant {plant}", qos=1)
            client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
            return

    logging.info("All Plants measured, sending data")
    client.publish(Topics["ROBOT_DATA_ALL"], json.dumps(allPlantData, indent=4), qos=1)


    logging.info("Robot driving home")
    errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py')
    if errorCode != 0:
        logging.error(f"Robot Error {errorCode} occurred")
        logging.info(f"Drive Home aborted, Robot at plant {plant}")
        client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plant}", qos=1)
        client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
        return

    logging.info("Robot home")
    client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)


#region MQTT callbacks

def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
    """
    Function to drive to plant according to request
    Starting Drive in Thread to not block main programm

    Args:
        client (mqtt.Client): current mqtt client
        userdata (_type_): _description_
        message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID
    """
    dictMessage = json.loads(str(message.payload.decode("UTF-8")))

    plantID = dictMessage["PlantID"]
    actionID = dictMessage["ActionID"]

    logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}")
    client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
    
    thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, client), daemon=True)
    thread.start()


# TODO Test Drive all Plants
# FIXME Only possible with color codes
def drive_plant_all(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
    """
    Function to drive to plants according to request
    Starting Drive in Thread to not block main programm

    Args:
        client (mqtt.Client): current MQTT client
        userdata (_type_): _description_
        message (mqtt.MQTTMessage): received DRIVEALL message with PlantIDs and ActionID
    """
    dictMessage = json.loads(str(message.payload.decode("UTF-8")))

    plantIDs = dictMessage["PlantID"]            # List of numbers
    actionID = dictMessage["ActionID"]

    logging.info(f"Received Drive-request to plants {plantIDs}, ActionID: {actionID}")
    client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
    
    thread = threading.Thread(target= drive_plant_all_thread, args=(plantIDs, actionID, client), daemon=True)
    thread.start()

def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
    """
    Callback function for GPS position request
    Function to send actual GPS position via MQTT in form of POSITION

    Args:
        client (mqtt.Client): current mqtt client
        userdata (_type_): _description_
        message (mqtt.MQTTMessage): received message
    """
    logging.info("Received position request")

    # TODO Write GPS Sensor Function
    position = {
        "Position": ""
    }

    clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4), qos=1)


def get_BatteryStatus(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
    """
    Callback function for battery status request
    Function to read battery status from ev3 and send via MQTT in form of BATTERY
    8,5V -> 100%
    5V -> 0%

    Args:
        client (mqtt.Client): current mqtt client
        userdata (_type_): _description_
        message (mqtt.MQTTMessage): received message
    """
    logging.info("Received battery status request")

    try:
        batteryLevel = int(os.popen('sshpass -p maker ssh robot@ev3dev.local cat /sys/devices/platform/battery/power_supply/lego-ev3-battery/voltage_now').read())
    except:
        logging.error("Robot not connected")
        client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1)
        return

    batteryLevel = round(batteryLevel / 1000000, 2)       # Voltage
    batteryLevel = batteryLevel - 5
    batteryLevel = round(batteryLevel / 3.5, 3) *100      # Percentage

    battery = {
        "Battery": batteryLevel
    }
    client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1)
    logging.info(f"Battery done {batteryLevel}")


def sendPicture(client: mqtt.Client):
    """
    Takes picture and publish via MQTT

    Args:
        client (mqtt.Client): current mqtt client
    """
    try:
        Sensors.takePicture()
    except Exception as e:
        logging.error(str(e))
        client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
        return

    with open("picture.png", "rb") as f:
        file = f.read()
        byteArr = bytearray(file)
        client.publish(Topics["ROBOT_DATA_PICTURE"], byteArr)
        logging.info("Picture Published")

#endregion