import ubluetooth
from machine import Pin, Timer, PWM, ADC
from time import sleep_ms

led = Pin(2, Pin.OUT)
       
class BLE():
    def __init__(self, name):
        self.name = name
        self.ble = ubluetooth.BLE()
        self.ble.active(True)

        self.led = Pin(2, Pin.OUT)
        self.timer1 = Timer(0)
        self.timer2 = Timer(1)
        
        self.disconnected()
        self.ble.irq(self.ble_irq)
        self.register()
        self.advertiser()
        
    def handle_mode_1(self):
        # Function to execute during "mode-1"
        while True:
            buffer = self.ble.gatts_read(self.tx)
            message = buffer.decode('UTF-8').strip()
            if 'exit-mode' in message:
                print("Exiting Mode 1")
                break  # Exit the loop on "exit-mode"
            else:
                print("Theremin function")
                                    
    def handle_mode_2(self):
        # Function to execute during "mode-2"
        while True:
            buffer = self.ble.gatts_read(self.tx)
            message = buffer.decode('UTF-8').strip()
            if message == "exit-mode":
                print("Exiting Mode 2")
                break  # Exit the loop on "exit-mode"
            else:
                print("Avoidance function")
    
    def handle_mode_3(self):
        # Function to execute during "mode-3"
        while True:
            buffer = self.ble.gatts_read(self.tx)
            message = buffer.decode('UTF-8').strip()
            
            if "exit-mode" in message:
                motors.stop()
                print("Exiting Mode 3")
                break  # Exit the loop on "exit-mode"
            else:
                print("Line Follow function")
        
    def connected(self):        
        self.timer1.deinit()
        self.timer2.deinit()

    def disconnected(self):        
        self.timer1.init(period=1000,mode=Timer.PERIODIC, callback=lambda t: self.led(1))
        sleep_ms(200)
        self.timer2.init(period=1000, mode=Timer.PERIODIC, callback=lambda t: self.led(0))   

    def ble_irq(self, event, data):
        
        if event == 1:
            '''Connected'''
            self.connected()
            self.led(0)
            print('Robot connected')
            
        elif event == 2:
            '''Disconnected'''
            self.advertiser()
            self.disconnected()
        
        elif event == 3:
            '''New message received'''            
            buffer = self.ble.gatts_read(self.tx)
            message = buffer.decode('UTF-8').strip()

            if 'mode-1' in message:
                self.handle_mode_1()
            elif 'mode-2' in message:
                self.handle_mode_2()
            elif 'mode-3' in message:
                self.handle_mode_3()
            elif 'exit-mode' in message:
                print("Exiting Mode")
                
            elif 'Connect-' in message:
                print('Type of robot connected: ')
                print(message[8:])
            
            elif 'update' in message:
                print('Update robot sensor values')
                
           
    def register(self):        
        # Nordic UART Service (NUS)
        NUS_UUID = '6e402006-c3b5-d647-e0a9-e50e42daac9d'
        TX_UUID = '6e402003-c3b5-d647-e0a9-e50e42daac9d'
            
        BLE_NUS = ubluetooth.UUID(NUS_UUID)
        BLE_TX = (ubluetooth.UUID(TX_UUID), ubluetooth.FLAG_NOTIFY | ubluetooth.FLAG_WRITE | ubluetooth.FLAG_READ)
            
        BLE_UART = (BLE_NUS, (BLE_TX,))
        SERVICES = (BLE_UART, )
        ((self.tx,), ) = self.ble.gatts_register_services(SERVICES)

    def send(self, data):
        self.ble.gatts_notify(0, self.tx, data + '')

    def advertiser(self):
        name = bytes(self.name, 'UTF-8')
        self.ble.gap_advertise(100, bytes([0x02, 0x01, 0x02]) + bytes([len(name) + 1, 0x09]) + name)

ble = BLE("Robot")