import utime
import machine
from machine import UART

rxPin=16
txPin=17

tx = machine.Pin(txPin, machine.Pin.OUT)
rx = machine.Pin(rxPin, machine.Pin.IN)
tx.value(0)
utime.sleep_ms(500)
tx.value(1)

uart=UART(1,2400)
print("START LISTENING THE HANDSHAKE OF THE SENSOR")
utime.sleep_ms(100)
#Init communication at 2.400 bps
uart.init(2400,bits=8,parity=None,stop=1,rx=rxPin,tx=txPin)

#I check for 2 ACK from the sensor
#before send my ACK
i=0
while True:
  data = uart.read(uart.any())
  a=len(data)
  if a > 0:
    print(data)
  utime.sleep_ms(5)
  if data.find(b'\x04') >= 0:
    i=i+1
    if i==2:
      print("OK, SEND ACK TO SENSOR")
      utime.sleep_ms(20)
      uart.write(b'\x04')
      break

#Ok, if I am here, the sensor has sent his presentation data
utime.sleep_ms(100)
print("START DATA COMMUNICATION")
#change the speed of communication channel
uart.init(115200,bits=8,parity=None,stop=1,rx=rxPin,tx=txPin)

utime.sleep_ms(200)

#send SELECT command to swith the sensor in "MODE 8" (color/distance value)
uart.write(b'\x43\x08\xB4\x02')
utime.sleep_ms(50)

#send a second SELECT command to swith the sensor in "MODE 8" (color/distance value)
uart.write(b'\x43\x08\xB4\x02')
utime.sleep_ms(50)

lastnack= utime.ticks_ms()
selectmode=lastnack
currentime=lastnack
iChangeMode=0
while True:
  currentime=utime.ticks_ms()
  #Send a change mode after 1,5 second
  #(in case of the first two SELECTconmmand dont' work)
  #
  if (currentime-selectmode) >= 1500:
    if (iChangeMode == 0):
      print("CHANGE MODE REQUEST")
      iChangeMode = 1
      uart.write(b'\x43\x08\xB4')
  #send a NACK to sensor every 100 ms
  if (currentime-lastnack) >= 80:
    lastnack=currentime
    uart.write(b'\x02')
  utime.sleep_ms(10)
  data = uart.read(uart.any())
  a=len(data)
  if a > 0:
    print(data)
    #pos = data.find(b'\xD0')
    if data.find(b'\xD0') >= 0:
      print("THERE IS A VALUE")