from machine import Pin, ADC, TouchPad, PWM
import time
import network
import espnow

# ---------------------- Hardware Initialization ----------------------
GND0 = Pin(18, Pin.OUT)
GND0.off()
GND1 = Pin(19, Pin.OUT)
GND1.off()
V1 = Pin(22, Pin.OUT)
V1.on()

# Joystick ADC Initialization
VRY_L = ADC(35, atten=ADC.ATTN_11DB)  # Upward decreases value
VRX_L = ADC(34, atten=ADC.ATTN_11DB)  # Leftward increases value
VRY_R = ADC(39, atten=ADC.ATTN_11DB)  # Upward decreases value
VRX_R = ADC(36, atten=ADC.ATTN_11DB)  # Leftward increases value

# Capacitive Touch Pad Initialization (Thresholds defined)
t_L = TouchPad(Pin(12))  # Idle: ~840, Pressed: ~620, Threshold: 730
t_R = TouchPad(Pin(15))  # Idle: ~722, Pressed: ~540, Threshold: 630

# PWM/LED Initialization
pwm1 = PWM(Pin(32), freq=20000, duty=0)
LED1_2 = Pin(33, Pin.OUT)  # LED1: Robot Connection/Run Status (Solid: Linked / Blinking: Unlinked / Off: Standby)
LED1_2.off()

pwm2 = PWM(Pin(25), freq=20000, duty=0)
LED2_2 = Pin(26, Pin.OUT)  # LED2: Left Touch Pad Status (On: Pressed / Off: Released)
LED2_2.off()

pwm3 = PWM(Pin(17), freq=20000, duty=0)
LED3_2 = Pin(16, Pin.OUT)  # LED3: Right Touch Pad Status (On: Pressed / Off: Released)
LED3_2.off()

# Joystick Buttons (Active Low: 0 when pressed)
SW_L = Pin(23, Pin.IN, Pin.PULL_UP)
SW_R = Pin(27, Pin.IN, Pin.PULL_UP)

# ---------------------- ESP-NOW Initialization ----------------------
sta = network.WLAN(network.WLAN.IF_STA)
sta.active(True)
sta.disconnect()

print("Device Default MAC Address:", sta.config('mac'))

e = espnow.ESPNow()
e.active(True)
peer = b'\xec\xe34+\xce\x08'  # Robot Receiver MAC Address
try:
    e.add_peer(peer)          # Add peer to avoid send failures
except:
    pass

# ---------------------- Global State Variables ----------------------
last_operation_time = time.time()  # Timestamp of last user input
OPERATE_TIMEOUT = 60               # Enter standby after 1 minute of inactivity
DEVICE_STATE = "STANDBY"           # States: STANDBY / RUNNING
CONNECT_STATE = False              # ESP-NOW Connection Status
BLINK_FLAG = False                 # Toggle flag for LED1 blinking
BLINK_INTERVAL = 0.5               # Blinking interval in seconds
last_blink_time = time.time()      # Timestamp of last blink toggle

# ---------------------- Core Function: Parameter Packing ----------------------
def combine_device_params():
    """
    Structured parameter packing function.
    Format: DeviceState | JoyADC_X,Y... | JoyButtons | TouchButtons | Checksum
    Example: RUNNING|1234,5678,2048,2048|0,0|1,0|42
    """
    # 1. Capture device state
    device_state = DEVICE_STATE
    
    # 2. Read Joystick ADC values (4 axes)
    vrx_l_val = VRX_L.read()
    vry_l_val = VRY_L.read()
    vrx_r_val = VRX_R.read()
    vry_r_val = VRY_R.read()
    joystick_vals = [str(vrx_l_val), str(vry_l_val), str(vrx_r_val), str(vry_r_val)]
    
    # 3. Read Joystick Button states (Convert to 1: Pressed, 0: Idle)
    sw_l_state = "1" if SW_L.value() == 0 else "0"
    sw_r_state = "1" if SW_R.value() == 0 else "0"
    joystick_key_vals = [sw_l_state, sw_r_state]
    
    # 4. Read Touch Pad states (Based on thresholds)
    t_l_state = "1" if t_L.read() < 730 else "0"
    t_r_state = "1" if t_R.read() < 630 else "0"
    touch_key_vals = [t_l_state, t_r_state]
    
    # 5. Combine groups using "|" and group members using ","
    param_str = "|".join([
        device_state,
        ",".join(joystick_vals),
        ",".join(joystick_key_vals),
        ",".join(touch_key_vals),
    ])
    
    # 6. Optional: Simple checksum (Modulo of parameter lengths)
    check_code = str(sum([len(p) for p in [device_state] + joystick_vals + joystick_key_vals + touch_key_vals]) % 100)
    final_param_str = f"{param_str}|{check_code}"
    
    return final_param_str

# ---------------------- Logic: State Update & LED Control ----------------------
def update_device_state():
    """
    Handles state transitions and LED feedback:
    - Switches between STANDBY/RUNNING based on inactivity timeout.
    - LED1: Linked (Solid/Off based on State) or Unlinked (Blinking).
    - LED2/3: Real-time touch feedback.
    """
    global last_operation_time, DEVICE_STATE, CONNECT_STATE, BLINK_FLAG, last_blink_time
    
    # Step 1: Detect User Activity
    has_operation = False
    
    # Check Touch Pads
    if t_L.read() < 730 or t_R.read() < 630:
        has_operation = True
    
    # Check Joystick Buttons
    if SW_L.value() == 0 or SW_R.value() == 0:
        has_operation = True
    
    # Check Joystick Deflection (Threshold to ignore static drift)
    middle_adc = 2048
    adc_threshold = 200
    joystick_adc_list = [VRX_L.read(), VRY_L.read(), VRX_R.read(), VRY_R.read()]
    for adc_val in joystick_adc_list:
        if abs(adc_val - middle_adc) > adc_threshold:
            has_operation = True
            break
    
    # Step 2: Reset timeout timer if activity is detected
    if has_operation:
        last_operation_time = time.time()
    
    # Step 3: Transition Logic
    current_time = time.time()
    if CONNECT_STATE:
        if (current_time - last_operation_time) > OPERATE_TIMEOUT:
            DEVICE_STATE = "STANDBY"
        else:
            DEVICE_STATE = "RUNNING"
    else:
        DEVICE_STATE = "STANDBY" # Force standby if connection is lost
    
    # Step 4: LED1 Control Logic
    if CONNECT_STATE:
        # Connected: Solid On if Running, Off if Standby
        pwm1.duty(100) if DEVICE_STATE == "RUNNING" else pwm1.duty(0)
    else:
        # Not Connected: Blink based on interval
        if (current_time - last_blink_time) > BLINK_INTERVAL:
            BLINK_FLAG = not BLINK_FLAG
            pwm1.duty(100) if BLINK_FLAG else pwm1.duty(0)
            last_blink_time = current_time
    
    # Step 5: LED2/LED3 Real-time Feedback
    pwm2.duty(100) if t_L.read() < 730 else pwm2.duty(0)
    pwm3.duty(100) if t_R.read() < 630 else pwm3.duty(0)

# ---------------------- Main Loop ----------------------
def main():
    global CONNECT_STATE
    print("System started. Attempting to link with robot...")
    
    while True:
        # 1. Pack current sensor data
        param_data = combine_device_params()
        
        # 2. Send via ESP-NOW and update connection state based on ACK result
        # e.send returns True if delivery is successful (blocking mode)
        try:
            CONNECT_STATE = e.send(peer, param_data, True)
        except:
            CONNECT_STATE = False
 
        # 3. Process logic and update hardware indicators
        update_device_state()
        
        # Optional: Debug output and loop delay
        # print(f"Conn: {CONNECT_STATE}, Data: {param_data}")
        # time.sleep(0.05)

if __name__ == "__main__":
    main()