import odrive
from odrive.enums import *
import time
import math
import subprocess
import threading

# Put the name of you MP3 file here:
audio_file = "Made_Me_Like_This.mp3"

# Calculate the length of the song
result = subprocess.run(["ffprobe", "-v", "error", "-show_entries", "format=duration",
                         "-of", "default=noprint_wrappers=1:nokey=1", audio_file], stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
audio_duration = float(result.stdout)



# Function to play the audio:
def play_audio():
    subprocess.run(["ffplay", "-nodisp", "-autoexit", audio_file])


def configure_odrive(odrv0):
    print("Configuring ODrive with provided settings...")

    odrv0.axis0.motor.config.current_lim = 80  # Overordnet
    odrv0.axis0.controller.config.vel_limit = 10  # Overordnet
    odrv0.axis0.motor.config.torque_constant = 8.27 / 90
    odrv0.axis0.motor.config.motor_type = MOTOR_TYPE_HIGH_CURRENT
    odrv0.axis0.encoder.config.cpr = 8192
    odrv0.axis0.motor.config.calibration_current = 20  # under kalibrering
    odrv0.axis0.encoder.config.use_index = True
    # odrv0.config.brake_resistance = 2  # Uncomment if needed
    odrv0.axis0.motor.config.pole_pairs = 7
    odrv0.config.dc_max_negative_current = -2

    odrv0.axis0.controller.config.input_mode = INPUT_MODE_POS_FILTER
    odrv0.axis0.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL

    print("Starting full calibration sequence...")
    odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
    while odrv0.axis0.current_state != AXIS_STATE_IDLE:
        time.sleep(0.1)
    print("Full calibration sequence completed")

    print("Setting to closed-loop control mode...")
    odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
    print("Closed-loop control mode set")


def start_and_skip_config(odrv0):
    odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
    print("Closed-loop control mode set")


def synchronize_motor_with_music(odrv0, bpm, duration, amplitude=0.25, phase=0):
    # Start playing the audio in a separate thread
    audio_thread = threading.Thread(target=play_audio)
    time.sleep(2)
    audio_thread.start()

    frequency = bpm / 60  # converting BPM to Hz
    sampling_rate = 30  # 30Hz sampling rate
    start_time = time.time()
    
    time_elapsed = 0

    while time_elapsed < duration:
        if odrv0.axis0.error != 0:
            print(f"Axis error: {odrv0.axis0.error}")
            break

        current_time = time.time() - start_time
        time_elapsed = current_time

        # Calculate the position command using the sine function
        pos_command = amplitude * math.sin(2 * math.pi * frequency * current_time + phase)

        # Send the command to the motor
        odrv0.axis0.controller.input_pos = pos_command

        # Wait until the next interval
        time.sleep(1 / sampling_rate)

    # Wait for the audio to finish
    audio_thread.join()

def main():
    print("Looking for ODrive...")
    odrv0 = odrive.find_any()
    print("ODrive found")

    # First time you run the code: 
    configure_odrive(odrv0)

    # When device is configured, use this to save time:
    #start_and_skip_config(odrv0=odrv0)


    bpm = 127 / 4  # Beats per minute of the music track
    duration = audio_duration  # Duration in seconds for how long you want the platform to run
    amplitude = 3  # Amplitude of the sine wave for the engine driving the platform
    phase = 0  # Initial phase
    
    input("Press Enter to start...")

    try:
        synchronize_motor_with_music(odrv0, bpm, duration, amplitude, phase)
    except KeyboardInterrupt:
        print("Synchronization interrupted.")
    finally:
        # Stop the motor at the end of the script
        odrv0.axis0.requested_state = AXIS_STATE_IDLE

    print("Synchronization completed.")

if __name__ == "__main__":
    main()