"""
------------------------------------------------------------
OpenMiDyno Firmware

main.py

Target
------
Raspberry Pi Pico 2 W

Purpose
-------
Firmware entry point.

OpenMiDyno v2.0.0
------------------------------------------------------------
"""

import time

from board import CONTROL_PERIOD

from motor import Motor

from pwm_driver import PWMDriver

from encoder import Encoder

from current_sensor import CurrentSensor

from torque_sensor import TorqueSensor

from current_controller import CurrentController

from daq import DAQ

from hardware_backend import HardwareBackend

from serial_interface import SerialInterface


#
# Hardware
#

motor = Motor()

pwm = PWMDriver()

encoder = Encoder()

current_sensor = CurrentSensor()

torque_sensor = TorqueSensor()

controller = CurrentController()

daq = DAQ(

    encoder,

    current_sensor,

    torque_sensor

)

backend = HardwareBackend(

    motor,

    pwm,

    controller,

    encoder,

    current_sensor,

    torque_sensor,

    daq

)

serial = SerialInterface()


#
# Boot
#

print("OpenMiDyno Firmware v2.0.0")
#
# Boot
#

print("OpenMiDyno Firmware v2.0.0")

serial.send("READY")

backend.start()

running = True

serial.send("RUNNING")

time_seconds = 0.0

bus_voltage = 24.0


#
# Main Loop
#

while True:

    #
    # Commands
    #

    command = serial.receive()

    if command is not None:

        command = command.upper()

        if command == "PING":

            serial.send("READY")

        elif command == "START":

            backend.start()

            running = True

            serial.send("RUNNING")

        elif command == "STOP":

            backend.stop()

            running = False

            serial.send("STOPPED")

    #
    # Run Hardware
    #

    if running:

        steady = backend.update(

            CONTROL_PERIOD

        )

        #
        # Telemetry
        #

        serial.send(

            daq.telemetry(

                time_seconds,

                bus_voltage

            )

        )

        #
        # Characterization
        #

        if steady:

            if not backend.next_step():

                backend.stop()

                running = False

                serial.send("STOPPED")

        time_seconds += CONTROL_PERIOD

    time.sleep(CONTROL_PERIOD)