from unihiker import GUI
from pinpong.board import Board, Pin
from pinpong.extension.unihiker import *

import serial
import serial.tools.list_ports
import time

Board().begin()
u_gui = GUI()

p_left_pin = Pin(Pin.P1, Pin.ANALOG)
p_right_pin = Pin(Pin.P0, Pin.ANALOG)

btn_success_pin = Pin(Pin.P3, Pin.IN)
btn_fail_pin = Pin(Pin.P4, Pin.IN)

u_gui.draw_text(text="Test 3: 125/114", x=40, y=5, font_size=18, color="#0000FF")
status_text = u_gui.draw_text(text="Initializing...", x=50, y=35, font_size=10, color="gray")

u_gui.draw_text(text="Step:", x=20, y=60, font_size=12, color="black")
info_text = u_gui.draw_text(text="--", x=60, y=55, font_size=25, color="#FF0000")

u_gui.draw_text(text="L(P1):", x=10, y=95, font_size=10, color="black")
val_l_text = u_gui.draw_text(text="0", x=55, y=90, font_size=15, color="purple")

u_gui.draw_text(text="R(P0):", x=120, y=95, font_size=10, color="black")
val_r_text = u_gui.draw_text(text="0", x=165, y=90, font_size=15, color="purple")

u_gui.draw_text(text="Avg:", x=10, y=120, font_size=10, color="black")
avg_text = u_gui.draw_text(text="0", x=55, y=115, font_size=15, color="blue")
action_text = u_gui.draw_text(text="--", x=120, y=115, font_size=15, color="orange")

u_gui.draw_text(text="Success:", x=10, y=150, font_size=10, color="green")
success_text = u_gui.draw_text(text="0", x=70, y=145, font_size=18, color="green")
u_gui.draw_text(text="Fail:", x=120, y=150, font_size=10, color="red")
fail_text = u_gui.draw_text(text="0", x=160, y=145, font_size=18, color="red")

ser = None
ports = list(serial.tools.list_ports.comports())
port_name = None
for p in ports:
    if "ACM" in p.device or "USB" in p.device:
        port_name = p.device
        break
if port_name:
    try:
        ser = serial.Serial(port_name, 9600, timeout=0.1)
        status_text.config(text=f"Conn: {port_name}", color="green")
    except:
        status_text.config(text="Connect Failed", color="red")

cycle_count = 0
success_count = 0
fail_count = 0
last_p3 = 0
last_p4 = 0
waiting_judgement = False 

def click_start():
    if ser and ser.is_open:
        ser.write(b'1')
        status_text.config(text="Running", color="blue")

def click_stop():
    if ser and ser.is_open:
        ser.write(b'0')
        status_text.config(text="Reset", color="red")
        action_text.config(text="--")

def click_clear():
    global success_count, fail_count
    success_count = 0
    fail_count = 0
    success_text.config(text="0")
    fail_text.config(text="0")
    action_text.config(text="--")

u_gui.add_button(text="Start", x=20, y=190, w=80, h=40, onclick=click_start)
u_gui.add_button(text="Reset", x=120, y=190, w=80, h=40, onclick=click_stop)
u_gui.add_button(text="Cls", x=90, y=240, w=50, h=30, onclick=click_clear)

while True:
    val_l = p_left_pin.read_analog() 
    val_r = p_right_pin.read_analog() 
    
    val_l_text.config(text=str(val_l))
    val_r_text.config(text=str(val_r))
    
    if ser and ser.is_open and ser.in_waiting > 0:
        try:
            line = ser.readline().decode().strip()
            
            if "STEP:" in line:
                s_num = line.split(":")[1]
                info_text.config(text=s_num)
            
            elif "WAITING" in line:
                info_text.config(text="Wait Btn")

            elif "STATUS:GRIPPED" in line:
                info_text.config(text="CHK")
                avg_val = (val_l + val_r) / 2
                avg_text.config(text=str(int(avg_val)))
                
                cmd = b'e'
                sub_deg = 0
                
                if avg_val < 1200:
                    cmd = b'a'; sub_deg = 6
                elif avg_val < 1400:
                    cmd = b'b'; sub_deg = 4
                elif avg_val < 1600:
                    cmd = b'c'; sub_deg = 2
                elif avg_val < 1800:
                    cmd = b'd'; sub_deg = 1
                else:
                    cmd = b'e'; sub_deg = 0
                
                action_text.config(text=f"+{sub_deg} deg")
                print(f"Read: {avg_val}, Send: {cmd}")
                time.sleep(0.1)
                ser.write(cmd) 
            
            elif "CYCLE_DONE" in line:
                waiting_judgement = True
                info_text.config(text="Judge")
                
        except:
            pass

    c_p3 = btn_success_pin.value()
    c_p4 = btn_fail_pin.value()
    
    if c_p3 == 1 and last_p3 == 0:
        if waiting_judgement:
            success_count += 1
            success_text.config(text=str(success_count))
            waiting_judgement = False
        if ser: ser.write(b'1') 
            
    if c_p4 == 1 and last_p4 == 0:
        if waiting_judgement:
            fail_count += 1
            fail_text.config(text=str(fail_count))
            waiting_judgement = False
        if ser: ser.write(b'1') 

    last_p3 = c_p3
    last_p4 = c_p4
    
    time.sleep(0.05)