# This file is part of Fast Servo Software Package.
#
# Copyright (C) 2023 Jakub Matyas
# Warsaw University of Technology <jakubk.m@gmail.com>
# SPDX-License-Identifier: GPL-3.0-or-later

from pyfastservo.dac import (
    CH0_HIGH_WORD_ADDR,
    CH0_LOW_WORD_ADDR,
    CH1_HIGH_WORD_ADDR,
    CH1_LOW_WORD_ADDR,
    write_sample,
    manual_override,
    read_from_memory as dac_read_from_memory
)
from pyfastservo.adc import (
    ADC_CH0_HIGH_ADDR,
    ADC_CH0_LOW_ADDR,
    ADC_CH1_HIGH_ADDR,
    ADC_CH1_LOW_ADDR,
    read_from_memory as adc_read_from_memory
)

def main():
    # Apply manual override for DAC
    manual_override(True)

    # Write constant value to DAC channels
    constant_value = 0x1FFF
    write_sample(0, constant_value)
    write_sample(1, constant_value)

    # Verify DAC register values
    dac_ch0 = (dac_read_from_memory(CH0_HIGH_WORD_ADDR, 1)[0] << 8) | dac_read_from_memory(CH0_LOW_WORD_ADDR, 1)[0]
    dac_ch1 = (dac_read_from_memory(CH1_HIGH_WORD_ADDR, 1)[0] << 8) | dac_read_from_memory(CH1_LOW_WORD_ADDR, 1)[0]

    print(f"DAC CH0: 0x{dac_ch0:04X}")
    print(f"DAC CH1: 0x{dac_ch1:04X}")

    # Read ADC values
    adc_ch0 = (adc_read_from_memory(ADC_CH0_HIGH_ADDR, 1)[0] << 8) | adc_read_from_memory(ADC_CH0_LOW_ADDR, 1)[0]
    adc_ch1 = (adc_read_from_memory(ADC_CH1_HIGH_ADDR, 1)[0] << 8) | adc_read_from_memory(ADC_CH1_LOW_ADDR, 1)[0]

    print(f"ADC CH0: 0x{adc_ch0:04X}")
    print(f"ADC CH1: 0x{adc_ch1:04X}")

if __name__ == "__main__":
    main()