"""
created by waldluis

This file contains the main script for the RaspberryPi of smart garden project
It has the task to control the EV3 robot and take measurements with the mounted sensor
The sensor data is published to MQTT topics to be available for the backend server
Used protocol for interaction: mqtt (paho-mqtt module)
Interaction with the EV3 via SSH
 """

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

from software.defines import Topics, RASPI_CLIENT_ID, MQTT_BROKER_GLOBAL



#region global Varaibles 

# FIXME Sensor not working
#sensors: RaspySensors


#endregion

#region global functions

# TODO auslagern
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
    """
    sensorData = sensors.readSensors()
    sensorData["PlantID"] = plantID
    sensorData["ActionID"] = actionID
    client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4))

# TODO auslagern
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')
    #CHECK if working properly

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

def on_connect(client: mqtt.Client, userdata, flags, rc):
    """
    This method gets called, when it connects to a mqtt broker.
    It is used to subscribe to the specific topics

    Args:
        client (mqtt.Client): current mqtt client
        userdata (_type_): _description_
        flags (_type_): _description_
        rc (_type_): _description_
    """
    if rc == 0:
         #Add callbacks
        client.message_callback_add("ROBOT/DATA", send_data_json)       #Testing
        client.message_callback_add(Topics["ROBOT_ACTION_DRIVE"], drive_plant)
        client.message_callback_add(Topics["ROBOT_ACTION_GETPOSITION"], get_position)
        client.message_callback_add(Topics["ROBOT_ACTION_GETBATTERY"], get_BatteryStatus)

        #Subscribe to topics
        client.subscribe("ROBOT/DATA")      #Testing
        client.subscribe(Topics["ROBOT_ACTION_DRIVE"])
        client.subscribe(Topics["ROBOT_ACTION_GETPOSITION"])
        client.subscribe(Topics["ROBOT_ACTION_GETBATTERY"])

        print("MQTT initialized")    
    

#endregion

# TODO auslagern
#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))


# CHECK if working properly
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 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



def main():
    """
    Initialises MQTT and Sensors
    Runs forever and controlls all robot functions
    """
    # CHECK if global
    # FIXME Sensor not working
    # global sensors
    # sensors = RaspySensors()
    client = mqtt.Client(RASPI_CLIENT_ID)
    client.on_connect = on_connect
    client.connect(MQTT_BROKER_GLOBAL)
    # dataDict = {}       #Testing

    # CHECK forever or start
    client.loop_start()

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




if __name__ == "__main__":
    main()