import machine
import utime

print ('lib import done')
# Pin assignments (adjust as needed)
mux_en_pin = machine.Pin(29, machine.Pin.OUT)  # Control group A/B selection
adc_pin1 = machine.ADC(26)  # ADC for group A/B
adc_pin2 = machine.ADC(27)  # ADC for group A/B
adc_pin3 = machine.ADC(28)  # ADC for group A/B

# Servo pins (adjust as needed)
servo1_pin = machine.PWM(machine.Pin(0))
servo2_pin = machine.PWM(machine.Pin(1))
servo3_pin = machine.PWM(machine.Pin(2))
servo4_pin = machine.PWM(machine.Pin(3))
servo5_pin = machine.PWM(machine.Pin(4))
servo6_pin = machine.PWM(machine.Pin(5))

#print ('define pins done')

# Function to select a group (A or B)
 #vstup jenom 1 nebo 2!!!!
def select_group(group_num):
    mux_en_pin.value(group_num - 1)
    #print ('select group done')

def read_potentiometer_group(adc_pins):
    pot_values = []
    for adc_pin in adc_pins:
        reading = adc_pin.read_u16()
        voltage = reading / 65535 * 5 # Assuming 5V reference   # TODO: opravit na 4095
        servo_position = int(voltage * 180 / 5.0)  # Map to 0-180 degrees
        pot_values.append(servo_position)
    #print ('read POT group done')
    return pot_values

def control_servos(servo_pins, positions):
    for i in range(len(servo_pins)):
        servo_pin = servo_pins[i]
        position = positions[i]
        servo_pin.freq(50)
        servo_position = max(min(position, 180), 0)  # Clamp to 0-180
        print (servo_position)
        servo_pin.duty_u16(int(servo_position * 65535 / 180))
    #print ('control servos done')

def scan():
    #while True:
        # Select group A, read values, and control servos
        select_group(1)
        pot_values_a = read_potentiometer_group([adc_pin1, adc_pin2, adc_pin3])
        control_servos([servo1_pin, servo3_pin, servo5_pin], pot_values_a)

        # Select group B, read values, and control servos
        select_group(2)
        pot_values_b = read_potentiometer_group([adc_pin1, adc_pin2, adc_pin3])
        control_servos([servo2_pin, servo4_pin, servo6_pin], pot_values_b)


        #print ('cycle done')
        #utime.sleep_ms(0)  # Adjust the delay as needed




while True:
    scan()
    utime.sleep_ms(0)  # Adjust the delay as needed