#include "mbed.h"
#include "BLE.h"
#include "UARTService.h"
#include "ble/DiscoveredCharacteristic.h"
#include "ble/DiscoveredService.h"
#include "UARTService.h"

#define SOFT_DEVICE_FATHER_HANDLE 3

#define BOARDS_COUNT 3

const Gap::Address_t mac_board_0 = {0xb8, 0xac, 0x4e, 0x8d, 0x8b, 0xeb}; 
const Gap::Address_t mac_board_1 = {0x9c, 0x43, 0x62, 0x30, 0xaf, 0xd2}; 
const Gap::Address_t mac_board_2 = {0x5f, 0x1a, 0x9e, 0x6a, 0x63, 0xdd}; 


// tiny ble board
#define LED_GREEN   p21
#define LED_RED     p22
#define LED_BLUE    p23
#define BUTTON_PIN  p17
#define BATTERY_PIN p1

#define MPU6050_SDA p12
#define MPU6050_SCL p13

#define UART_TX     p9
#define UART_RX     p11
#define UART_CTS    p8
#define UART_RTS    p10

DigitalOut led(LED_RED);
DigitalOut alivenessLED(LED_GREEN);
InterruptIn button(BUTTON_PIN);
AnalogIn battery(BATTERY_PIN);
Serial pc(UART_TX, UART_RX);

bool mac_equals(const Gap::Address_t mac_1, const Gap::Address_t mac_2)
{
    #if 0
        if (mac_1[0] != mac_2[0])
        {
            return false;
        }
        if (mac_1[1] != mac_2[1])
        {
            return false;
        }
        if (mac_1[2] != mac_2[2])
        {
            return false;
        }
        if (mac_1[3] != mac_2[3])
        {
            return false;
        }
        if (mac_1[4] != mac_2[4])
        {
            return false;
        }
        if (mac_1[5] != mac_2[5])
        {
            return false;
        }
    #else
        for (int i = 0; i < 6; i++)
        {
            if (mac_1[i] != mac_2[i])
            {
                //pc.printf("0x%02x != 0x%02x at %d\r\n", mac_1[i], mac_2[i], i);
                return false;
            }
            else
            {
                //pc.printf("0x%02x == 0x%02x at %d\r\n", mac_1[i], mac_2[i], i);
            }
        }
    #endif
    return true;
}

int get_board_index(const Gap::Address_t mac)
{
    if (mac_equals(mac, mac_board_0))
    {
        return 0;
    }
    if (mac_equals(mac, mac_board_1))
    {
        return 1;
    }
    if (mac_equals(mac, mac_board_2))
    {
        return 2;
    }
    
    return -1;
}

void periodicCallback(void) 
{
    alivenessLED = !alivenessLED; /* do blinky on alivenessLED while we're waiting for BLE events */
}


// Mixed role ****************************************************
BLE ble;
Gap::Address_t my_mac;
int my_board_index = -1;

// Device role ****************************************************
UARTService * uartServicePtr = NULL;
const static char     DEVICE_NAME[]        = "ChangeMe!!"; // change this
static const uint16_t uuid16_list[] = {UARTServiceShortUUID};
volatile int central_handle = -1;


// Central role ****************************************************
Gap::Handle_t connectionHandle = 0xFFFF;
DiscoveredCharacteristic uartTXCharacteristic;
DiscoveredCharacteristic uartRXCharacteristic;
bool foundUartRXCharacteristic = false;
volatile int device_handle = -1;


// Device role ****************************************************
void onReceivedDataFromCentralCallback(const GattWriteCallbackParams *params) 
{
    if (uartServicePtr != NULL)
    {
        if ((params->handle == uartServicePtr->getTXCharacteristicHandle()) && (params->len >= 1)) 
        {
            if (params->data[0] != '0')
            {
                led = 1;
            }
            else
            {
                led = 0;
            }
            
            for(int i = 0; i < params->len; i++) 
            {
                pc.printf("%c", params->data[i]);
            }
            
            pc.printf(" (%d, %d)\r\n", params->handle, params->connHandle);
        }
    }
}

// Central role ****************************************************
void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) 
{
    // do connections like a triangle
    int peer_board_index = get_board_index(params->peerAddr);
    
    int next_board_index = my_board_index + 1;
    if (next_board_index >= BOARDS_COUNT)
    {
        next_board_index = 0;
    }
    
    //pc.printf("adv %d, %d, %d\r\n", peer_board_index, my_board_index, next_board_index);
    
    // force order
    if ((central_handle != -1) || (peer_board_index == 0))
    {
        if (peer_board_index == next_board_index)
        {
            //pc.printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u\r\n",
            //       params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
            //       params->rssi, params->isScanResponse, params->type);
        
            ble.gap().connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
        }
    }
}

void serviceDiscoveryCallback(const DiscoveredService *service) 
{
    if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) 
    {
        pc.printf("S UUID-%x attrs[%u %u]\r\n", service->getUUID().getShortUUID(), service->getStartHandle(), service->getEndHandle());
    } 
    else
    {
        //pc.printf("S UUID-");
        const uint8_t *longUUIDBytes = service->getUUID().getBaseUUID();
        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) 
        {
            pc.printf("%02x", longUUIDBytes[i]);
        }
        pc.printf(" attrs[%u %u]\r\n", service->getStartHandle(), service->getEndHandle());
    }
}

void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP) 
{
    //pc.printf("  C UUID-%x valueAttr[%u] props[%x]\r\n", characteristicP->getUUID().getShortUUID(), characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());
    if (characteristicP->getUUID().getShortUUID() == UARTServiceTXCharacteristicShortUUID) 
    { 
        pc.printf("fit TX 0x%04x\r\n", UARTServiceTXCharacteristicShortUUID);
        /* !ALERT! Alter this filter to suit your device. */
        uartTXCharacteristic        = *characteristicP;
    }
    else if (characteristicP->getUUID().getShortUUID() == UARTServiceRXCharacteristicShortUUID) 
    { 
        pc.printf("fit RX 0x%04x\r\n", UARTServiceRXCharacteristicShortUUID);
        /* !ALERT! Alter this filter to suit your device. */
        uartRXCharacteristic        = *characteristicP;
        foundUartRXCharacteristic = true;
    }
}

void discoveryTerminationCallback(Gap::Handle_t connectionHandle) 
{
    pc.printf("terminated SD for handle %u\r\n", connectionHandle);
}

void onReceivedDataFromDeviceCallback(const GattHVXCallbackParams *params) 
{
    //pc.printf("received HVX callback for handle %u; type %s\r\r\n", params->handle, (params->type == BLE_HVX_NOTIFICATION) ? "notification" : "indication");
    if (params->type == BLE_HVX_NOTIFICATION)
    {
        if ((params->handle == uartRXCharacteristic.getValueHandle()) && (params->len > 0))
        {
            for (int i = 0; i < params->len; i++) 
            {
                pc.printf("%c", params->data[i]);
            }
            
            pc.printf(" (%d, %d)\r\n", params->handle, params->connHandle);
        }
    }
    else
    {
        pc.printf("%d\r\n", params->type);
    }
}

// Mixed role ****************************************************
void connectionCallback(const Gap::ConnectionCallbackParams_t *params) 
{
    if (params->role == Gap::CENTRAL) 
    {
        if (central_handle == -1)
        {
            ble.stopAdvertising(); // stop advertising during discovery, incoming connection breaks discovery
        }
        
        device_handle = params->handle;
        pc.printf("connected as central (handle = %d)\r\n\r", params->handle);
        connectionHandle = params->handle;
        ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
        int ret = ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, UARTServiceShortUUID/*, 0xa001*/);
        
        if (ret != BLE_ERROR_NONE)
        {
            pc.printf("launchServiceDiscovery failed error = %d\r\n\r", ret);
        }
    }
    else
    {
        central_handle = params->handle;
        pc.printf("connected as device (handle = %d)\r\n\r", params->handle);
        
        //pc.printf("Conn. params => min=%d, max=%d, slave=%d, supervision=%d\r\n", params->connectionParams->minConnectionInterval, params->connectionParams->maxConnectionInterval, params->connectionParams->slaveLatency, params->connectionParams->connectionSupervisionTimeout);
        /*
        Gap::ConnectionParams_t connectionParams;
        connectionParams.minConnectionInterval        = 6;
        connectionParams.maxConnectionInterval        = 12;
        connectionParams.slaveLatency                 = 40;
        connectionParams.connectionSupervisionTimeout = 500;
        
        int ret = ble.updateConnectionParams(params->handle, &connectionParams);
        if (ret != BLE_ERROR_NONE) 
        {
            pc.printf("failed to update connection parameter\r\n");
        }
        */
    }
    pc.printf("own %02x:%02x:%02x:%02x:%02x:%02x (%s), peer %02x:%02x:%02x:%02x:%02x:%02x (%s)\r\n", params->ownAddr[5], params->ownAddr[4], params->ownAddr[3], params->ownAddr[2], params->ownAddr[1], params->ownAddr[0], (params->ownAddrType == Gap::ADDR_TYPE_PUBLIC) ? "public" : "random", params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0], (params->peerAddrType == Gap::ADDR_TYPE_PUBLIC) ? "public" : "random");
}

void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) 
{
    char * ascii_reason = "?";
    switch (reason)
    {
        case Gap::CONNECTION_TIMEOUT:
            ascii_reason = "connection timeout";
            break;    
        case Gap::REMOTE_USER_TERMINATED_CONNECTION:
            ascii_reason = "user terminated connection";
            break;    
        case Gap::REMOTE_DEV_TERMINATION_DUE_TO_LOW_RESOURCES:
            ascii_reason = "low resources";
            break;    
        case Gap::REMOTE_DEV_TERMINATION_DUE_TO_POWER_OFF:
            ascii_reason = "power off";
            break;    
        case Gap::LOCAL_HOST_TERMINATED_CONNECTION:
            ascii_reason = "host terminated connection";
            break;    
        case Gap::CONN_INTERVAL_UNACCEPTABLE:
            ascii_reason = "interval unacceptable";
            break;    
        default:
            ascii_reason = "unknown";
            break;
    }
    
    pc.printf("disconnected (reason = %s, handle = %d)\r\n", ascii_reason, handle);
    
    
    if (handle == SOFT_DEVICE_FATHER_HANDLE)
    {
        central_handle = -1;
        // restart advertising
        ble.startAdvertising(); 
    }
    else
    {
        device_handle = -1;
        // restart scan
        ble.gap().startScan(advertisementCallback);
    }
}



void serialTxCallback() 
{

}
 
int rx_char = -1;
void serialRxCallback() 
{   
    if (rx_char != -1)
    {
        pc.printf("overflow\r\n");
    }
     
     //computer.putc(computer.getc());
     rx_char = pc.getc();
}

/*
void gattServerOnDataSent(unsigned count)
{
 
}
*/


int main(void) 
{
    alivenessLED = 0;
    
    pc.baud(115200);
    //pc.attach(&serialTxCallback, Serial::TxIrq);
    pc.attach(&serialRxCallback, Serial::RxIrq);
    
    // clear terminal output
    for (int k = 0; k < 32; k++)
    {
        pc.printf("\r\n");    
    }
    
    pc.printf("Central and device\r\n");
   
    Ticker ticker;
    ticker.attach(periodicCallback, 1);
    
    
    // Mixed role ****************************************************
    ble.init();
    
    Gap::AddressType_t my_mac_type;
    ble.gap().getAddress(&my_mac_type, my_mac);
    my_board_index = get_board_index(my_mac);
    pc.printf("me %02x:%02x:%02x:%02x:%02x:%02x (%s)\r\n", my_mac[5], my_mac[4], my_mac[3], my_mac[2], my_mac[1], my_mac[0], (my_mac_type == Gap::ADDR_TYPE_PUBLIC) ? "public" : "random");
    

    // try to speed up but looks like if it was ignored
    Gap::ConnectionParams_t fast;
    if (ble.getPreferredConnectionParams(&fast) != BLE_ERROR_NONE) 
    {
        pc.printf("getPreferredConnectionParams failed\r\n");
    }
    else
    {
        fast.minConnectionInterval = 16; // 20 ms
        fast.maxConnectionInterval = 32; // 40 ms
        fast.slaveLatency = 0;
        if (ble.gap().setPreferredConnectionParams(&fast) != BLE_ERROR_NONE) 
        {
            pc.printf("setPreferredConnectionParams failed\r\n");
        }
    }
    ble.gap().onConnection(connectionCallback);
    ble.gap().onDisconnection(disconnectionCallback);
    
    // Device role ****************************************************
    ble.gattServer().onDataWritten(onReceivedDataFromCentralCallback);
    //ble.gattServer().onDataSent(gattServerOnDataSent);

    UARTService uartService(ble);
    uartServicePtr = &uartService;

    // setup advertising
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); // BLE only, no classic BT
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); // add name
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); // UUID's broadcast in advertising packet
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); // advertising type
    ble.setAdvertisingInterval(100); 
    
    // Central role ****************************************************
    ble.gattClient().onHVX(onReceivedDataFromDeviceCallback);
    ble.gap().setScanParams(500, 450);
    
    
    // start advertising and scan
    ble.startAdvertising(); 
    ble.gap().startScan(advertisementCallback);

    while (true) 
    {
          // allow notifications from device
          if (foundUartRXCharacteristic && !ble.gattClient().isServiceDiscoveryActive()) 
          {
            foundUartRXCharacteristic = false; /* need to do the following only once */
 
            uint16_t value = BLE_HVX_NOTIFICATION;
            int ret = ble.gattClient().write(GattClient::GATT_OP_WRITE_REQ,
                                   connectionHandle,
                                   uartRXCharacteristic.getValueHandle() + 1, /* HACK Alert. We're assuming that CCCD descriptor immediately follows the value attribute. */
                                   sizeof(uint16_t),                          /* HACK Alert! size should be made into a BLE_API constant. */
                                   reinterpret_cast<const uint8_t *>(&value));
                                   
            if (ret == BLE_ERROR_NONE) 
            {
                pc.printf("\r\ndevice notifications enabled\r\n");
            }
            else
            {
                switch (ret)
                {
                    case BLE_STACK_BUSY:
                        foundUartRXCharacteristic = true; // retry later
                        break;
                    case BLE_ERROR_NO_MEM:
                        foundUartRXCharacteristic = true; // retry later
                        break;
                    case BLE_ERROR_INVALID_STATE:
                        pc.printf("\r\ndevice notifications enable failed\r\n");
                        break;
                    default:
                        break;
                }
            }
            
            if (!foundUartRXCharacteristic)
            {
                if (central_handle == -1)
                {
                    ble.startAdvertising();        
                }
            }
        }
        
        // while a new char from computer is available
        while (rx_char != -1)
        { 
            uint8_t temp[20];
            int length = 1;
            
            uint8_t command = rx_char;
            rx_char = -1;
        
            // if special char to test a 20 bytes frame
            /*
            if (command == '*')
            {
                pc.printf("20 chars\r\n");
                
                int c = 0;
                for (c = 0; c < 20; c++)
                {
                    temp[c] = 'a' + c;
                }
                length = 20;
            }
            else
            {
                temp[0] = command;
            }
            */
            temp[0] = command;
       
            // to central
            //if (command == '1')     
            {
                if (central_handle != -1)
                {
                    // device to central
                    while (1)
                    {             
                        if (central_handle == -1)
                        {
                            pc.printf("\r\ndisconnected 1 (to central)\r\n");
                            break;
                        }              
                        if (!ble.gap().getState().connected) 
                        {
                            pc.printf("\r\ndisconnected 2 (to central)\r\n");
                            break;
                        }
                        int ret = ble.gattServer().write(uartServicePtr->getRXCharacteristicHandle(), temp, length);
                        
                        if (ret == BLE_ERROR_NONE) 
                        {
                            //pc.printf("\r\nok (to central)\r\n");
                            break;
                        }
                        else if (ret == BLE_STACK_BUSY)
                        {
                            //pc.printf("\r\nbusy (to central)\r\n");
                            //break;
                        }
                        else if (ret == BLE_ERROR_OPERATION_NOT_PERMITTED)
                        {
                            pc.printf("\r\nnot permitted (to central)\r\n");
                            break;
                        }
                        else if (ret == BLE_ERROR_INVALID_STATE)
                        {
                            pc.printf("\r\ninvalid state (to central)\r\n");
                            break;
                        }
                        else
                        {
                            pc.printf("\r\ncode %d (to central)\r\n", ret);
                            break;
                        }
                    }
                }
                else
                {
                    pc.printf("\r\nnot connected with central\r\n");
                }
            }
             
            // to device
            //if (command == '2')   
            {
                if (device_handle != -1)
                {
                    // central to device 
                    while (1)
                    {               
                        if (device_handle == -1)
                        {
                            pc.printf("\r\ndisconnected (to device)\r\n");
                            break;
                        }     
                        int ret = uartTXCharacteristic.write(length, temp);
                        if (ret == BLE_ERROR_NONE) 
                        {
                            //pc.printf("\r\nok (to device)\r\n");
                            break;
                        }
                        else if (ret == BLE_STACK_BUSY) 
                        {
                            //pc.printf("\r\nbusy (to device)\r\n");
                            //break;
                        }
                        else if (ret == BLE_ERROR_OPERATION_NOT_PERMITTED)
                        {
                            pc.printf("\r\nnot permitted (to device)\r\n");
                            break;
                        }
                        else if (ret == BLE_ERROR_INVALID_STATE)
                        {
                            pc.printf("\r\ninvalid state (to device)\r\n");
                            break;
                        }
                        else
                        {
                            pc.printf("\r\ncode %d (to device)\r\n", ret);
                            break;
                        }
                    }
                }
                else
                {
                    pc.printf("\r\nnot connected with device\r\n");
                }
            }
        }
        
        ble.waitForEvent(); // save power
    }
}