import paho.mqtt.client as mqtt
import json
import threading
import os
import raspy_sensors as RaspySensors
from defines import Topics

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

    Args:
        plantID (_type_): plant to measure
        actionID (_type_): current ID of driving action
        client (mqtt.Client): current mqtt client for publishing
    """
    sensor = RaspySensors()
    sensorData = sensor.readSensors()
    sensorData["PlantID"] = plantID
    sensorData["ActionID"] = actionID
    client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4))


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
    """
    os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')

    print("Measuring Sensors")

    #FIXME Sensor not working
    # measure_send_data(plantID, actionID, client)

    print("Robot driving home")
    if plantID % 2 == 0:
        leftRight = -50         #rotating left
    else:
        leftRight = 50          #rotating right
    os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {leftRight}')
    print("Robot home")

    #TODO decide about robot occupied message
    #client.publish("ROBOT/DATA/OCCUPIED", "false")


#region MQTT callbacks

#Testing
def send_data_json(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
    strIn = str(message.payload.decode("UTF-8"))
    dataDict = json.loads(strIn)
    print("Received data: ", json.dumps(dataDict))


def drive_plant(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
    """
    Function to drive to plant according to request
    Starting Drive in Thread

    Args:
        clients (mqtt.Client): current mqtt client
        userdata (_type_): _description_
        message (mqtt.MQTTMessage): received message
    """
    dictMessage = json.loads(str(message.payload.decode("UTF-8")))
    plantID = dictMessage["PlantID"]
    actionID = dictMessage["ActionID"]

    print(f"received drive to plant {plantID}")
    
    thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, clients), 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

    Args:
        clients (mqtt.Client): current mqtt client
        userdata (_type_): _description_
        message (mqtt.MQTTMessage): received message
    """
    # TODO Write GPS Sensor Function
    position = {
        "Position": ""
    }

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


def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
    """
    Callback function for battery status request
    Function to read battery status from ev3 and send via MQTT

    Args:
        clients (mqtt.Client): current mqtt client
        userdata (_type_): _description_
        message (mqtt.MQTTMessage): received message
    """

    battery = {
        "Battery": 0.0
    }

    #TODO read Battery
    clients.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4))

    
#endregion