# Import step-response and neopixel libraries
from steptime import STEPTIME
from ws2812 import WS2812
from ssd1306 import SSD1306_I2C

# Import native libraries
from machine import Pin, freq, I2C
import utime

# Set up clock frequency, needed for steptime lib
freq(250000000)

# Power up built-in XIAO NeoPixel LED
power = machine.Pin(11, machine.Pin.OUT)
power.value(1)

# Define colors, we need 6, one for each of the touch buttons
RED = (255, 0, 0)
YELLOW = (255, 150, 0)
GREEN = (0, 255, 0)
CYAN = (0, 255, 255)
BLUE = (0, 0, 255)
PURPLE = (180, 0, 255)
BLACK = (0, 0, 0) # And black, of course

# Init LED and set it to initial color
led = WS2812(12, 1, 0.5, 6) # args: pin_num, led_count, brightness, state_machine_id (6, since we take 0-5 with touch pads)

# Define function for setting neopixel color
def set_led_color(color):
led.pixels_fill(color)
led.pixels_show()

set_led_color(RED)

# Init OLED display
i2c = I2C(1, scl=Pin(7), sda=Pin(6), freq=200000)#Grove - OLED Display 0.96" (SSD1315)
oled = SSD1306_I2C(128, 64, i2c)

# Configure left side buttons
# x [26]
# x x [27, 1]
# x [2]
Pin(26,Pin.IN,Pin.PULL_UP)
Pin(27,Pin.IN,Pin.PULL_UP)
Pin(1,Pin.IN,Pin.PULL_UP)
Pin(2,Pin.IN,Pin.PULL_UP)

# Configure right side buttons
# x [4]
# x [3]
Pin(3,Pin.IN,Pin.PULL_UP)
Pin(4,Pin.IN,Pin.PULL_UP)

actions = [
(set_led_color, RED),
(set_led_color, YELLOW),
(set_led_color, GREEN),
(set_led_color, CYAN),
(set_led_color, BLUE),
(set_led_color, PURPLE),
]

# Create megastructure so we can loop through it comfortably
# StateMachine, MinVal, action, color, button state
channels = [
[STEPTIME(0,26), [1e6], actions[0], [BLACK], [False]], # STEPTIME args: state_machine_id, pin_num
[STEPTIME(1,27), [1e6], actions[1], [BLACK], [False]],
[STEPTIME(2,1), [1e6], actions[2], [BLACK], [False]],
[STEPTIME(3,2), [1e6], actions[3], [BLACK], [False]],
[STEPTIME(4,3), [1e6], actions[4], [BLACK], [False]],
[STEPTIME(5,4), [1e6], actions[5], [BLACK], [False]],
]

buttons = [0, 0, 0, 0, 0, 0]

loop = 200
settle = 20000
thresh = 10000

def process_touch():
out_parts = []
for ch in channels:
sm, min_val, (fn, val), color, btn_state = ch
sm.put(loop)
sm.put(settle)
result = 4294967296 - sm.get()
if result < min_val[0]:
min_val[0] = result
out_parts.append(str(result - min_val[0]))
if result - min_val[0] > thresh:
btn_state[0] = True
else:
btn_state[0] = False

line = ",".join(out_parts)
#print(f"7500,{line}") # 7500 for scale

def draw_character():
oled.fill(0)
if channels[0][4][0]:
set_led_color(YELLOW)
oled.text("HEY!",10,20)
else:
set_led_color(YELLOW)
oled.text("NOP!",20,10)
oled.show()

while True:
process_touch()
draw_character()