from src.variables.service import get_variable, set_variable
from src.modules.IndustrialAutomation.service_analog import read_0_10_out, set_0_10_out
import os

#variables names
variable_control_channel: str = "speed_controller_channel"
variable_control_stack: str = "speed_controller_stack"
#variables rpm
variable_actual_rpm: str = "actual_rpm"
variable_requested_rpm: str = "requested_rpm"
#variables limits
variable_max_rpm: str = "max_rpm"
variable_max_U: str = "max_U"
#variables controling step
variable_control_step_U: str = "control_step_U"
variable_control_step_rpm: str = "control_step_rpm"
variable_control_step_U_1: str = "control_step_U_1"
variable_control_step_rpm_1: str = "control_step_rpm_1"
variable_control_step_U_2: str = "control_step_U_2"
variable_control_step_rpm_2: str = "control_step_rpm_2"

#init variables in variables system
if get_variable(variable_actual_rpm) == None:
    set_variable(variable_actual_rpm, 0, True)

if get_variable(variable_requested_rpm) == None or int(get_variable(variable_requested_rpm)) != 0:
    set_variable(variable_requested_rpm, 0, True)

if get_variable(variable_max_rpm) == None:
    set_variable(variable_max_rpm, 1900, True)

if get_variable(variable_max_U) == None:
    set_variable(variable_max_U, 8, True)

if get_variable(variable_control_step_U) == None:
    set_variable(variable_control_step_U, 0.1, True)

if get_variable(variable_control_step_rpm) == None:
    set_variable(variable_control_step_rpm, 200, True)

if get_variable(variable_control_step_U_1) == None:
    set_variable(variable_control_step_U_1, 0.01, True)

if get_variable(variable_control_step_rpm_1) == None:
    set_variable(variable_control_step_rpm_1, 100, True)

if get_variable(variable_control_step_U_2) == None:
    set_variable(variable_control_step_U_2, 0.001, True)

if get_variable(variable_control_step_rpm_2) == None:
    set_variable(variable_control_step_rpm_2, 5, True)

if get_variable(variable_control_stack) == None:
    set_variable(variable_control_stack, 0, True)

if get_variable(variable_control_channel) == None:
    set_variable(variable_control_channel, 1, True)


# set rpm control output
def set_output(value: float):
    if value < float(get_variable(variable_max_U)):
        set_0_10_out(stack=int(get_variable(variable_control_stack)), channel=int(get_variable(variable_control_channel)), value=value)

def printer(text):
    if bool(os.getenv("DEBUG")):
        print(text)

#control RPM
def rpm_control():
    actual_rpm = float(get_variable(variable_actual_rpm))
    requested_rpm = float(get_variable(variable_requested_rpm))
    actual_set_U = read_0_10_out(stack=int(get_variable(variable_control_stack)), channel=int(get_variable(variable_control_channel)))

    #slow down rotating if over maximum
    if actual_rpm > float(get_variable(variable_max_rpm)):
        set_output(actual_set_U - 0.1)
        print("max_rpm_reached")
        return
    
    if requested_rpm == 0:
        set_output(0)
        print("control_set: 0")
        return
    
    if actual_set_U > float(get_variable(variable_max_U)):
        set_output(float(get_variable(variable_max_U)))
        print("Maximum out voltage")
        return
    
    #algorithm for RPM control
    if (requested_rpm - float(get_variable(variable_control_step_rpm))) > actual_rpm:
        set_output(actual_set_U + float(get_variable(variable_control_step_U)))
        printer("RPM control 1 +")
        return
    
    elif (requested_rpm + float(get_variable(variable_control_step_rpm))) < actual_rpm:
        set_output(actual_set_U - float(get_variable(variable_control_step_U)))
        printer("RPM control 1 -")
        return

    if (requested_rpm - float(get_variable(variable_control_step_rpm_1))) > actual_rpm:
        set_output(actual_set_U + float(get_variable(variable_control_step_U_1)))
        printer("RPM control 2 +")
        return

    elif (requested_rpm + float(get_variable(variable_control_step_rpm_1))) < actual_rpm:
        set_output(actual_set_U - float(get_variable(variable_control_step_U_1)))
        printer("RPM control 2 -")
        return

    if (requested_rpm - float(get_variable(variable_control_step_rpm_2))) > actual_rpm:
        set_output(actual_set_U + float(get_variable(variable_control_step_U_2)))
        printer("RPM control 3 +")
        return

    elif (requested_rpm + float(get_variable(variable_control_step_rpm_2))) < actual_rpm:
        set_output(actual_set_U - float(get_variable(variable_control_step_U_2)))
        printer("RPM control 3 -")
        return
    
    print("RPM control any change")