from   machine import Pin, ADC, PWM
import machine
import _thread
import time
import math

#<--------------------------------------------------------------->
# Variable initialisations
#<--------------------------------------------------------------->

#=== PID Parameters ===
Kp = 1.0    # Proportional gain
Ki = 0.1    # Integral gain
Kd = 0.05   # Derivative gain

#=== Servo Setup ===
SERVO_PIN  = 0            # adjust to your servo pin
SERVO_FREQ = 50          # 50Hz for hobby servo
servo      = PWM(Pin(SERVO_PIN))
servo.freq(SERVO_FREQ)

# Center and range for 16-bit duty cycle
SERVO_CENTER = 32768
SERVO_RANGE  = 8000

#=== Global State ===
ir_sensor = {
    'an_right': [0, 0, 0, 0],
    'an_left':  [0, 0, 0, 0],
    'dg_right': [0, 0],
    'dg_left':  [0, 0]
}
trd_En = False

#=== Target grid position (if used by search logic) ===
tgt_row = 5
tgt_col = 10

#=== Digital IR Pins ===
dir_rgt1 = Pin(4, Pin.IN, Pin.PULL_DOWN)
dir_rgt2 = Pin(5, Pin.IN, Pin.PULL_DOWN)
dir_lft1 = Pin(6, Pin.IN, Pin.PULL_DOWN)
dir_lft2 = Pin(7, Pin.IN, Pin.PULL_DOWN)

#=== Analog IR ADCs ===
ir_left  = ADC(Pin(27))
ir_right = ADC(Pin(26))

#=== Shift register pins ===
en_pin   = Pin(0, Pin.OUT)
out_pin  = Pin(1, Pin.OUT)
clk_pin  = Pin(2, Pin.OUT)

#=== PID state variables ===
integral   = 0.0
last_error = 0.0
setpoint   = 0.0   # desired measurement (e.g. zero difference)

def shiftClock(clk, out):
    clk.on();  time.sleep_us(10)
    clk.off(); time.sleep_us(10)

    out.on();  time.sleep_us(10)
    out.off(); time.sleep_us(10)
    time.sleep_ms(200)


#<--------------------------------------------------------------->
# PID Compute Function
#<--------------------------------------------------------------->
def pid_compute(measurement, dt):
    global integral, last_error
    error = setpoint - measurement
    integral += error * dt
    derivative = (error - last_error) / dt if dt > 0 else 0.0
    output = Kp * error + Ki * integral + Kd * derivative
    last_error = error
    return output

#<--------------------------------------------------------------->
# IR Calibration
#<--------------------------------------------------------------->
def calibrate_IR(leftADC, rightADC, en, clk):
    en.on()
    offset = {'an_right': [0,0,0,0], 'an_left': [0,0,0,0]}
    for i in range(500 * 8):
        clk.on(); en.off(); clk.off()
        pos = i % 8
        if pos > 3:
            continue
        offset['an_right'][pos] += rightADC.read_u16()
        offset['an_left'][pos]  += leftADC.read_u16()
    # average
    for j in range(3):
        offset['an_right'][j] //= 500
        offset['an_left'][j]  //= 500
    return offset

#<--------------------------------------------------------------->
# Sensor Reading & Steering Thread
#<--------------------------------------------------------------->
def readSensor():
    global trd_En
    an_conv = 3.3 / 65535
    trd_En = True
    offsets = calibrate_IR(ir_left, ir_right, en_pin, clk_pin)

    prev_time = time.ticks_ms()
    while trd_En:
        # shift register enable
        en_pin.on()
        time.sleep_ms(1)
        # read 3 analog sensors
        for pos in range(4):
            shiftClock(clk_pin, out_pin)
            en_pin.off()
            
            # read and offset
            an_r = (ir_right.read_u16() - offsets['an_right'][pos]) * an_conv
            an_l = (ir_left.read_u16()  - offsets['an_left'][pos])  * an_conv
            ir_sensor['an_right'][pos] = an_r
            ir_sensor['an_left'][pos]  = an_l
            time.sleep_ms(1)

        # compute measurement (difference between central left and right)
        shiftClock(clk_pin, out_pin)
        measurement = ir_sensor['an_left'][2] - ir_sensor['an_right'][2]
        print(f'sns 1 {ir_sensor["an_right"][0] / an_conv}')
        print(f'sns 2 {ir_sensor["an_right"][1] / an_conv}')
        print(f'sns 3 {ir_sensor["an_right"][2] / an_conv}')
        print(f'sns 4 {ir_sensor["an_right"][3] / an_conv}')
        print(f'')
        # compute dt
        now = time.ticks_ms()
        dt = time.ticks_diff(now, prev_time) / 1000
        prev_time = now

        # get PID output
        control = pid_compute(measurement, dt)

        # map control to servo duty
        duty = int(SERVO_CENTER + control * SERVO_RANGE)
        # clamp
        min_d = SERVO_CENTER - SERVO_RANGE
        max_d = SERVO_CENTER + SERVO_RANGE
        duty = max(min(duty, max_d), min_d)
        servo.duty_u16(duty)

        # read digital sensors
        ir_sensor['dg_right'][0] = dir_rgt1.value()
        ir_sensor['dg_left'][0]  = dir_lft1.value()
        ir_sensor['dg_right'][1] = dir_rgt2.value()
        ir_sensor['dg_left'][1]  = dir_lft2.value()

        # debug print
#         print("A L/R:{:.2f}/{:.2f} M:{:.2f} PID:{:.2f} DG_R:{} DG_L:{} SV_D:{}".format(
#             ir_sensor['an_left'][1], ir_sensor['an_right'][2], measurement,
#             control, ir_sensor['dg_right'], ir_sensor['dg_left'], duty
#         ))

        time.sleep(0.05)

#<--------------------------------------------------------------->
# Main function
#<--------------------------------------------------------------->
def main():
    _thread.start_new_thread(readSensor, ())
    col_count = 0
    row_count = 0
    line_int  = 0
    while True:
        if row_count == math.floor((tgt_row + 1) / 2):
            col_count, line_int = search(col_count, line_int, "col")
        else:
            row_count, line_int = search(row_count, line_int, "row")

#<--------------------------------------------------------------->
# Search for rows and columns
#<--------------------------------------------------------------->
def search(count, line_int, cat):
    #initialise variables
    count   = count
    tempInt = line_int
    sensor  = ir_sensor[f'dg_right'][0] if (tgt_row % 2 is 1) and (cat is not "row") else ir_sensor[f'dg_left'] [0]
    
    #check if changed
    if sensor is tempInt:
        return count, tempInt
    
    time.sleep(0.01)
    
    #check if change is the correct value
    if sensor is not 1:
        tempInt = sensor
        return count, tempInt
    
    #check which type of turn it is
    if   (cat is "row") and count is math.floor(tgt_row / 2):
        print("turn left")
    
    elif (cat is "col") and count is tgt_col:
        print("turn right" if (tgt_row % 2 is 1) else "turn left")
    
    count  += 1
    tempInt = sensor
    return count, tempInt
#<--------------------------------------------------------------->
# Code Execution
#<--------------------------------------------------------------->
try:
    readSensor()
except KeyboardInterrupt:
    trd_En = False
    machine.soft_reset()
finally:
    trd_En = False
    machine.soft_reset()