import machine
import utime
import time
import math
import sys
import select

lens_cs = machine.Pin(7, machine.Pin.IN)
body_cs = machine.Pin(8, machine.Pin.OUT)
vd_lens = machine.Pin(9, machine.Pin.OUT)
tx_pin = machine.Pin(16, machine.Pin.OUT)

poll_obj = ''

focal_length = 0
aperture_raw = 0
focus = 0
focus_limit = 0
status_sent = False

counter = 0 #VD_LENS pulse counter
ap = 4884 #aperture
ap_odd = False #variable for controlling one bit in send messages

focus_pos = 0 #focus position to be set manually

af_in_progress = False
focus_mode = "MF" #AF/MF

focus_min = 12000
focus_max = 23000
ap_min = 4884
ap_max = 6132

focus_pos = 0
change_focus = False

utime.sleep(1)
vd_lens.value(1)
body_cs.value(0)
tx_pin.value(0)

utime.sleep(0.1)

vd_lens.value(0)
tx_pin.value(1)
time.sleep_ms(6)
tx_pin.value(0)
time.sleep_ms(106)
tx_pin.value(1)
time.sleep_ms(64)
tx_pin.value(0)
time.sleep_ms(80)
vd_lens.value(1)
time.sleep_ms(20)

rx_ack = False

def Comms_Init():
    global poll_obj
    
    poll_obj = select.poll()
    poll_obj.register(sys.stdin, select.POLLIN)
    
def Comms_Read():
    global poll_obj, ap, focus_mode, change_focus, focus_pos
    
    poll_results = poll_obj.poll(1)
    if poll_results:
        rw = sys.stdin.readline().strip()
        rw = rw.upper()
        if "APERTURE" in rw:
            data = rw.split(' ')
            ap = int(data[1])
        if "FOCUS" in rw:
            data = rw.split(' ')
            focus_pos = int(data[1])
            change_focus = True
        if "AF" in rw:
            focus_mode = "AF"
        if "MF" in rw:
            focus_mode = "MF"
            

def focal_length_conv(raw):
    return (math.exp((raw+92063.3)/8082.9)+406.979)/3256.82
    
def aperture_count(value, leftMin=2.8, leftMax=16, rightMin=4884, rightMax=6132):
    leftSpan = leftMax - leftMin
    rightSpan = rightMax - rightMin

    valueScaled = float(value - leftMin) / float(leftSpan)
    return rightMin + (valueScaled * rightSpan)

def aperture(value):
    if value < ap_min:
        value = ap_min
    if value > ap_max:
        value = ap_max
    return value
    
def rx_end(pin):
    rx_ack = True
    
def vd_pulse():
    global counter
    vd_lens.value(0)
    time.sleep_us(30)
    vd_lens.value(1)
    if counter > 239:
        counter = 1
    else:
        counter += 1
    
def print_rx():
    global focal_length, aperture_raw, focus, focus_limit, counter
    
    msg = b''
    msg_len = 0
    index = 0
    read_in_progress = False
    
    if uart_bodytx.any():
        received_data = uart_bodytx.read()
        for b in received_data:
            if read_in_progress:
                msg += b.to_bytes(1, 'little')
                if index == 2:
                    msg_len = b
                if index == 3:
                    msg_len = msg_len + (b << 8)
                if index == msg_len:
                    read_in_progress = False
                    if msg[5] == 6:
                        focal_length = focal_length_conv(msg[17] + (msg[18] << 8))
                        focus = msg[8] + (msg[9] << 8)
                        if msg[6] == 0x02:
                            focus_limit = "NO"
                        elif msg[6] == 0x0A:
                            focus_limit = "NEAR"
                        elif msg[6] == 0x12:
                            focus_limit = "FAR"
                        else:
                            focus_limit = "UNKNOWN"
                    if msg[5] == 5:
                        aperture_raw = msg[6] + (msg[7] << 8)
                    print("Focus limit:", focus_limit, "Focus:", focus, "Focal length:", focal_length, "Aperture:", aperture_raw)
                
            else:
                if b == 0xf0:
                    index = 1
                    read_in_progress = True
                    msg = b''
                    msg += b.to_bytes(1, 'little')
            index += 1
        
def tx(data):
    body_cs.value(1)
    uart_bodytx.write(data)
    while not uart_bodytx.txdone():
        pass
    body_cs.value(0)
    time.sleep_us(800)

def send_msg(type, payload):
    global counter
    start = b'\xf0'
    end = b'\x55'
    length = len(payload)+6+3
    checksum = 0
    normal_msg = b'\x01'
    msg = b''
    
    msg = msg+length.to_bytes(2, 'little')+normal_msg+counter.to_bytes(1, 'little')+type+payload
    
    for b in msg:
        checksum += b
        
    msg += checksum.to_bytes(2, 'little')
    
    tx(start+msg+end)
    
def send_aperture(ap):
    global ap_odd
    
    aper = int(aperture(ap))
    
    payload = b'\x3f\x75\x00'+aper.to_bytes(2,'little')+aper.to_bytes(2,'little')+b'\x1c\x00\x00\x02'
    if ap_odd:
        ap_odd = False
        payload += b'\x00\x00'
    else:
        ap_odd = True
        payload += b'\x01\x01'
    payload += b'\x02\x00\x02\x01\x00\x00\x00\x2f\x15\x17'
    send_msg(b'\x03', payload)

def send_af():
    global ap, ap_closing, af_in_progress, focus_mode
    
    vd_pulse()
    time.sleep_ms(10)
    send_aperture(ap)
    #jump out and move in
    #send_msg(b'\x04', b'\x00\x00\x19\x83\x00\x00\x66\x00\x00\x80\x41\x00\x00\x2f\x09\x0a\x1f\x06\x02\x83\x88\xd5\x4b\xa4\x3f\x7f\x53\x03\x00\x00')
    #jump in and move out
    #send_msg(b'\x04', b'\x00\x00\x19\x83\x00\x00\x66\x00\x00\x80\x41\x00\x00\x2f\x09\x0a\x1f\x02\x02\x83\x88\xd5\x4b\xa4\x3f\x7f\x53\x03\x00\x00')
    send_msg(b'\x04', b'\x00\x00\x19\x83\x00\x00\x66\x00\x00\x00\x41\x00\x00\x2f\x09\x0a\x1f\x02\x02\x83\x84\xd5\x54\xf2\x3e\x1f\x53\x03\x00\x00')
    af_in_progress = True
    time.sleep_ms(6)
    vd_pulse()
    time.sleep_ms(10)
    send_aperture(ap)
    send_focus(focus_mode, af_in_progress)
    time.sleep_ms(6)
    
def move_focus(position):
    global ap, ap_closing, af_in_progress, focus_mode, focus_pos, focus_min, focus_max
    
    if position < focus_min:
        position = focus_min
    if position > focus_max:
        position = focus_max
    
    vd_pulse()
    time.sleep_ms(10)
    send_aperture(ap)
    send_msg(b'\x04', b'\x00\x00\x19\x83\x00\x00\x66\x00\x00\x00\x41\x00\x00\x2f\x00\x00\x1d'+position.to_bytes(2, 'little')+b'\x00\x40')
    af_in_progress = False
    time.sleep_ms(6)
    vd_pulse()
    time.sleep_ms(10)
    send_aperture(ap)
    send_focus(focus_mode, af_in_progress)
    time.sleep_ms(6)
    
def send_focus(mode, af_in_progress):
    payload = b''
    mode_sw = b'\x81'
    if mode == "AF":
        mode_sw = b'\x83'
    
    if af_in_progress:
        payload = b'\x00\x00\x19'+mode_sw+b'\x00\x00\x66\x00\x00\x80\x41\x00\x00'
    else:
        payload = b'\x00\x00\x19'+mode_sw+b'\x00\x00\x66\x00\x00\x00\x01\x00\x00'
        
    send_msg(b'\x04', payload)

def shutdown():
    vd_pulse()
    time.sleep_ms(10)
    print_rx() #00 00 198300 00 f600 00 00 09 00 00 1d 0000 002cc80255
    tx(b'\xf0\x1b\x00\x01\xc4\x04\x00\x00\x19\x83\x00\x00\xf6\x00\x00\x00\x09\x00\x00\x1d\x00\x00\x00\x2c\xc8\x02\x55')
    tx(b'\xf0\x16\x00\x01\x1c\x04\x00\x00\x19\x81\x00\x00\x66\x00\x00\x00\x01\x00\x00\x38\x01\x55') #Focus??
    time.sleep_ms(6)

def normal_loop():
    global ap, ap_closing, af_in_progress, focus_mode, focus_pos, change_focus
    
    if change_focus:
        move_focus(focus_pos)
        change_focus = False
    else:    
        vd_pulse()
        time.sleep_ms(10)
        send_aperture(ap)
        send_focus(focus_mode, af_in_progress)
        time.sleep_ms(6)
        print_rx()
        vd_pulse()
        time.sleep_ms(10)
        send_aperture(ap)
        send_focus(focus_mode, af_in_progress)
        time.sleep_ms(6)
        print_rx()

Comms_Init()
lens_cs.irq(handler=rx_end, trigger=machine.Pin.IRQ_FALLING)

vd_pulse()
body_cs.value(1)
time.sleep_ms(4)
uart_bodytx = machine.UART(0, baudrate=750000, tx=machine.Pin(16), rx=machine.Pin(17))  # UART1, GP0 (TX) and GP1 (RX)
body_cs.value(0)
time.sleep_ms(15)
vd_pulse()
time.sleep_ms(20)
vd_pulse()

uart_bodytx.read()

tx(b'\xf0\x0b\x00\x02\x00\x0b\x60\x00\x78\x00\x55')
print_rx()
tx(b'\xf0\x0d\x00\x02\x00\x09\x00\x00\x00\x00\x18\x00\x55')
print_rx()
tx(b'\xf0\x0a\x00\x02\x00\x0d\x01\x1a\x00\x55')
print_rx()
tx(b'\xf0\x0a\x00\x02\x00\x10\x1f\x3b\x00\x55') #Self calibration??
print_rx()
time.sleep_ms(10)
for i in range(0,22):
    vd_pulse()
    time.sleep_ms(20)
time.sleep_ms(6)
print_rx()
tx(b'\xf0\x19\x00\x02\x00\x0a\xff\xff\x03\x00\x00\x00\x00\x00\x3f\x00\x00\x00\x00\x00\x00\x00\x65\x02\x55')
print_rx()
time.sleep_ms(10)


while True:
    normal_loop()
    Comms_Read()