from machine import Pin, PWM
import time
import network
import BlynkLib
from neopixel import Neopixel
import utime
import random

time_tick = 0.0001

# Change LED lights every N seconds
led_frequency = int(2 / time_tick)
led_brightness = 42
led_count = 6

red = (255, 0, 0)
orange = (255, 50, 0)
yellow = (255, 100, 0)
green = (0, 255, 0)
blue = (0, 0, 255)
indigo = (100, 0, 90)
violet = (200, 0, 100)
blank = (0,0,0)
colors_rgb = [red, orange, yellow, green, blue, indigo, violet, blank]

servo_min = 520000
servo_max = 2200000
servo_home = (servo_min + servo_max) // 2

servo_max_speed = 60
servo_acceleration = 5
servo_near_goal_threshold = 4

# Goal distance from home position
servo_goal = 0
# Current position from home position
servo_position = 0
# Absolute velocity towards goal
servo_velocity = 0

behavior_move = 0
behavior_stop = 1
# After reaching destination, stop for N seconds
open_stop_time = int(10 / time_tick)
closed_stop_time = int(2 / time_tick)
current_behavior = behavior_move

def servo_set(position):
    pwm.duty_ns(position)

def servo_init():
    servo_set(servo_home)

def calc_sqrt(x):
    res = 0
    for i in range(7, -1, -1):
        tmp = res | (1 << i)
        if tmp * tmp <= x:
            res = tmp
    return res

# Smooth control a servo motor
#
# The servo has a current position, current speed and a goal.  The PWM required for the motor is simulated by a timer
# on a GPIO because later on, many motors are required to be controlled and there aren't enough PWM's for them.
#
# The servo is controlled in 20ms periods, where the goal, G, dictates the position of the servo, i.e., the high time
# of the PWM, according to the following formula:
#
#    SERVO_HOME + G
#
# In other words, G is the offset from servo's home (1.5ms).  G is limited to +-SERVO_RANGE.  All values are in us.
#
# Current position, P, dictates the current position of the servo and follows the same formula as G.  In each period
# of the control algorithm, P moves towards G with the current speed V.  During the movement, V is increased until
# it reaches its maximum of SERVO_MAX_SPEED, with SERVO_ACCELERATION acceleration.  Once P gets close to G,
# it will start decelerating with SERVO_ACCELERATION.
#
# Given maximum speed V and deceleration -A, the time it takes for the speed to reach zero is given by:
#
#    t = V / A
#
# and the distance D traveled is given by:
#
#    D = -1/2 A * t^2 + V * t = V^2 / 2A
#
# With a large distance to travel (from position P), the speed vs distance graph would thus look like this:
#
#    v ^
#      |
#      |    __________
#    V |  _-|        |-_
#      | /  |        |  \
#      |/   |        |   \
#      ||   |        |   |
#      ------------------------>
#      O    D       P-D  P     d
#
# Taking into consideration the cases where P < D and P < 2D, one simple solution is to accelerate when the
# point (p, v) showing current position and velocity is below the curve L from O to (D, V) and decelerate
# otherwise.  There are two options to prevent oscillation around O.  One is to ensure V doesn't go over the
# curve L, and the other is to set velocity to zero when getting close enough to O.  The first method suffers
# from arithmetic errors calculating points of L, while the second reduces precision.  A hybrid solution is
# possible.  First, curve L can be overestimated so that clamping velocities higher than L to L wouldn't bring
# them below L.  This ensures that around O, V is very small.  Therefore, a tiny threshold can be used to set
# V to 0 when getting close to O.

def servo_control():
    global servo_goal
    global servo_position
    global servo_velocity

    distance = servo_goal - servo_position

    # if at goal, do nothing
    if distance == 0:
        return

    abs_distance = -distance if distance < 0 else distance
    allowed_v_underestimate = calc_sqrt(2 * servo_acceleration * abs_distance)
    allowed_v_overestimate = allowed_v_underestimate + 1 

    # if going faster than currently allowed, slow down
    if servo_velocity > allowed_v_overestimate:
        servo_velocity = allowed_v_overestimate

    # TODO: make velocity signed, and check if it is in the opposite direction of goal.  This means the goal was changed before I reached it.  In such
    # a case, I should always decelerate first and only then change direction
    new_velocity = 0 
    delta_distance = servo_velocity
    possible_acceleration = servo_acceleration
    possible_deceleration = servo_acceleration

    if servo_max_speed - servo_velocity < servo_acceleration:
        possible_acceleration = servo_max_speed - servo_velocity
    if servo_velocity < servo_acceleration:
        possible_deceleration = servo_velocity

    # the distance that would be traveled by the next time this function is called is given by:
    #
    #     d = 1/2at^2 + vt
    #
    # where t is always 1.
    if servo_velocity < allowed_v_underestimate:
        # accelerate
        new_velocity = servo_velocity + possible_acceleration
        delta_distance += possible_acceleration // 2
    else:
        # decelerate
        new_velocity = servo_velocity - possible_deceleration
        delta_distance = delta_distance - possible_deceleration // 2 if delta_distance > possible_deceleration // 2 else 0

    if delta_distance > abs_distance:
        delta_distance = abs_distance

    # set the new location
    if servo_goal > servo_position:
        servo_position += delta_distance
    else:
        servo_position -= delta_distance

    # if reached close enough to goal, just get to goal and be done with it
    if abs_distance - delta_distance < servo_near_goal_threshold:
        servo_position = servo_goal
        new_velocity = 0

    # set the new velocity
    if new_velocity > servo_max_speed:
        new_velocity = servo_max_speed
    servo_velocity = new_velocity

    # set the corresponding PWM to the new position
    servo_set(servo_home + servo_position)

def adjust_led():
    strip.set_pixel(random.randint(0, led_count-1), colors_rgb[random.randint(0, len(colors_rgb)-1)])
    strip.set_pixel(random.randint(0, led_count-1), colors_rgb[random.randint(0, len(colors_rgb)-1)])
    strip.set_pixel(random.randint(0, led_count-1), colors_rgb[random.randint(0, len(colors_rgb)-1)])
    strip.set_pixel(random.randint(0, led_count-1), colors_rgb[random.randint(0, len(colors_rgb)-1)])
    strip.set_pixel(random.randint(0, led_count-1), colors_rgb[random.randint(0, len(colors_rgb)-1)])
    strip.show()    


# Setup peripherals
# PWM
pwm = PWM(Pin(15)) 
pwm.freq(50) #20ms PWM period
# LED
strip = Neopixel(led_count, 0, 0, "RGB")
strip.brightness(led_brightness)

# Initially go to home
servo_init()

# Set goal to one side
servo_goal = servo_max - servo_home

led_counter = 0

wait_counter = 0

while True:
    servo_control()

    if current_behavior == behavior_move:
        # Stop for a while once goal is reached
        if servo_position == servo_goal:
            current_behavior = behavior_stop
            wait_counter = 0

    elif current_behavior == behavior_stop:
        wait_counter += 1
        stop_time = open_stop_time if servo_goal < 0 else closed_stop_time
        if wait_counter >= stop_time:
            # Move to the other end after the wait
            current_behavior = behavior_move
            servo_goal = -servo_goal

    led_counter += 1
    if led_counter >= led_frequency:
        led_counter = 0
        adjust_led()

    # Control frequency
    time.sleep(time_tick)