### Einbinden der Pakete ###

# Einbinden eigener Pakete
import Fahrstrategie
import recieve_data_from_TB
import send_data_to_TB

# generelle Pakete 
from genericpath import exists
from queue import PriorityQueue
import threading
import time
from time import sleep
import psutil 
from statistics import mean
import requests
import json


# Pakete für das User-Tracking
from PyNuitrack import py_nuitrack
from itertools import cycle
import matplotlib.pyplot as plt

### Definieren der Funktionen ###


# erzeuge nuitrack
nuitrack = py_nuitrack.Nuitrack()
# setze die user_found variable inital auf False
user_found = False
# Counter und Listen für die Plot-Erzeugung im user-tracker
count = 0
x_list = []
z_list = []

# Ohne Thingsboard für Testzwecke
manuel_mode = False


def user_tracker(plotter = False):
    """Diese Funktion ist für das Tracken des Users zuständig. Sie basiert auf der SDK nuitrack. Getrackt wird der Kopf des Users.


    Parameters
    ----------
    plotter : bool, optional
        Wenn diese Variable auf True gesetzt wird, wird ein Fenster geöffnet, welches den Abstand zum erkannten User als Graphen plottet.

    Returns
    -------
    Liste: x und z Abstand des Users zur Kamera.
        
    """
    global nuitrack
    global count, x_list, z_list
    global user_found

    # setzen der Aktualisierungsrate
    sleep(1/10)
    # update Kamera
    nuitrack.update()
    # holen der Skelekton-Tracker Daten
    data = nuitrack.get_skeleton()

 
    for skeleton in data.skeletons:
      
        if plotter == True:
            z_comp = float(skeleton.head.real[2])
            
            z_list.append(z_comp)
            x_list.append(count)
            
            plt.plot(x_list,z_list)

                
            plt.title("z axis distance to head tracker")
            plt.draw()
            plt.pause(0.005)

            count = count + 1
        
        back_values = [skeleton.head.real[0],skeleton.head.real[2]]
        

    try:
        if back_values is not None:
            user_found = True
        return back_values
    except:
        user_found = False
        return [0,1400]
        

# Erzeugung der Warteschlagen-Elemente

q_distance = PriorityQueue()

q_speeds = PriorityQueue()

# Warteschlangen Funktionen

def put_q_distance(priority, message):
    """Einreihung der Nachrichten in die Warteschlange.
    Parameter
    ----------
    priority: Priorität, 1 ist die Höchste.
    message: Zu versendene Nachricht
    Rückgabe
    -------
    none
    """
    q_distance.put((priority, message))

def get_q_distance():
    """ Ausgabe der Nachrichten aus der Warteschlange mit der höchsten Priorität """
    return q_distance.get()

def put_q_speeds(priority, message):
    """Einreihung der Nachrichten in die Warteschlange.
    Parameter
    ----------
    priority: Priorität, 1 ist die Höchste.
    message: Zu versendene Nachricht
    Rückgabe
    -------
    none
    """
    q_speeds.put((priority, message))

def get_q_speeds():
    """ Ausgabe der Nachrichten aus der Warteschlange mit der höchsten Priorität """
    return q_speeds.get()

### Definieren der Threads ###

class collect_pos_data(threading.Thread):
    """ Dieser Thread ist für das sammeln der Positionsdaten des Users da. Die Daten werden abschließend in eine Warteschlange eingereiht.  """
    def run(self):
        global nuitrack
        while True:
            # warte bis der CameraThread bereit ist
            if not CameraThread_running: 
                continue

            nuitrack.init()

            # prüfe auf vorhandene und unterstützte Kamera-Sensoren
            devices = nuitrack.get_device_list()
            for i, dev in enumerate(devices):
                print(dev.get_name(), dev.get_serial_number())
                if i == 0:
                    print(dev.get_activation())
                    nuitrack.set_device(dev)
            # Zeige Version und Lizenz
            print(nuitrack.get_version())
            print(nuitrack.get_license())

            nuitrack.create_modules()
            nuitrack.run()

            modes = cycle(["depth", "color"])
            mode = next(modes)

            
            while True:
                try:
                    full_message = user_tracker()
                except:
                    print("Nuitrack license ran out")
                    break
                if RoboThread_running:
                    put_q_distance(1, full_message)
                if not CameraThread_running:
                    break 
                

class send_2_robo(threading.Thread):
    """ Dieser Thread sendet die Datensätze zum Steuern der Motoren an den Arduino, über eine serielle Verbindung. """
    def run(self):
        # inital auf False, zeigt, dass die serielle Verbindung noch nicht initialsiert wurde
        drive_initialized = False
        while True: 
            # initialsiere ser.-Verb., wenn noch nicht erfolgt
            if not drive_initialized and RoboThread_running:
                Fahrstrategie.initialize_serial_connection(port='COM3')
                drive_initialized = True
            # hole Datensätze aus Warteschlange    
            message = get_q_distance()
            # Zugriff auf Einzelwerte
            pos_0 = message[1][0]
            pos_1 = message[1][1]
            # prüfe ob der Roboter fahren darf
            if RoboThread_running:
                # sende Daten an Roboter
                turn, translate = Fahrstrategie.drive(pos_0, pos_1)
                # Reihe Daten in Warteschlange ein, für Verwendung im Thingsboard
                put_q_speeds(1, (turn, translate))


class recieve_from_Thingsboard(threading.Thread):
    """Dieser Thread stellt eine Verbindung zum Thingsboard her und wartet auf Befehle. """
    def run(self):
        recieve_data_from_TB.run_demo_mode()


class starter_Thread(threading.Thread):
    """Dieser Thread pausiert bei Bedarf die anderen Threads. Dient dem Starten und Stoppen der Folgen -und Tracken-Funktion  """
    def run(self):
        global CameraThread_running 
        global RoboThread_running 
        CameraThread_running = False
        RoboThread_running = False
        print("Hey")
        while True:
            # print("Im in the loop",recieve_data_from_TB.demo_mode,recieve_data_from_TB.drive_status)

            if recieve_data_from_TB.demo_mode == True:
                CameraThread_running = True
            elif recieve_data_from_TB.demo_mode == False:
                CameraThread_running = False
                if RoboThread_running == True:
                    RoboThread_running = False
            if recieve_data_from_TB.drive_status == True and recieve_data_from_TB.demo_mode == True:
                RoboThread_running = True
            if recieve_data_from_TB.drive_status == False:
                RoboThread_running = False
            # herabsetzen der Prozessorbelastung
            time.sleep(0.5)

class send_2_Thingsboard(threading.Thread):
    """Dieser Thread stellt eine Verbindung zum Thingsboard her und übergibt die aktuellsten Werte. """

    def run(self):
        global user_found 
        # initalsieren Daten und deren Typen
        data_sent_past = None
        turns = []
        translates = []
        turn_av = 0
        
        # setzen der Statuse im Thingsboard auf False
        # headers = {        'Content-Type': 'application/json',    }
        # requests.post('https://thingsboard.cloud/api/v1/sCdIh0TLguO5gvSRZcL8/attributes', headers=headers, data=json.dumps({"1":False,"2":False}))

        while True:
            # Auslesen Akkustand und Energiequelle
            battery = psutil.sensors_battery()
            if battery.power_plugged:
                power_type = "Cable"
                battery_time = "infinite"
            else:  
                power_type = "Battery"
                battery_time = time.strftime('%H:%M:%S', time.gmtime(battery.secsleft))
            
            # Prüfen, ob die Kamera einen User erkennt
            if user_found:
                user_found_text = "User found"
            elif not user_found:
                user_found_text = "No user could be found"
            
            # Ermitteln der aktuellen Geschwindigkeit 
            if not q_speeds.empty():
                vals = get_q_speeds()
                turns.append(abs(vals[1][0][0]))
                translates.append(abs(vals[1][1][0]))

            # Mitteln der aktuellen Geschwindigkeit über 20 Werte hinweg
            if len(turns) >= 20:
                turn_av = mean(turns)
                turns = []
                translates = []
            
            # Aufbau des json-Strings für die Übermittlung der Daten 
            data_to_send={
                "error_code" : user_found_text,
                "user_found": user_found,
                "Temp":12, #bool
                "status":CameraThread_running, #bool #Demo Mode on/off
                "in_use":RoboThread_running, #bool Following
                "battery":battery.percent, #int Battery in %
                "power_type":power_type, #bool Cable or Battery
                "Predicted_battey_time":battery_time,  #time 00:31:21
                "speed_of_movement_interday" :turn_av
            }
            
            # Prüfen, ob die Daten sich verändert haben. Daten nur übergeben, wenn sie sich verändert haben, um Bandbreite zu sparen
            if not data_to_send == data_sent_past:
                resp_code = send_data_to_TB.send_data(data_to_send)
                data_sent_past= data_to_send
                if str(resp_code) == "200":
                    print("Data transfer successful.")
                else:
                    print("Data transfer failed, response was: "+str(resp_code))


### Threads starten ### 

# Thread Initialisierung

Th_Camera = collect_pos_data()
Th_Robo = send_2_robo()
if not manuel_mode:
    TH_recieve_TB = recieve_from_Thingsboard()
    TH_send_TB = send_2_Thingsboard()
    TH_starter = starter_Thread()

# Threads starten
if manuel_mode:
    CameraThread_running = True
    RoboThread_running = True

if not manuel_mode:
    TH_starter.start()
Th_Camera.start()
Th_Robo.start()
if not manuel_mode:
    TH_recieve_TB.start()
    TH_send_TB.start()