import paho.mqtt.client as mqtt
import json
from raspySensors import RaspySensors

#region global Varaibles 
sensors = RaspySensors()
client = mqtt.Client()

sensorData = {
    "Air Temperature [°C]" : 0,
    "Air Humidity [%]" : 0,
    "Earth Humidity [%]" : 0,
    "Brightness [Lux]" : 0,
    "Plant ID": 0,
    "Action ID": 0
}

gpsPosition = {
    "Position": 0,
    "Action ID": 0
}

batteryStatus = {
    "Battery": 0,
    "Action ID": 0
}


#endregion

#region global functions

def measure_send_data(plantID, actionID):
    '''Measure data for one plant via sensor class and send via MQTT'''
    sensorData = sensors.readSensors()
    sensorData["Plant_ID"] = plantID
    sensorData["Action_ID"] = actionID
    client.publish("Robot/Data/SensorData", json.dumps(sensorData, indent=4))


def init_mqtt():
    '''Initialise MQTT client'''
    mqttBroker = "mqtt.eclipseprojects.io"
    client = mqtt.Client("Robot")

    #Add callbacks
    client.message_callback_add("Robot/Data", send_data_json)       #Testing
    client.message_callback_add("Robot/Action/Drive", drive_plant)
    client.message_callback_add("Robot/Action/GetPosition", get_position)
    client.message_callback_add("Robot/Action/GetBattery", get_BatteryStatus)

    #Subscribe to topics
    client.subscribe("Robot/Data")      #Testing
    client.subscribe("Robot/Action/Drive")
    client.subscribe("Robot/Action/GetPosition")
    client.subscribe("Robot/Action/GetBattery")

    client.connect(mqttBroker)


#endregion

#region MQTT callbacks

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


def drive_plant(clients, userdata, message):
    '''Function to drive to plant according to request'''
    
    #[ ]TODO handle MQTT message
    print(f"Driving to plant {message}")
    #[ ]TODO Start drive forward -> Thread

    print(f"Measuring data at Plant {message}")
    measure_send_data()       # With Signal not here

    print("Driving back to start position")
    #[ ]TODO Start Drive Back Function in Thread

    print("Back at starting Position") #Signal
    

def get_position(clients, userdata, message):
    '''Callback function for GPS position request
    Function to send actual GPS position via MQTT'''

    #[ ]TODO handle MQTT message
    gpsPosition["Position"] = sensors.readPosition()
    gpsPosition["Action_ID"] = message 
    client.publish("Robot/Data/Position", json.dumps(gpsPosition, indent=4))


def get_BatteryStatus(clients, userdata, message):

    '''Callback function for battery status request
    Function to read battery status from ev3 and send via MQTT'''

    #[ ]TODO handle MQTT message
    batteryStatus["Battery"] = sensors.readBattery()
    batteryStatus["Action ID"] = message
    client.publish("Robot/Data/Battery", json.dumps(batteryStatus, indent=4))

    batteryStatus()


    
#endregion



def main():
    init_mqtt()
    dataDict = {}       #Testing

    client.loop_start()

    while True:
        pass




if __name__ == "__main__":
    main()