File content as of revision 2:b94ca9b44b80:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2015 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

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

#include "ble_gatt.h"

char HVC_ADDRESS[]= {0xd5,0x32,0x26,0x3b,0x47,0x3d};
bool Reconnect=true;
char HumanDetect=1;
char HandDetect=1;
char FaceDetect=1;
char FaceDirection=1;
char FaceOld=1;
char FaceSex=1;
char FaceLook=1;
char FaceEyeClose=1;
char FaceExpression=1;
const uint8_t WriteCharacteristicAddr[]= {0x35,0x10,0x00,0x02,0xd1,0x3a,0x4f,0x39,0x8a,0xb3,0xbf,0x64,0xd4,0xfb,0xb4,0xb4};
const uint8_t NotifyCharacteristicAddr[]= {0x35,0x10,0x00,0x03,0xd1,0x3a,0x4f,0x39,0x8a,0xb3,0xbf,0x64,0xd4,0xfb,0xb4,0xb4};
const uint8_t DeviceCharacteristicAddr[]= {0x35,0x10,0x00,0x04,0xd1,0x3a,0x4f,0x39,0x8a,0xb3,0xbf,0x64,0xd4,0xfb,0xb4,0xb4};
BLE ble;
Serial pc(USBTX, USBRX);

Gap::Handle_t connectionHandle = 0xFFFF;

DigitalOut alivenessLED(LED1, 1);
bool foundHVCWriteCharacteristic = false;
bool foundHVCNotifyCharacteristic = false;
bool foundHVCDeviceCharacteristic = false;
int state=-1;
bool WaitSend;
DiscoveredCharacteristic HVCWriteCharacteristic;
DiscoveredCharacteristic HVCNotifyCharacteristic;
DiscoveredCharacteristic HVCDeviceCharacteristic;

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

void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{
    if ((params->peerAddr[5] != HVC_ADDRESS[0]) ||
            (params->peerAddr[4] != HVC_ADDRESS[1]) ||
            (params->peerAddr[3] != HVC_ADDRESS[2]) ||
            (params->peerAddr[2] != HVC_ADDRESS[3]) ||
            (params->peerAddr[1] != HVC_ADDRESS[4]) ||
            (params->peerAddr[0] != HVC_ADDRESS[5])) {
        //  printf("Not 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);
        return;
    }
    //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);
    printf("#Look at HVC-C1B 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);  //Connection
}

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)
{
    bool flag;
    if (characteristicP->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        printf("#short:\r\n");
        //dousasuruka wakaranai
        pc.printf("#  C UUID-%x valueAttr[%u] props[%x]\r\n", characteristicP->getUUID().getShortUUID() , characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());
        //  if (characteristicP->getShortUUID() == 2) {
        if (characteristicP->getUUID().getShortUUID() == 2) {
            HVCWriteCharacteristic      = *characteristicP;
            foundHVCWriteCharacteristic = true;
            printf("#HVCWriteCharacteristic Found\r\n");
        }
        //if (characteristicP->getShortUUID() == 3) {
        if (characteristicP->getUUID().getShortUUID() == 3) {
            HVCNotifyCharacteristic      = *characteristicP;
            foundHVCNotifyCharacteristic = true;
            printf("#HVCNotifyCharacteristic Found\r\n");
        }
        //if (characteristicP->getShortUUID() == 4) {
        if (characteristicP->getUUID().getShortUUID() == 4) {
            HVCDeviceCharacteristic      = *characteristicP;
            foundHVCDeviceCharacteristic = true;
            printf("#HVCDeviceCharacteristic Found\r\n");
        }
    } else {
        printf("#long:\r\n");
        const uint8_t *longUUIDBytes = characteristicP->getUUID().getBaseUUID();
        pc.printf("#  C UUID-");
        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
            printf("%02x", longUUIDBytes[i]);
        }
        printf(" valueAttr[%u] props[%x]\r\n",  characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());
        flag=true;
        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
            if( longUUIDBytes[i]!=WriteCharacteristicAddr[i])flag=false;
        }
        if (flag==true) {
            HVCWriteCharacteristic      = *characteristicP;
            foundHVCWriteCharacteristic = true;
            printf("#HVCWriteCharacteristic Found\r\n");
        } else {
            flag=true;
            for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
                if( longUUIDBytes[i]!=NotifyCharacteristicAddr[i])flag=false;
            }
            if (flag==true) {
                HVCNotifyCharacteristic      = *characteristicP;
                foundHVCNotifyCharacteristic = true;
                printf("#HVCNotifyCharacteristic Found\r\n");
            } else {
                flag=true;
                for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
                    if( longUUIDBytes[i]!=DeviceCharacteristicAddr[i])flag=false;
                }
                if (flag==true) {
                    HVCDeviceCharacteristic      = *characteristicP;
                    foundHVCDeviceCharacteristic = true;
                    printf("#HVCDeviceCharacteristic Found\r\n");
                }
            }
        }
    }
}

void discoveryTerminationCallback(Gap::Handle_t connectionHandle)
{
    pc.printf("# discovery terminated CallBack %u\r\n", connectionHandle);
}

void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    if (params->role == Gap::CENTRAL) {
        connectionHandle = params->handle;
        ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
        ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback/*, 0xa000, 0xa001*/);
        ble.gap().stopScan();
        pc.printf("#HVC-C1B Connect\r\n");
        pc.printf("#SCAN Stop\r\n");
    }
}

void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
    pc.printf("#HVC-C1B Disconnected\r\n");
    if(Reconnect==true) {
        state=-1;
        ble.gap().startScan(advertisementCallback);
        pc.printf("#RE SCAN Start\r\n");
    }
}

void hvxCallback(const GattHVXCallbackParams *params)
{
    static bool flag=false; //flag=true no toki tudukiga aru
    static int l;
    static unsigned char buffer[100];
    int k,j,j1,j2,human_no,hand_no,face_no;
    pc.printf("#received HVX callback for handle %u; type %s\r\n", params->handle, (params->type == BLE_HVX_NOTIFICATION) ? "notification" : "indication");

    if(flag==false)l=0;

    for (unsigned index = 0; index < params->len; index++) {
        buffer[l]=params->data[index];
        l++;
    }
    if((buffer[0]==0xfe)||(buffer[1]==0x00)) {
        //Sucess
        if(state==3) {
            if((l+1)>(6+0x13-1)) {
                printf("# State3 %c%c%c%c%c%c%c%c%c%c%c%c %x %x %x %x\r\n",
                       buffer[6],buffer[7],buffer[8],buffer[9],buffer[10],buffer[11],
                       buffer[12],buffer[13],buffer[14],buffer[15],buffer[16],buffer[17],
                       buffer[18],buffer[19],buffer[20],
                       buffer[21]*0x1000000+buffer[22]*0x10000+buffer[23]*0x100+buffer[24]);
                WaitSend=true;
                flag=false;
            } else {
                flag=true;
            }
        } else if(state==4) {
            k=buffer[2]+buffer[3]*0x100+buffer[4]*0x10000+buffer[5]*0x1000000;
            if((l+1)>(6+k-1)) {
                human_no=buffer[6];
                hand_no=buffer[7];
                face_no=buffer[8];

                if(human_no>0) {
                    printf("HumanDetect:%d \r\n",human_no);
                    for(k=0; k<human_no; k++) {
                        printf("Human(%d):x=%d,y=%d,size=%d,depend=%d\r\n",
                               k,buffer[10+k*8+0]+buffer[10+k*8+1]*0x100,
                               buffer[10+k*8+2]+buffer[10+k*8+3]*0x100,
                               buffer[10+k*8+4]+buffer[10+k*8+5]*0x100,
                               buffer[10+k*8+6]+buffer[10+k*8+7]*0x100);
                    }
                } else {
                    if(HumanDetect==1) {
                        printf("HumanDetect:0\r\n");
                    }
                }
                if(hand_no>0) {
                    printf("HandDetect:%d \r\n",hand_no);
                    j=10+human_no*8;
                    for(k=0; k<hand_no; k++) {
                        printf("Hand(%d):x=%d,y=%d,size=%d,depend=%d\r\n",
                               k,buffer[j+k*8+0]+buffer[j+k*8+1]*0x100,
                               buffer[j+k*8+2]+buffer[j+k*8+3]*0x100,
                               buffer[j+k*8+4]+buffer[j+k*8+5]*0x100,
                               buffer[j+k*8+6]+buffer[j+k*8+7]*0x100);
                    }
                } else {
                    if(HandDetect==1) {
                        printf("HandDetect:0\r\n");
                    }
                }
                if(face_no>0) {
                    printf("FaceDetect:%d \r\n",face_no);
                    j=10+human_no*8+hand_no*8;
                    j2=FaceDetect*8+FaceDirection*8+FaceOld*3+ FaceSex*3+ FaceLook*2+FaceEyeClose*4+FaceExpression*3;
                    if(FaceDetect==1) {
                        j1=0;
                        for(k=0; k<face_no; k++) {
                            printf("Face(%d):x=%d,y=%d,size=%d,depend=%d\r\n",
                                   k,buffer[j+k*j2+j1+0]+buffer[j+k*j2+j1+1]*0x100,
                                   buffer[j+k*j2+j1+2]+buffer[j+k*j2+j1+3]*0x100,
                                   buffer[j+k*j2+j1+4]+buffer[j+k*j2+j1+5]*0x100,
                                   buffer[j+k*j2+j1+6]+buffer[j+k*j2+j1+7]*0x100);
                        }
                    }
                    if(FaceDirection==1) {
                        j1=FaceDetect*8;
                        for(k=0; k<face_no; k++) {
                            printf("FaceDirection(%d):holizon=%d,vertical=%d,angle=%d,depend=%d\r\n",
                                   k,(signed short)(buffer[j+k*j2+j1+0]+buffer[j+k*j2+j1+1]*0x100),
                                   (signed short)(buffer[j+k*j2+j1+2]+buffer[j+k*j2+j1+3]*0x100),
                                   (signed short)(buffer[j+k*j2+j1+4]+buffer[j+k*j2+j1+5]*0x100),
                                   (signed short)(buffer[j+k*j2+j1+6]+buffer[j+k*j2+j1+7]*0x100));
                        }
                    }
                    if(FaceOld==1) {
                        j1=FaceDetect*8+FaceDirection*8;
                        for(k=0; k<face_no; k++) {
                            printf("FaceOld(%d):old=%d,depend=%d\r\n",
                                   k,(signed char)(buffer[j+k*j2+j1+0]),
                                   (short)(buffer[j+k*j2+j1+1]+buffer[j+k*j2+j1+2]*0x100));
                        }
                    }
                    if(FaceSex==1) {
                        j1=FaceDetect*8+FaceDirection*8+FaceOld*3;
                        for(k=0; k<face_no; k++) {
                            printf("FaceSex(%d):Sex=%d,depend=%d\r\n",
                                   k,(signed char)(buffer[j+k*j2+j1+0]),  //0--feminine 1---male
                                   (short)(buffer[j+k*j2+j1+1]+buffer[j+k*j2+j1+2]*0x100));
                        }
                    }
                    if(FaceLook==1) {
                        j1=FaceDetect*8+FaceDirection*8+FaceOld*3+FaceSex*3;
                        for(k=0; k<face_no; k++) {
                            printf("FaceLook(%d):holizon=%d,vertical=%d\r\n",
                                   k,(signed char)(buffer[j+k*j2+j1+0]),  //0--feminine 1---male
                                   (signed char)(buffer[j+k*j2+j1+1]));
                        }
                    }
                    if(FaceEyeClose==1) {
                        j1=FaceDetect*8+FaceDirection*8+FaceOld*3+FaceSex*3+FaceLook*2;
                        for(k=0; k<face_no; k++) {
                            printf("FaceEyeClose(%d):xleft=%d,right=%d\r\n",
                                   k,(signed short)(buffer[j+k*j2+j1+0]+buffer[j+k*j2+j1+1]*0x100),
                                   (signed short)(buffer[j+k*j2+j1+2]+buffer[j+k*j2+j1+3]*0x100));
                        }
                    }
                    if(FaceExpression==1) {
                        j1=FaceDetect*8+FaceDirection*8+FaceOld*3+FaceSex*3+FaceLook*2+FaceEyeClose*4;
                        for(k=0; k<face_no; k++) {
                            printf("FaceExpression(%d):TopExpression=%d,TopScore=%d,look=%d\r\n",
                                   k,(signed char)(buffer[j+k*j2+j1+0]),
                                   (signed char)(buffer[j+k*j2+j1+1]),
                                   (signed char)(buffer[j+k*j2+j1+2]));
                        }
                    }

                } else {
                    if(FaceDetect==1)        printf("FaceDetect:0\r\n");
                    if(FaceDirection==1)     printf("FaceDirection:0\r\n");
                    if(FaceOld==1)           printf("FaceOld:0\r\n");
                    if(FaceSex==1)           printf("FaceSex:0\r\n");
                    if(FaceLook==1)          printf("FaceLook:0\r\n");
                    if(FaceEyeClose==1)      printf("FaceEyeClose:0\r\n");
                    if(FaceExpression==1)    printf("FaceExpression:0\r\n");
                }
                flag=false;
                WaitSend=true;
            } else {
                flag=true;
            }
        }
    } else {
        flag=false;
    }
}

void DataReadCallback(const GattReadCallbackParams *params)
{
    //    pc.printf("#Data Readed \r\n");
    //for (unsigned index = 0; index < params->len; index++) {
    //        pc.printf("# %02x", params->data[index]);
    //}
    //    pc.printf("\r\n");
}

void DataWriteCallback(const GattWriteCallbackParams *params)
{
    pc.printf("#send data %x\r\n",params->writeOp);
    //  for (unsigned index = 0; index < params->len; index++) {
    //      pc.printf(" %02x", params->data[index]);
    //  }
    //  pc.printf("\r\n");
    if(state==2) {
        printf("#State=2 Wited\r\n");
        WaitSend=true;
    }
}

int main(void)
{
    pc.baud(9600);
    pc.printf("#Progran Start\r\n");
    Ticker ticker;   //LED  HertBeat
    ticker.attach(periodicCallback, 1);

    ble.init();

    ble.gap().onConnection(connectionCallback);
    ble.gap().onDisconnection(disconnectionCallback);

    ble.gap().setScanParams(500, 400);
    ble.gap().startScan(advertisementCallback);

    ble.gattClient().onDataRead(DataReadCallback);
    ble.gattClient().onDataWrite(DataWriteCallback);
    ble.gattClient().onHVX(hvxCallback);

    uint8_t SendByte0[4]= {0xfe,00,00,00,};
    uint8_t SendByte1[7]= {0xfe,03,03,0,4,0x1,0};
    SendByte1[4]=FaceEyeClose*0x80+FaceLook*0x40+FaceSex*0x20+FaceOld*0x10+FaceDirection*0x8+FaceDetect*0x4+HandDetect*0x2+HumanDetect;
    SendByte1[5]=FaceExpression;
    state=-1;

    while (true) {
        if(state<0) {
            if (foundHVCWriteCharacteristic && foundHVCNotifyCharacteristic && foundHVCDeviceCharacteristic && !ble.gattClient().isServiceDiscoveryActive()) {
                printf("# State 1 Wait for Caracteristic Found \r\n");
                state=1;
            }
        } else if(state==1&&!ble.gattClient().isServiceDiscoveryActive()) {
            printf("# State 2 Notify Enable \r\n");
            state=2;
            uint16_t value = BLE_HVX_NOTIFICATION;
            WaitSend=false;
            ble.gattClient().write(GattClient::GATT_OP_WRITE_REQ,
                                   connectionHandle,
                                   HVCNotifyCharacteristic.getValueHandle() + 1,
                                   sizeof(uint16_t),
                                   reinterpret_cast<const uint8_t *>(&value));
        } else if(state==2&&!ble.gattClient().isServiceDiscoveryActive()&&WaitSend) {
            //3 KIDOUKAKUNIN
            state=3;
            WaitSend=false;
            pc.printf("# State3 kidoukakunin\r\n");
            ble.gattClient().write(GattClient::GATT_OP_WRITE_REQ,
                                   connectionHandle,
                                   HVCWriteCharacteristic.getValueHandle(),
                                   4,SendByte0);
        } else if((state==3||state==4)&&!ble.gattClient().isServiceDiscoveryActive()&&WaitSend) {
            //4 DataSend
            state=4;
            WaitSend=false;
            pc.printf("# State4 DataSending\r\n");
            ble.gattClient().write(GattClient::GATT_OP_WRITE_REQ,
                                   connectionHandle,
                                   HVCWriteCharacteristic.getValueHandle(),
                                   7,SendByte1);
        }
        ble.waitForEvent();
    }
}