import paho.mqtt.client as mqtt
import json
import threading
import os
import signal
import time
from raspy_sensors import RaspySensors

#region global Varaibles 
#sensors:RaspySensors()
leftRight = 50
plantNumber = 0

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 drive_home():
    '''Function to drive robot back to starting position in thread'''
    print("Robot driving home")
    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")


def drive_plant_thread():
    '''Function to drive to plant according to number from MQTT message in thread'''
    os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantNumber}.py')
    print("Raising Signal to meassure")
    signal.raise_signal(signal.SIGUSR1)


def init_mqtt():
    '''Initialise MQTT client'''
    mqttBroker = "mqtt.eclipseprojects.io"
    global client
    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)

    client.connect(mqttBroker)      #Has to be before subscribing

    #Subscribe to topics
    client.subscribe("ROBOT/DATA")      #Testing
    client.subscribe("ROBOT/ACTION/DRIVE")
    client.subscribe("ROBOT/Action/GETPOSITION")
    client.subscribe("ROBOT/ACTION/GETBATTERY")

    


def signal_measure(signum, frame):
    '''Signal function to measure one plant and start driving home'''
    print("Measuring Sensors")
    #measure_send_data()
    print("Driving Home")
    drive_home()

#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
    Starting Drive in Thread'''
    global plantNumber, leftRight
    #TODO define MQTT message
    plantNumber = int(message.payload.decode("UTF-8"))

    print(f"received drive to plant {plantNumber}")
    
    if plantNumber % 2 == 0:
        leftRight = -50         #rotating left
    else:
        leftRight = 50          #rotating right

    thread = threading.Thread(target= drive_plant_thread, daemon=True)
    thread.start()


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

    
#endregion



def main():
    '''Initialises MQTT and Sensors
    Runs forever and controlls all robot functions'''

    init_mqtt()
    print("MQTT initialized")
    signal.signal(signal.SIGUSR1, signal_measure)
    # dataDict = {}       #Testing

    client.loop_start()

    print("Starting Loop")
    while True:
    #     print("Looping")
    #     time.sleep(1)
        pass




if __name__ == "__main__":
    main()