import time
from pyfastservo import adc, dac
from pyfastservo.common import (
    ADC_CH0_HIGH_ADDR, ADC_CH0_LOW_ADDR,
    ADC_CH1_HIGH_ADDR, ADC_CH1_LOW_ADDR,
    CH0_HIGH_WORD_ADDR, CH0_LOW_WORD_ADDR,
    CH1_HIGH_WORD_ADDR, CH1_LOW_WORD_ADDR,
    read_from_memory, write_to_memory
)

def read_adc(channel):
    if channel == 0:
        return adc.read_adc_channel(ADC_CH0_HIGH_ADDR, ADC_CH0_LOW_ADDR)
    elif channel == 1:
        return adc.read_adc_channel(ADC_CH1_HIGH_ADDR, ADC_CH1_LOW_ADDR)
    else:
        raise ValueError("Invalid ADC channel")

def write_dac(value):
    dac.set_dac_output(value)

def read_dac(channel):
    if channel == 0:
        high_word = read_from_memory(CH0_HIGH_WORD_ADDR, 1)[0]
        low_word = read_from_memory(CH0_LOW_WORD_ADDR, 1)[0]
    elif channel == 1:
        high_word = read_from_memory(CH1_HIGH_WORD_ADDR, 1)[0]
        low_word = read_from_memory(CH1_LOW_WORD_ADDR, 1)[0]
    else:
        raise ValueError("Invalid DAC channel")
    return (high_word << 8) | low_word

def perform_loopback_test(test_value):
    print(f"\nSetting DAC output to {test_value}...")
    write_dac(test_value)
    time.sleep(0.1)  # Allow time for the signal to stabilize

    for channel in [0, 1]:
        print(f"\nTesting Channel {channel}")
        
        print(f"Reading ADC CH{channel} value...")
        adc_value = read_adc(channel)
        print(f"ADC CH{channel} readback: {adc_value}")

        print(f"Reading DAC CH{channel} value...")
        dac_value = read_dac(channel)
        print(f"DAC CH{channel} readback: {dac_value}")

        if abs(test_value - adc_value) <= 2 and abs(test_value - dac_value) <= 2:
            print(f"Loopback test for Channel {channel} PASSED!")
        else:
            print(f"Loopback test for Channel {channel} FAILED!")
            print(f"Expected: {test_value}, ADC: {adc_value}, DAC: {dac_value}")

def main():
    test_value = 0x0FFF  # Mid-range value (4095 in decimal)
    perform_loopback_test(test_value)

if __name__ == "__main__":
    main()