from microbit import *
from machine import freq, reset, time_pulse_us
from time import ticks_us, sleep_ms
from utime import sleep_us

# An OSErro 28 out of memeory is caused when write_configuration is called too many time. Only reloading .hex solves prolem.

id = ""
TX_pin = ''
RX_pin = ''
IP_1 = '192.168.10.1'
IP_2 = '192.168.10'
tello_is_connected = 'fail'
is_connected = False

def wait4ok():
  sleep(200)
  while True:
    if uart.any():
      msg = str(uart.read())
    else:
      msg='none'
    if msg.find("OK")==-1:
        break
  return()
  
def discover(tx,rx):
  display.show("?")
  global TX_pin 
  global RX_pin   
  timeout = 20
  TX_pin = tx
  RX_pin = rx
  TX = locals()[TX_pin]
  RX = locals()[RX_pin] 
  uart.init(baudrate=115200,tx=TX,rx=RX)
  uart.write('AT+RST')
  sleep(2000)
  wait4ok()
  uart.write('AT+CWMODE_DEF=3\r\n')
  wait4ok()
  uart.write('AT+CWLAPOPT=1,2\r\n')
  count = 0
  while count < timeout:
    uart.write('AT+CWLAP\r\n')
    while not(uart.any()):
      sleep(10)
    uart.read()     
    sleep(1000)
    msg = str(uart.read())
    start=msg.find('TELLO')
    if start != -1:
      end = msg.find('"',start)
      ssid = msg[start:end]
      uart.init(baudrate=115200)
      return(ssid)
    else:
      count += 1
  uart.init(baudrate=115200)
  display.show(Image.NO) 
  return('fail')

def attach(ssid,pswd=""):
  global TX_pin 
  global RX_pin   
  global tello_is_connected
  TX = locals()[TX_pin]
  RX = locals()[RX_pin] 
  display.show(id)
  timeout=40
  uart.init(baudrate=115200,tx=TX,rx=RX)
  uart.write('AT+CWJAP_DEF="{}","{}"\r\n'.format(ssid,pswd))
  not_connected = True
  timer = 0
  for n in range(8):
    display.clear()
    sleep(500)
    display.show(id)
    sleep(500)
  while not_connected and timer < timeout:
    timer += 1
    sleep(500)
    display.show(id)
    sleep(500)
    display.clear()
    if(uart.any()):
      sleep(100)
      msg = str(uart.read())
      if msg.find("WIFI GOT IP") !=-1:
        not_connected = False
        display.show(Image.YES) 
        tello_is_connected = 'connected'  
  if timer >= timeout:
    display.show(Image.NO) 
    tello_is_connected ='fail'
  uart.init(baudrate=115200)
  return(tello_is_connected)
  
def connect(tx,rx,id):
  global TX_pin 
  global RX_pin  
  timeout = 40
  timeout2 = 2000
  TX_pin = tx
  RX_pin = rx
  TX = locals()[TX_pin]
  RX = locals()[RX_pin] 
  display.show(id)
  uart.init(baudrate=115200,tx=TX,rx=RX)
  uart.write('AT+RST')
  sleep(1000)
  wait4ok()
  ap_not_connected = True
  timer = 0
  while ap_not_connected and timer < timeout: 
    display.clear()
    timer += 1
    uart.write('AT+CWJAP?\r\n')
    uart.read()
    sleep(400)
    display.show(id)
    timer2 = 0
    while not(uart.any()) and timer2 <timeout2:
      timer2 +=1
      sleep(10)
    if timer2 >= timeout2:
      display.show(Image.NO)  
      uart.init(baudrate=115200)
      return('fail') 
    msg = str(uart.read())

    if msg.find("TELLO")!=-1:
      ap_not_connected = False
    else:
      ap_not_connected = True
      sleep(100) 
  if timer >= timeout:
    uart.init(baudrate=115200)
    display.show(Image.NO) 
    return('fail') 
  uart.write('AT+CIFSR\r\n')
  sleep(200)# connect problem in here. too short wait?
  while not(uart.any()):
    pass
  msg = str(uart.read())  
  if msg.find(IP_2)!=-1:
    ip_status = True
  else:
    ip_status = False
  if not ap_not_connected and ip_status:
    global tello_is_connected  
    tello_is_connected = 'connected' 
  else:
    global tello_is_connected  
    tello_is_connected = 'fail'
    display.show(Image.NO)  
  uart.init(baudrate=115200)
  return(tello_is_connected) 
  
def command(id):
  display.show(id)
  msg = write('command',10)
  if msg == 'ok':
    display.show(Image.YES)
  elif msg =='unactive':
    display.show(Image.NO)
  return(msg)

def write(cmd,timeout):
  global TX_pin 
  global RX_pin   
  global tello_is_connected
  if tello_is_connected.find('connected')!=-1:
    TX = locals()[TX_pin]
    RX = locals()[RX_pin] 
    uart.init(baudrate=115200,tx=TX,rx=RX)
    uart.write('AT+CIPMUX=1\r\n')
    sleep(200)
    while not(uart.any()):
      pass
    msg = str(uart.read())
    msg = 'AT+CIPSTART=0,"UDP","{}",8889,8889,0\r\n'.format(IP_1)
    uart.write(msg)
    sleep(100) 
    while not(uart.any()):
      pass
    msg = str(uart.read())
    msg = 'AT+CIPSEND=0,{},"{}",8889\r\n'.format(str(len(cmd)),IP_1)
    uart.write(msg)
    sleep(100)
    wait4ok()
    udp = bytes(cmd, 'utf-8')
    uart.write(udp)
    timer = 0
    msg_not_received = True
    while msg_not_received and timer < timeout:
      sleep(100)
      timer += 1
      if uart.any():
        sleep(10)
        msg = str(uart.read()) 
        if msg.find("n+IPD,0")!=-1:
          msg_not_received = False
    if timer >= timeout:
      return_msg = 'timeout'
    else:
      start = msg.find("n+IPD,0")
      tmp = msg[start+10:]
      return_msg = tmp.replace("\\r\\n'","")
    uart.write('AT+CIPCLOSE\r\n')
    sleep(100)  
    wait4ok()
    uart.init(baudrate=115200)
    sleep(200)
    return(return_msg.strip("'"))  
  else:
    return('not connected')

def control(cmd):
  msg = write(cmd,300)
  if msg.find("ok")!=-1:
    return ('ok')
  else:
    return('fail')
    
def read(cmd):
  msg = write(cmd,100)
  parsed_msg = msg.replace("\\r\\n'","")
  return (parsed_msg)

def read_configuration():
  try:
    cfg=open('tello.cfg','r')
    msg = cfg.read()
    sleep(100)
    d1 = msg.find('@')
    d2 = msg.find('$')
    d3 = msg.find('%')
    ssid = msg[:d1]
    tx = msg[d1+1:d2]
    rx = msg[d2+1:d3]
    id = msg[d3+1:]
    display.show(id)
    return ssid, tx,rx, id 
  except:
    return('fail')

def delete_configuration():
  try:
    #import os
    #sleep(200)
    #os.remove('tello.cfg')
    write_configuration("?","?","?","?")
    return ("ok")
  except:
    return('fail')

def ranger(pin=pin0,time = True):
  timeout_us=30000      
  pin.write_digital(0)
  sleep_us(2)
  pin.write_digital(1)
  sleep_us(10)
  pin.write_digital(0)
  pin.read_digital()
  duration = time_pulse_us(pin, 1, timeout_us)/1e6 # t_echo in seconds  
  distance = 343 * duration/2 * 100
  if time:
    return duration
  else:
    return distance

def time_pulses(pin,pulses):
    try:
        pin.read_digital()
        # wait for one trigger pulse
        while not pin.read_digital():
            pass
        while pin.read_digital():
            pass
        while not pin.read_digital():
            pass
        # begin timing pulses
        t0=ticks_us()
        for n in range(pulses-1):
            while (not pin.read_digital()):
                pass
            while pin.read_digital():
                pass
        tf=ticks_us()
        pulses_time = (tf-t0)/1000000
        return(str(pulses_time))
    except:
        pass

def time_H_to_L(pin):
        pin.read_digital()
        while (pin.read_digital()):
            pass
        t0=ticks_us()
        while not (pin.read_digital()):
            pass
        tf=ticks_us()
        pulse_time = (tf-t0)/1000000
        return(str(pulse_time))

def time_L_to_H(pin):
        pin.read_digital()
        while not (pin0.read_digital()):
            pass
        t0=ticks_us()
        while (pin.read_digital()):
            pass
        tf=ticks_us()
        pulse_time = (tf-t0)/1000000
        return(str(pulse_time))
        

class BME280():
  def __init__(self):
    self.ready = False
    self.IDRegister  = 0xD0         
    self.CTRL_HUM    = 0xF2                 
    self.CTRL_MEAS   = 0xF4          
    self.CONFIG      = 0xF5       
    self.t_fine      = 0
    self.dig_T1      = 0
    self.dig_T2      = 0
    self.dig_T3      = 0
    self.dig_P1      = 0    
    self.dig_P2      = 0    
    self.dig_P3      = 0   
    self.dig_P4      = 0   
    self.dig_P5      = 0  
    self.dig_P6      = 0
    self.dig_P7      = 0  
    self.dig_P8      = 0
    self.dig_P9      = 0
    self.dig_H1      = 0                         
    self.dig_H2      = 0
    self.dig_H3      = 0
    self.dig_H4      = 0
    self.dig_H5      = 0
    self.dig_H6      = 0
    self.TRAW      = 0
    self.PRAW      = 0
    self.HRAW      = 0
    self.ConfigurationData           = bytearray(6)
    self.CalData00_25                = bytearray(25)
    self.CalData00_25BaseAddress     = bytearray(1)
    self.CalData00_25BaseAddress[0]  = 0x88
    self.CalData26_41                = bytearray(7)
    self.CalData26_41BaseAddress     = bytearray(1)
    self.CalData26_41BaseAddress[0]  = 0xE1
    self.RawSensorData               = bytearray(8)
    self.RawSensorDataBaseAddress    = bytearray(1)
    self.RawSensorDataBaseAddress[0] = 0xF7
    
  def init(self):
    try:
      IDAddress     = bytearray(1)
      IDAddress[0]  = self.IDRegister
      i2c.write(0x76, IDAddress, repeat = False)
      id = i2c.read(0x76, 1, repeat = False)
      i2c.write(0x76, self.CalData00_25BaseAddress, repeat = False)    
      self.CalData00_25 = i2c.read(0x76, 25, repeat = False)
      i2c.write(0x76, self.CalData26_41BaseAddress, repeat = False)    
      self.CalData26_41 = i2c.read(0x76, 7, repeat = False)
      self.dig_T1 = self.BuildU16(self.CalData00_25[1], self.CalData00_25[0])     
      self.dig_T2 = self.BuildS16(self.CalData00_25[3], self.CalData00_25[2])     
      self.dig_T3 = self.BuildS16(self.CalData00_25[5], self.CalData00_25[4])     
      self.dig_P1 = self.BuildU16(self.CalData00_25[7], self.CalData00_25[6])     
      self.dig_P2 = self.BuildS16(self.CalData00_25[9], self.CalData00_25[8])     
      self.dig_P3 = self.BuildS16(self.CalData00_25[11], self.CalData00_25[10])   
      self.dig_P4 = self.BuildS16(self.CalData00_25[13], self.CalData00_25[12])   
      self.dig_P5 = self.BuildS16(self.CalData00_25[15], self.CalData00_25[14])   
      self.dig_P6 = self.BuildS16(self.CalData00_25[17], self.CalData00_25[16])   
      self.dig_P7 = self.BuildS16(self.CalData00_25[19], self.CalData00_25[18])   
      self.dig_P8 = self.BuildS16(self.CalData00_25[21], self.CalData00_25[20]) 
      self.dig_P9 = self.BuildS16(self.CalData00_25[23], self.CalData00_25[22])
      self.dig_H1 = self.CalData00_25[24]                           
      self.dig_H2 = self.BuildS16(self.CalData26_41[1],self.CalData26_41[0])
      self.dig_H3 = self.CalData26_41[2]
      self.dig_H4 = (self.BuildS8(self.CalData26_41[3]) << 4) | (self.CalData26_41[4] & 0x0F)
      self.dig_H5 = (self.BuildS8(self.CalData26_41[5]) << 4) | ((self.CalData26_41[4] >> 4) & 0x0F)
      self.dig_H6 = self.BuildS8(self.CalData26_41[6]) 
      self.ConfigurationData[0] = self.CTRL_HUM       
      self.ConfigurationData[1] = 0b00000101      
      self.ConfigurationData[2] = self.CTRL_MEAS       
      self.ConfigurationData[3] = 0b10110111 
      self.ConfigurationData[4] = self.CONFIG     
      self.ConfigurationData[5] = 0b01000000       
      i2c.write(0x76,self.ConfigurationData, repeat=False)
      sleep(100)
      self.ready = True
    except:
      pass

  def BuildS16(self,msb, lsb):
      sval = ((msb << 8) | lsb)
      if sval > 32767:  
          sval -= 65536
      return sval
         
  def BuildU16(self,msb, lsb):
      return ((msb << 8) |lsb)


  def BuildS8(self,b):
      if b > 127:
          return (b-256)
      else:
          return b
          
  def CalculateTemperature(self):   
      self.t_fine
      Traw = float(self.TRAW)
      v1 = (Traw/ 16384.0 - float(self.dig_T1) / 1024.0) * float(self.dig_T2)
      v2 = ((Traw / 131072.0 - float(self.dig_T1) / 8192.0) * (
      Traw / 131072.0 - float(self.dig_T1) / 8192.0)) * float(self.dig_T3)
      self.t_fine = int(v1 + v2)
      T = (v1 + v2) / 5120.0
      return T

  def CalculatePressure(self):
      Praw = float(self.PRAW)
      v1 = float(self.t_fine) / 2.0 - 64000.0
      v2 = v1 * v1 * float(self.dig_P6) / 32768.0
      v2 = v2 + v1 * float(self.dig_P5) * 2.0
      v2 = v2 / 4.0 + float(self.dig_P4) * 65536.0
      v1 = (float(self.dig_P3) * v1 * v1 / 524288.0 + float(self.dig_P2) * v1) / 524288.0
      v1 = (1.0 + v1 / 32768.0) * float(self.dig_P1)
      if v1 == 0:
          return 0
      p  = 1048576.0 - Praw
      p  = ((p - v2 / 4096.0) * 6250.0) / v1
      v1 = float(self.dig_P9) * p * p / 2147483648.0
      v2 = p * float(self.dig_P8) / 32768.0
      p  = p + (v1 + v2 + float(self.dig_P7)) / 16.0
      return p
      
  def CalculateHumidity(self):
      self.t_fine
      Hraw = float(self.HRAW)
      h = float(self.t_fine) - 76800.0
      h = (Hraw - (float(self.dig_H4) * 64.0 + float(self.dig_H5) / 16384.0 * h)) * (
          float(self.dig_H2) / 65536.0 * (1.0 + float(self.dig_H6) / 67108864.0 * h * (
          1.0 + float(self.dig_H3) / 67108864.0 * h)))
      h = h * (1.0 - float(self.dig_H1) * h / 524288.0)
      if h > 100:
          h = 100
      elif h < 0:
          h = 0
      return h

  def read(self):
    if self.ready:
      try:
        i2c.write(0x76, self.RawSensorDataBaseAddress, repeat = False)
        sleep(100)
        self.RawSensorData = i2c.read(0x76, 8, repeat = False)
        sleep(100)
        self.TRAW = ((self.RawSensorData[3] << 16) | (self.RawSensorData[4] << 8) | self.RawSensorData[5]) >> 4
        self.PRAW = ((self.RawSensorData[0] << 16) | (self.RawSensorData[1] << 8) | self.RawSensorData[2]) >> 4
        self.HRAW = (self.RawSensorData[6] << 8)   | self.RawSensorData[7]
        return self.CalculateTemperature(),self.CalculatePressure(),self.CalculateHumidity()
      except:
        pass
    else:
      self.init()
      if self.ready:
        return (self.read())
      else:
       return None

        
class DHT20():
  def __init__(self):
    self.ready = False
    
  def init(self):
    try:
      i2c.init()
      i2c.write(0x38, bytes([0xa8,0x00,0x00]))
      sleep_ms(100)
      i2c.write(0x38, bytes([0xbe,0x08,0x00]))
      sleep(100)
      raw = i2c.read(0x38, 7, True)
      self.ready = True
    except:
      pass    
    
  def read(self):
    if self.ready:
      try:
        i2c.write(0x38, bytes([0xac,0x33,0x00]))
        sleep(100)
        raw = i2c.read(0x38, 7, True)
        sleep(100)
        data = []
        for i in raw[:]:
          data.append(i)
        temperature = 0
        temperature = (temperature | data[3]) << 8
        temperature = (temperature | data[4]) << 8
        temperature = temperature | data[5]
        temperature = temperature & 0xfffff
        temperature = (temperature * 200 * 10 / 1024 / 1024 - 500)/10
        humidity = 0
        humidity = (humidity | data[1]) << 8
        humidity = (humidity | data[2]) << 8
        humidity = humidity | data[3]
        humidity = humidity >> 4
        humidity = (humidity * 100 * 10 / 1024 / 1024)/10    
        return temperature, humidity
      except:
        pass
    else:
      self.init()
      if self.ready:
        return (self.read())
      else:
       return None


class SGP30:
  def __init__(self):
    self.ready = False
    
  def init(self):
    try:
      i2c.write(0x58,bytes([0x36, 0x82]))
      self.iaq_init()
      self.ready = True
    except:
      pass  
    
  def TVOC(self):
    return self.iaq_measure()[1]
    
  def eCO2(self):
    return self.iaq_measure()[0]   

  def iaq_init(self):
    self.run(['iaq_init',[0x20,0x03],0,10])

  def iaq_measure(self):
    return self.run(['iaq_measure',[0x20,0x08],2,50])
     
  def baseline_TVOC(self):
    return self.get_iaq_baseline()[1]
        
  def baseline_eCO2(self):
    return self.get_iaq_baseline()[0]
    
  def get_iaq_baseline(self):
    return self.run(['iaq_get_baseline',[0x20,0x15],2,10])
    
  def set_iaq_baseline(self,eCO2,TVOC):
    if eCO2==0 and TVOC==0:raise RuntimeError('Invalid baseline')
    b=[]
    for i in [TVOC,eCO2]:
      a=[i>>8,i&0xFF]
      a.append(self.g_crc(a))
      b+=a
    self.run(['iaq_set_baseline',[0x20,0x1e]+b,0,10])
    
  def set_iaq_humidity(self,PM3):
    b=[]
    for i in [int(PM3*256)]:
      a=[i>>8,i&0xFF]
      a.append(self.g_crc(a))
      b+=a
    self.run(['iaq_set_humidity',[0x20,0x61]+b,0,10])

    
  def run(self,profile):
    n,cmd,s,d=profile
    return self.get(cmd,d,s)

  
  def get(self,cmd,d,rs):
    i2c.write(0x58,bytearray(cmd))
    sleep(d)
    if not rs:return None
    cr=i2c.read(0x58,rs*3)
    o=[]
    for i in range(rs):
      w=[cr[3*i],cr[3*i+1]]
      c=cr[3*i+2]
      if self.g_crc(w)!=c:raise RuntimeError('CRC Error')
      o.append(w[0]<<8|w[1])
    return o
    
  def g_crc(self,data):
    c=0xFF
    for byte in data:
      c^=byte
      for _ in range(8):
        if c&0x80:c=(c<<1)^0x31
        else:c<<=1
    return c&0xFF

  
  def read(self):
    if self.ready:
      try:
        return self.eCO2(), self.TVOC()
      except:
        pass
    else:
      self.init()
      if self.ready:
        return (self.read())
      else:
       return None

 
def ismb():
  return(True)
  
def get_version():
    print ("3.0.7 BETA")
 
ti = Image("03500:""05595:""55555:""05550:""00500")
dht20=DHT20()
sgp30=SGP30()
bme280=BME280()
if button_b.is_pressed() and pin_logo.is_touched():
  msg = delete_configuration()
  if msg.find('ok') != -1:
    display.show(Image.SKULL)
  else:
    display.show(Image.SAD)
else: 
  display.show(ti)




import gc

def write_configuration(ssid,tx,rx,ID):
  print(1, gc.mem_free())
  try: 
    print(2, gc.mem_free())
    global id
    id = ID
    print(3, gc.mem_free())
    cfg=open('tello.cfg','w')
    print(4, gc.mem_free())
    a = ssid + "@" + tx + "$" + rx + "%" +  id
    print(5, gc.mem_free())
    cfg.write(a)
    print(6, gc.mem_free())
    cfg.close()
    return("ok")
  except Exception as e:
    print(e)
    return('fail')


i = 0
while True:
    i += 1
    print(i)
    result = write_configuration("TELLO-123456","pin2","pin1","1")
    if result != "ok":
        break