"""
Parameter für die Arduinosteuerung und die Simulation
Autor: Nils Landmark
"""

### Setup ###
# Numpy für Pi
import numpy as np

### Parameter Geschwindigkeitsberechnung ###
# Parameter Entfernungsbereiche
far_areas = [100, 100]
close_areas = [75, 100]
distance_min = 1300
distance_max = 1500

# Geschwindigkeitsstufen translatorische Bewegung
transl_speeds = [30, 60, 100]

# Parameter Winkelbereiche
angle_area_ok = np.pi / 180 * 3
angle_areas = list(np.array([5, 15]) * np.pi / 180)

# Geschwindigkeitsstufen Drehung
turn_speeds = [5, 15, 60]

# Abstand ziwschen den Rädern des EasyFollow
wheelbase = 235

### Parameter für die Simulation ###
# Dictionary für die Fahrtrichtungen
speed_settings = {
        1: 0,  # Stop
        2: 1,  # Forward
        3: -1  # Backward
    }
    
def actual_speed(speed_setting: float) -> float:
    """Berechnet aus prozentualer Geschwindigkeit die tatsächliche Geschwindigkeit des EasyFollow in mm/s

    Parameters
    ----------
    speed_setting : float
        Prozentuale Geschwindkeit

    Returns
    -------
    float
        Geschwindikeit in mm/s
    """
    # Steigung der linearen Geschwindigkeitsfunktion
    speed_increase = 3.1
    # Grundwert der linearen Funktion
    base_speed = 68

    # 0 mm/s bei 0%
    if speed_setting == 0:
        speed = 0
    # Geschwindigkeit bei positiven Prozentwerten
    elif speed_setting > 0:
        speed = base_speed + speed_increase * speed_setting
    # Geschwindigkeit bei positiven Prozentwerten
    elif speed_setting < 0:
        speed = - base_speed + speed_increase * speed_setting
    # Fehler bei der Geschindkeit --> Rückgabe Geschwindigkeit 0
    else:
        print("bad speed_setting", speed_setting)
        speed = 0

    return speed