"""Simulates speed testing."""
import matplotlib.pyplot as plt
from time import sleep
from math import ceil


dt = 0.050  # seconds
thread_pitch = 0.5906

run_speed = 240.0  # Hz
rampdown_speed = 20.0  # Hz

accel_time = 2.0  # seconds
decel_time = 2.0  # seconds

stroke_length = 60  # inches
upper_offset = 2  # inches
lower_offset = 2  # inches

motor_full_speed_hz = 120  # Hz
motor_full_speed_rpm = 1200  # RPM


def sim(run_freq, approach_freq, stroke_limit):
    """Run the main simulation loop."""
    strokes = 0

    time_to_rampdown = ((run_freq - rampdown_speed) / motor_full_speed_hz) * decel_time
    print("Rampdown Time: {} s.".format(time_to_rampdown))
    distance_to_rampdown = 0.75 * (run_freq - rampdown_speed) * (motor_full_speed_rpm / motor_full_speed_hz) * (1.0 / 60.0) * time_to_rampdown * 0.5906
    print("Rampdown Distance: {} in.".format(distance_to_rampdown))

    time_to_turnaround = (rampdown_speed / motor_full_speed_hz) * decel_time
    print("Turn Time: {} s.".format(time_to_turnaround))
    distance_to_turnaround = 0.75 * rampdown_speed * (motor_full_speed_rpm / motor_full_speed_hz) * (1.0 / 60.0) * time_to_turnaround * 0.5906
    print("Turn Distance: {} in.".format(distance_to_turnaround))

    upper_rampdown_target = stroke_length - (upper_offset + distance_to_rampdown)
    upper_turnaround_target = stroke_length - (upper_offset + distance_to_turnaround)
    upper_offset_target = stroke_length - upper_offset
    lower_rampdown_target = lower_offset + distance_to_rampdown
    lower_turnaround_target = lower_offset + distance_to_turnaround

    print("--")
    print("Targets")
    print("Lower Offset: {} in.".format(lower_offset))
    print("Upper Rampdown Target: {} in.".format(upper_rampdown_target))
    print("Turnaround Target: {} in.".format(upper_turnaround_target))
    print("Upper Offset: {} in.".format(upper_offset_target))
    print("Lower Rampdown Target: {} in.".format(lower_rampdown_target))
    print("Lower Turnaround Target: {} in.".format(lower_turnaround_target))
    print("--")

    position = lower_offset
    time = 0.0

    direction = 1
    starting_speed = 0.0  # Hz
    speed = starting_speed
    last_speed = speed

    speed_array = []
    position_array = []
    time_array = []
    stroke_part = -1

    while strokes != stroke_limit:
        last_speed = speed
        if direction == 1:
            if position < upper_rampdown_target:
                # below offset and rampdown distance, ramp up to setpoint, then run at setpoint
                if stroke_part != 0:
                    stroke_part = 0
                    print("{} - {}".format(stroke_part, position))
                if speed < run_freq:
                    speed += (motor_full_speed_hz / accel_time) * dt
                if speed > run_freq:
                    speed = run_freq
            elif position >= upper_rampdown_target and position < upper_turnaround_target:
                # between rampdown distance and turnaround distance, ramp to rampdown speed, then run at rampdown speed
                if stroke_part != 1:
                    stroke_part = 1
                    print("{} - {}".format(stroke_part, position))
                if speed > rampdown_speed:
                    speed += -1.0 * (motor_full_speed_hz / decel_time) * dt
            elif position >= upper_turnaround_target:
                # above turnaround distance, ramp to 0 and change direction
                if stroke_part != 2:
                    stroke_part = 2
                    print("{} - {}".format(stroke_part, position))
                if speed > 0:
                    speed += -1.0 * (motor_full_speed_hz / decel_time) * dt
                if speed <= 0:
                    speed = 0
                    direction = -1

        elif direction == -1:
            if position > lower_rampdown_target:
                # above offset and rampdown distance, ramp up to setpoint (negative), then run at setpoint
                if stroke_part != 3:
                    stroke_part = 3
                    print("{} - {}".format(stroke_part, position))
                if speed > (-1.0 * run_freq):
                    speed += -1.0 * (motor_full_speed_hz / accel_time) * dt
                if speed < (-1.0 * run_freq):
                    speed = -1.0 * run_freq
            elif position > lower_turnaround_target and position <= lower_rampdown_target:
                # between turnaround distance and rampdown distance, ramp ro rampdown speed, then run at rampdown speed
                if stroke_part != 4:
                    stroke_part = 4
                    print("{} - {}".format(stroke_part, position))
                if speed < (-1.0 * rampdown_speed):
                    speed += 1.0 * (motor_full_speed_hz / decel_time) * dt
            elif position <= lower_turnaround_target:
                # below turnaround distance, ramp to 0 and change direction
                if stroke_part != 5:
                    stroke_part = 5
                    print("{} - {}".format(stroke_part, position))
                if speed < 0:
                    speed += 1.0 * (motor_full_speed_hz / decel_time) * dt
                if speed >= 0:
                    speed = 0
                    direction = 1
                    strokes += 1

        delta_x = (speed + last_speed) / 2.0 * (motor_full_speed_rpm / motor_full_speed_hz) * (dt / 60.0) * thread_pitch
        # delta_x = speed * (motor_full_speed_rpm / motor_full_speed_hz) * (dt / 60.0) * thread_pitch
        position += delta_x
        time += dt
        speed_array.append(speed * (motor_full_speed_rpm / motor_full_speed_hz))
        position_array.append(position)
        time_array.append(time)
        print("Time: {} sec., Position: {} in., Speed: {}".format(time, position, speed * (motor_full_speed_rpm / motor_full_speed_hz)))
        # sleep(dt)

    SPM = 60.0 / (time / strokes)
    max_position = max(position_array)
    min_position = min(position_array)
    print("Max: {}, Min: {}".format(max_position, min_position))
    print("SPM: {}".format(SPM))
    fig, ax1 = plt.subplots()
    ax1.plot(time_array, speed_array, "b-")
    ax1.set_ylabel("RPM", color="b")
    ax2 = ax1.twinx()
    ax2.plot(time_array, position_array, "r-")
    ax2.set_ylabel("in.", color="r")
    fig.tight_layout()
    plt.show()


sim(run_speed, rampdown_speed, 1)
