import serial
import time

# 확인된 포트 경로 설정
SERIAL_PORT = "/dev/cu.usbserial-0001"
BAUD_RATE = 420000

CRSF_SYNC = 0xC8
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16

def parse_channels(payload):
    if len(payload) < 22:
        return []
    
    # 22바이트의 채널 데이터를 정수로 변환 후 11비트씩 언패킹
    data = int.from_bytes(payload[:22], byteorder='little')
    channels = []
    for _ in range(16):
        raw_val = data & 0x07FF  # 11-bit 마스크
        # CRSF 원본 값(172~1811)을 RC 마이크로초(1000~2000µs) 단위로 변환
        us_val = int((raw_val - 172) * (2000 - 1000) / (1811 - 172) + 1000)
        channels.append(us_val)
        data >>= 11
    return channels

def main():
    try:
        ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=0.1)
        print(f"Connected to {SERIAL_PORT} at {BAUD_RATE} baud.")
        print("스틱과 스위치를 움직여 채널 값을 확인하세요. (종료: Ctrl+C)\n")
    except Exception as e:
        print(f"포트 연결 오류: {e}")
        return

    buf = bytearray()
    while True:
        try:
            if ser.in_waiting:
                buf.extend(ser.read(ser.in_waiting))

                while len(buf) > 3:
                    if buf[0] != CRSF_SYNC:
                        buf.pop(0)
                        continue

                    length = buf[1]
                    total_packet_len = length + 2

                    if len(buf) < total_packet_len:
                        break  # 전체 패킷 대기

                    packet = buf[:total_packet_len]
                    buf = buf[total_packet_len:]

                    frame_type = packet[2]
                    if frame_type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
                        payload = packet[3:-1]  # CRC 제외 payload
                        channels = parse_channels(payload)
                        
                        # read_crsf.py 출력 부분 수정
                        print(
                            f"\rCH1: {channels[0]:4d} | CH2: {channels[1]:4d} | "
                            f"CH3: {channels[2]:4d} | CH4: {channels[3]:4d} | "
                            f"CH5(SA/SC): {channels[4]:4d} | CH6(SD): {channels[5]:4d} | "
                            f"CH7: {channels[6]:4d} | CH8: {channels[7]:4d}", 
                            end="", flush=True
                            )

        except KeyboardInterrupt:
            print("\n모니터링 종료")
            break
        except Exception as e:
            print(f"\n데이터 수신 오류: {e}")
            break

    ser.close()

if __name__ == "__main__":
    main()