# cpb_ticklish.py

# Demonstrate reactive behavior: an idle movement sequence, interrupted with
# reactive movements in response to a sensor input.

# This sketch assumes two external circuits: a resistive soft-sensor connected
# to A2, and a hobby servo on 'SDA A5'.  Please refer to the cpb_soft_sensing.py
# and cpb_sine_servo.py sketches for more details on the wiring.

# ----------------------------------------------------------------
# Import any needed standard Python modules.
import time, math

# Import the board-specific input/output library.
from adafruit_circuitplayground import cp

# Import the low-level hardware libraries.
import board
import digitalio
import analogio
import pwmio

# Import the Adafruit helper library.
from adafruit_motor import servo

# ----------------------------------------------------------------
# Initialize hardware.

# Configure the digital input pin used for its pullup resistor bias voltage.
bias = digitalio.DigitalInOut(board.D10)        # pad A3
bias.switch_to_input(pull=digitalio.Pull.UP)

# Configure the analog input pin used to measure the sensor voltage.
sensor = analogio.AnalogIn(board.A2)
scaling = sensor.reference_voltage / (2**16)

# Create a PWMOut object on pad SDA A5 to generate control signals.
pwm = pwmio.PWMOut(board.A5, duty_cycle=0, frequency=50)

# Create a Servo object which controls a hobby servo using the PWMOut.
actuator = servo.Servo(pwm, min_pulse=1000, max_pulse=2000)
actuator.angle = 0

# Configure the NeoPixel LED array for bulk update using show().
cp.pixels.auto_write = False

# Turn down the NeoPixel brightness, otherwise it is somewhat blinding.
cp.pixels.brightness = 0.5

# ----------------------------------------------------------------
# Define functions used by the main loop.

# Use the CPB NeoPixel LEDS as a dial gauge.  A normalized value between zero
# and one is shown as a spot of light like a dial needle.  The USB connector is
# considered to be the top; the LED to its right is zero, with values increasing
# clockwise around to the LED to the left of the USB.  The spot is anti-aliased
# to increase precision, i.e. it cross-fades between LEDs as the value changes.

def update_LED_dial(value):
    # Clamp the input to the range limits.
    clamped = min(max(value, 0.0), 1.0)

    # Convert the normalized value to an indicator angle in degrees with respect to the top.
    indicator = (clamped * 300) + 30

    # Project the angle onto the NeoPixels. The list assumes the Neopixels are
    # uniformly spaced at 30 degree intervals, with the bottom position absent
    # (e.g. at the battery connector).
    for i, led_angle in enumerate((330, 300, 270, 240, 210, 150, 120, 90, 60, 30)):
        diff = abs(indicator - led_angle)
        if diff < 30:
            c = min(max(255 - int(10*diff), 0), 255)
            cp.pixels[i] = (c, c, c) # shade of gray
        else:
            cp.pixels[i] = (0,0,0)   # no light

    # Send new data to the physical LED chain.
    cp.pixels.show()

# ----------------------------------------------------------------
# Initialize global variables for the main loop.

# Convenient time constant expressed in nanoseconds.
second = 1000000000

# Integer time stamp for the next console output.
next_print_time = time.monotonic_ns()

# Previous touch value, used to detect changes.
last_touch = 0

# Behavior mode symbol.  This is the state label for a simple behavioral state machine.
behavior_mode = 'idle'

# Integer time stamp for a state timeout.
next_timeout_time = 0

# Integer time stamp for next behavior activity to begin.
next_activity_time = time.monotonic_ns() + 2 * second

# Flag to trigger motion.
sweep_start = False

# Commanded value for motion in degrees/second.
sweep_rate = 0

# Current value for motion speed in degrees/second.
sweep_direction = 0

# Integer time stamp for next servo update.
next_servo_update = time.monotonic_ns()

# ----------------------------------------------------------------
# Begin the main processing loop.
while True:

    # Read the current integer clock.
    now = time.monotonic_ns()

    #---- soft sensor input and display -----------------------------
    # Read the integer sensor value and scale it to a value in Volts.
    volts = sensor.value * scaling

    # Normalize the soft sensor reading.  Typically you'll need to adjust these values to your device.
    low_pressure_voltage  = 0.65
    high_pressure_voltage = 0.20
    pressure = (low_pressure_voltage - volts) / (low_pressure_voltage - high_pressure_voltage)

    # Update the LED dial gauge display.
    update_LED_dial(pressure)

    #---- behavior state machine update -----------------------------
    if behavior_mode == 'idle':

        # Check whether the touch sensor has been activated
        if pressure > 0.5:
            # Change behavior state, which will create movement on the next cycle.
            behavior_mode = 'tickled'
            next_activity_time = now
            next_timeout_time = now + 4 * second
            print("Entering tickled state.")

        # Check whether to start another idle movement
        elif now >= next_activity_time:
            next_activity_time += 12 * second
            sweep_rate = 60 # deg/sec
            sweep_start = True

    elif behavior_mode == 'tickled':

        # Check whether the touch sensor is still activated
        if pressure > 0.5:
            next_timeout_time = now + 4 * second

        # Check whether to start another tickled movement
        if now >= next_activity_time:
            next_activity_time += 3 * second
            sweep_rate = 120 # deg/sec
            sweep_start = True

        elif now >= next_timeout_time:
            print("Ending tickled state.")
            next_activity_time = now + 6*second
            behavior_mode = 'idle'

    #---- periodic servo motion commands ----------------------------
    # If the time has arrived to update the servo command signal:
    if now >= next_servo_update:
        next_servo_update += 20000000  # 20 msec in nanoseconds (50 Hz update)

        # If a start event has been received, move in a positive direction.
        if sweep_start is True:
            print(f"Starting sweep motion at {sweep_rate} deg/sec.")
            sweep_start = False
            sweep_direction = sweep_rate

        # Calculate the next servo position.
        next_angle = actuator.angle + sweep_direction / 50.0

        # Change activity at the limits
        if next_angle > 180.0:
            sweep_direction = -sweep_direction
            next_angle = actuator.angle + sweep_direction / 50.0

        elif next_angle < 0.0:
            print("Ending sweep motion.")
            next_angle = 0.0
            sweep_direction = 0.0

        # Send the angle to the actuator.
        actuator.angle = next_angle

    #---- periodic console output -----------------------------------
    # Poll the time stamp to decide whether to emit console output.
    if now >= next_print_time:

        # Advance the time stamp to the next event time.
        next_print_time += second / 2

        # Periodically print the readings on the console.
        print(f"({volts}, {pressure}, {actuator.angle})")

    #---- end of the event loop -------------------------------------