File content as of revision 4:a0150e0f285b:

#include "mbed.h"
#include "neopixel.h"
#include "nrf_soc.h"
#include "common.h"
#include "ble/BLE.h"
#include "ble/services/HeartRateService.h"
#include "ble/services/BatteryService.h"
#include "ble/services/DeviceInformationService.h"
#include "UARTService.h"
#include "ble_gap.h"
#include "ble/services/DFUService.h" // ★OTA用

#define RAINBOW_RATIO 1
#define RCFILTER  0.9
#define RCFILTER2  0.9
#define NEOPIXEL_COUNT 6
#define MIN_LIGHT 0
#define PNNX_FACTOR 0.1 // 0.06
#define MAXARRAY 15 //15  // 心拍平均値計算用の配列サイズ //20
#define MAXHRVARRAY 10 //30 // 脈間ゆらぎ平均値計算用の配列サイズ
#define MIN_INTERVAL 200
#define INTERVAL 10  // Loopの間隔 通常時推奨1 通信時推奨２０
#define MAXCOLORARRAY 1 //(MIN_INTERVAL / INTERVAL) // 色平均値計算用の配列サイズ (MIN_INTERVAL / 7)
#define MAXCONARRAY 10
#define MAXSTRESSARRAY 3
#define DIM_LED 0.2 //0.2
//#define DIM_LED 1.0

// 心拍数の最低値、最高値
// 人間の場合は60～150、標準80
// 犬の場合は40～200、標準60
#define HR_MIN 40
#define HR_MAX 150

#define RELAX_RATIO_MIN 0.6

// ラジアンを度に変換する．
#define RadianToDegree(radian)    ((180 / 3.14159) * (radian))

// 度をラジアンに変換する．
#define DegreeToRadian(degree)    ((3.14159 / 180) * (degree))

// 色の識別値設定（ピンではない）
#define RED 0
#define BLUE 1
#define GREEN 2

#define SHOW_EXCITE 1
#define SHOW_HAPPY  2
#define SHOW_CONCENTRATION    4

int displayFlag = 0;

BLE  ble;
DFUService *dfuService; // ★OTA用

AnalogIn HRSigIn(P0_4);


//const static char     DEVICE_NAME[]        = "InupLogo";
static char     DEVICE_NAME[16]        = "IND";
static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE,
                                              GattService::UUID_DEVICE_INFORMATION_SERVICE};
static volatile bool  triggerSensorPolling = true;

short ColorArrayR[NEOPIXEL_COUNT];
short ColorArrayG[NEOPIXEL_COUNT];
short ColorArrayB[NEOPIXEL_COUNT];

short TargetColorArrayR[NEOPIXEL_COUNT];
short TargetColorArrayG[NEOPIXEL_COUNT];
short TargetColorArrayB[NEOPIXEL_COUNT];


#define NEOPIXEL_COMMANDLEN ((NEOPIXEL_COUNT * 3) + 1 + 1)


unsigned long pixelStartCounter;

int mainCurrentPixel = 0;
int mainPrevPixel = 0;
int mainPixelLightCount;
int mainPixelStep = 0;
int mainStepsRemain = 0;

unsigned long currentmillis = 0;
unsigned long prevmillis = 0;
unsigned long intervalmillis = 0;
unsigned long prevIntervalmillis = 0;
unsigned long intervalmillisArray[MAXARRAY] = {1000};
unsigned long intervalmillisHRVArray[MAXHRVARRAY] = {1000};
float conArray[MAXCONARRAY] = {0.5};

//float stressArray[MAXSTRESSARRAY] = {0.5};

float prevFilteredAngle = 0.5;
float happyRatio = 0.0;
float happyRatioNow = 0.0;
float prevRelaxRatio = 1.0;
float prevFilteredDistRatio = 0.0;

#define HR_THRESHOLD 2
int HRThreshold[] = {120, 1000}; 


// 気分判定フラグ
#define IS_RELAX 0     
#define IS_EXCITED 1
#define IS_HAPPY 2   
#define IS_INTERESTED 3  
#define IS_STRESSED 4   // IS_STRESSEDはMOODの個数としても扱うので最後の番号にすること
#define MOOD_COUNT IS_STRESSED + 1

// 気分判定係数
#define CONMARGIN 0.85
#define RELAXMARGIN 0.2
#define HAPPYMARGIN 0.225

// 気分判定フラグを索引にする
int MoodColorR[NEOPIXEL_COUNT][MOOD_COUNT];
int MoodColorG[NEOPIXEL_COUNT][MOOD_COUNT];
int MoodColorB[NEOPIXEL_COUNT][MOOD_COUNT];


unsigned char RainbowR[] = { 50,  50,  50, 127, 200, 255};
unsigned char RainbowG[] = { 50, 127, 255, 127, 100,  50};
unsigned char RainbowB[] = {255, 127,  50,  50,  50,  50};
/*
unsigned char HappyR[] = {255, 100, 100, 255, 100, 100};
unsigned char HappyG[] = {100, 255, 100, 100, 255, 100};
unsigned char HappyB[] = {100, 100, 255, 100, 100, 255};
*/
unsigned char HappyR[] = {255, 200, 100, 255, 200, 100};
unsigned char HappyG[] = {100, 255, 200, 100, 255, 200};
unsigned char HappyB[] = {100, 100, 255, 100, 100, 255};

unsigned char RelaxR[] = {  0, 100, 255,   0, 100, 255};
unsigned char RelaxG[] = {255, 255, 255, 255, 255, 255};
unsigned char RelaxB[] = {  0, 100, 255,   0, 100, 255};

unsigned char ExciteR[] = {255, 255, 255, 255, 255, 255};
unsigned char ExciteG[] = {100, 100, 100, 100, 100, 100};
unsigned char ExciteB[] = { 75,  50,  25,  25,  50,  75};
/*
unsigned char StressR[] = {255, 255, 255, 255, 100, 100};
unsigned char StressG[] = {  0,   0,   0,   0,   0,   0};
unsigned char StressB[] = {  0,   0, 100, 100, 255, 255};
*/
unsigned char StressR[] = {100, 100, 255, 255, 100, 100};
unsigned char StressG[] = {  0,   0,   0,   0,   0,   0};
unsigned char StressB[] = {255, 255, 100, 100, 255, 255};

unsigned char InterestR[] = {255, 255, 255, 255, 255, 255};
unsigned char InterestG[] = {255, 255, 255, 255, 255, 255};
unsigned char InterestB[] = {255, 100,   0,   0, 100, 255};

float rainbowOffset = 0.0;

bool bConcentration;
unsigned long loopCounter = 0;
int lightPos = 1;
int prevLightPos = 1;

double relaxRatio = 0.0;
float conRatio = 0.5;
float conRatioNow = 0.5;
double HR_ratio = 0.0;

unsigned long IBI;

int x3, y3;

int mainPixelCounter;

int prevInterval;
int prevprevInterval;

void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
    ble.gap().startAdvertising(); // restart advertising
}
/*
void ConnectTimeoutCallback(Gap::EventCallback_t timeoutCallback)
{
    ble.gap().startAdvertising(); // restart advertising
}
  */  

uint8_t BPM = 0;
float sensorValue = 0;
Timer t;
Timer tRand;

float redVal = 0;
float blueVal = 255;
float greenVal = 0;

Ticker neopixelTick;
    
UARTService *uartServicePtr;

void neopixelTick_Handler() {
    return;
}

void SetMoodColor() {
    int i = 0;
    
    for(i = 0; i < NEOPIXEL_COUNT; i++) {
        MoodColorR[i][IS_RELAX] = RelaxR[i];
        MoodColorG[i][IS_RELAX] = RelaxG[i];
        MoodColorB[i][IS_RELAX] = RelaxB[i];
    }
    for(i = 0; i < NEOPIXEL_COUNT; i++) {
        MoodColorR[i][IS_EXCITED] = ExciteR[i];
        MoodColorG[i][IS_EXCITED] = ExciteG[i];
        MoodColorB[i][IS_EXCITED] = ExciteB[i];
    }
    
    for(i = 0; i < NEOPIXEL_COUNT; i++) {
        MoodColorR[i][IS_HAPPY] = HappyR[i];
        MoodColorG[i][IS_HAPPY] = HappyG[i];
        MoodColorB[i][IS_HAPPY] = HappyB[i];
    }
    for(i = 0; i < NEOPIXEL_COUNT; i++) {
        MoodColorR[i][IS_INTERESTED] = InterestR[i];
        MoodColorG[i][IS_INTERESTED] = InterestG[i];
        MoodColorB[i][IS_INTERESTED] = InterestB[i];
    }
    for(i = 0; i < NEOPIXEL_COUNT; i++) {
        MoodColorR[i][IS_STRESSED] = StressR[i];
        MoodColorG[i][IS_STRESSED] = StressG[i];
        MoodColorB[i][IS_STRESSED] = StressB[i];
    }
}

// 現在の気分の判定
int getMood(int bpm, double relaxRatio, double happyRatio, double conRatio) {
    // 第一優先：集中
    // 集中係数が0.95以上なら集中と判定
    if(conRatio >= CONMARGIN)
    {
        return IS_INTERESTED;
    }
    
    // 第二優先：Stress
    // リラックス係数が0.2以下
    if(relaxRatio <= RELAXMARGIN) {
        return IS_STRESSED;
    }
    
    // 第三優先：Happy
    // Happy係数が0.2以上
    if(happyRatio >= HAPPYMARGIN && relaxRatio > 0.5) {
        return IS_HAPPY;
    }

    // それ以外は興奮度合い
    // 心拍数の閾値判定
    int i = 0;
    for(i = 0; i < HR_THRESHOLD; i++) {
        if (BPM <= HRThreshold[i]) {
            break;
        }
    }
    switch(i) {
    // 第１領域：NORMAL
    case 0: 
        return IS_RELAX;
    // 第２領域：EXCITED1
    case 1: 
        return IS_EXCITED;
    }
    
    return IS_RELAX;
}

//Serial mySerial(USBTX, USBRX);
//Serial mySerial(P0_9, P0_11);

// MACアドレスを取得してBLEのデバイス名に組み込む
void setBleDeviceName(char *deviceName)
{
    int i;
        
    //BLEProtocol::AddressType_t type;    //< The type of the BLE address.
    //BLEProtocol::AddressBytes_t mac_address; //< The BLE address. // typedef uint8_t AddressBytes_t[ADDR_LEN]
    
    ble_gap_addr_t mac_address;
    
    if(sd_ble_gap_address_get(&mac_address) != NRF_SUCCESS){
        return;
    }
    
    //  ADDR_LENは6
    for(i=BLE_GAP_ADDR_LEN-1; i>=0; i--){
        sprintf(deviceName, "%s%02x", deviceName, (mac_address.addr)[i]);
    }
    sprintf(deviceName, "%s%s", deviceName, "\0");    
            
    /*
    if(ble.getAddress(&type, mac_address) == BLE_ERROR_NONE){
        //  ADDR_LENは6
        for(i=BLEProtocol::ADDR_LEN-1; i>=0; i--){
            sprintf(deviceName, "%s%02x", deviceName, mac_address[i]);
        }
        sprintf(deviceName, "%s%s", deviceName, "\0");
    }
    */
} 

int main(void)
{
    char msgIBI[100];
    int nMsgIBILen = 0;
    char msgHappy[10];
    int nMsgHappyLen = 0;
    char msgConcentration[10];
    int nMsgConcentrationLen = 0;
    char msgRelax[10];
    int nMsgRelaxLen = 0;
    
    char msg[256];
    int nMsgLen = 0;

    float Brightness = 1.0;
    bool brighten = true;
    float brightenSpeedMs = 1000.0;
    float colorSpeedMs = 1000.0;
    float conRatioShow = 0.5;
    
    int i;
    unsigned long lightPrevMillis = 0; 
    
    int currentBPM;

    float fBeatIndex = 0.0;
    
//    displayFlag = SHOW_EXCITE;

    displayFlag = SHOW_EXCITE | SHOW_HAPPY | SHOW_CONCENTRATION;
    //displayFlag = SHOW_HAPPY ;

//    mySerial.baud(9600);
//    mySerial.format(8, Serial::None, 1);
    
//    PwrPin = 1;
    
    static uint8_t rrIntervalH    = 0;
    static uint8_t rrIntervalL    = 1;
//    static uint8_t bpm[8]         = {0x10, hrmCounter, 0, 0, 0, 0, rrIntervalH, rrIntervalL};

    neopixel_strip_t neopixel1;
    neopixel_init(&neopixel1, P0_7, 1); 
    neopixel_clear(&neopixel1);
    
    neopixel_strip_t neopixel2;
    neopixel_init(&neopixel2, P0_8, 1);
    neopixel_clear(&neopixel2);
    
    neopixel_strip_t neopixel3;
    neopixel_init(&neopixel3, P0_10, 1);
    neopixel_clear(&neopixel3);
    
    neopixel_strip_t neopixel4;
    neopixel_init(&neopixel4, P0_9, 1);
    neopixel_clear(&neopixel4);
    
    neopixel_strip_t neopixel5;
    neopixel_init(&neopixel5, P0_15, 1);
    neopixel_clear(&neopixel5);
    
    neopixel_strip_t neopixel6;
    neopixel_init(&neopixel6, P0_11, 1);
    neopixel_clear(&neopixel6);
    
    neopixel_strip_t *pixels[6];
        
    pixels[0] = &neopixel1;
    pixels[1] = &neopixel2;
    pixels[2] = &neopixel3;
    pixels[3] = &neopixel4;
    pixels[4] = &neopixel5;
    pixels[5] = &neopixel6;
    
    
    int pixelIndex = 0;
    SetMoodColor();
//    my_analogin_init();

        
    ble.init();
    ble.gap().onDisconnection(disconnectionCallback);

    
//    ble.gap().onTimeout(ConnectTimeoutCallback);
    
    UARTService uartService(ble);
    uartServicePtr = &uartService;

    // ★OTA用
    dfuService = new DFUService(ble, NULL);
    
    /* Setup primary service. */
    HeartRateService hrServiceBPM(ble, BPM, HeartRateService::LOCATION_CHEST);
//    HeartRateService hrServiceIBI(ble, IBI, HeartRateService::LOCATION_FINGER);

//    BatteryService batteryService(ble, 100);
    
    /* Setup auxiliary service. */
    //DeviceInformationService deviceInfo(ble, "ARM", "Model1", "SN1", "hw-rev1", "fw-rev1", "soft-rev1");
    // ★
    DeviceInformationService deviceInfo(ble, "INUPATHY01", "0.0.1", "SN1", "0.0.1", "0.0.1", "0.0.1");

    // MACアドレスをDEVICE_NAMEに組み込む   
    setBleDeviceName(DEVICE_NAME); 

    /* Setup advertising. */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(800); /* 1000ms */
    ble.gap().startAdvertising();
 
    
    for( i = 0; i < NEOPIXEL_COUNT; i++)
    {
//        ColorArrayR[i] = RainbowR[i];
//        ColorArrayG[i] = RainbowG[i];
//        ColorArrayB[i] = RainbowB[i];
        ColorArrayR[i] = StressR[i];
        ColorArrayG[i] = StressG[i];
        ColorArrayB[i] = StressB[i];
        TargetColorArrayR[i] = ColorArrayR[i];
        TargetColorArrayG[i] = ColorArrayG[i];
        TargetColorArrayB[i] = ColorArrayB[i];
    
        neopixel_set_color(
            pixels[i],
            0,
            short(float(ColorArrayR[i] * DIM_LED)),
            short(float(ColorArrayG[i] * DIM_LED)),
            short(float(ColorArrayB[i] * DIM_LED)));
        neopixel_show(pixels[i]);
    }
 
    t.start();
    
    prevmillis = t.read_ms();
    
    bool bPulseEvent = false;

    float degPoint = 0;
    float degRatio = 0;

    bool bSwingR = false;
    
    float swingIndex = 0.0;
    
    float midPoint = 0.5;

//    neopixelTick.attach(neopixelTick_Handler, 0.005);

    while (1) {

//        batteryService.updateBatteryLevel( (uint8_t)read_battery_level());
            
        float currentValue = HRSigIn; 

        bPulseEvent = false;
        
        if ((currentValue <= 0.3) && (sensorValue > 0.3))
        {
            bPulseEvent = true;
        }
        
        int sens = 1023 * currentValue;
        
//        printf("S%d", sens);
//        printf("S%d\n", sens);

        sensorValue = currentValue;
        
        if(bPulseEvent) {
            fBeatIndex = 0.0;
//            batteryService.updateBatteryLevel( (uint8_t)read_battery_level());

            // 現在の時刻を取得
//            t.stop();         // stop, startは時間計測の結果を有意に短くしてしまうので使わない
            currentmillis = t.read_ms();
//            t.start();
            // 現在時刻から過去時刻を引き、間隔を求める
            intervalmillis = currentmillis - prevmillis;
    
            // 心拍の間の間隔
            IBI = intervalmillis;
            
            // 現在の時刻を過去時刻として記録
            prevmillis = currentmillis;
        
            // 平均値算出用配列に値を詰める
            double averageIntervalMillis;
            
            for(i = 0; i < MAXARRAY ; i++)
            {
                // 最後尾以外の場合
                if(i != MAXARRAY - 1)
                {
                    // 次の要素の値をコピーする
                    intervalmillisArray[i] = intervalmillisArray[i + 1];
                }
                // 最後尾の場合
                else {
                    // 最新値を詰める
                    intervalmillisArray[i] = intervalmillis;
                }
            }
            // 先頭が0の間は何もしない
            unsigned long sumIntervalMillis = 0;
            unsigned long sumIntervalMillisV = 0;
            if (intervalmillisArray[0] != 0)
            {
                // 平均値算出
                averageIntervalMillis = 0;
                for(i = 0; i < MAXARRAY ; i++)
                {
                    sumIntervalMillis += intervalmillisArray[i];
                    sumIntervalMillisV += intervalmillisArray[i] * intervalmillisArray[i];
                }
                double squ;
                double var;
                double stdDev;
                
                averageIntervalMillis = (double)sumIntervalMillis / (double)MAXARRAY;
                
                squ = sumIntervalMillisV - (double)(sumIntervalMillis * sumIntervalMillis) / (double)MAXARRAY;
                var = squ / (double)(MAXARRAY - 1);
                stdDev = sqrt(var);
                
                BPM = min32_of(60000 / averageIntervalMillis, 255);
//                if(intervalmillis < averageIntervalMillis - stdDev) { // || intervalmillis > averageIntervalMillis + stdDev) {
                
                currentBPM = min32_of(60000 / intervalmillis, 255);      
/*                }
                else {
                    currentBPM = BPM;
                } */
            }
            /*
            int IBI1024 = IBI * (1024/1000);
            
            bpm[1] = BPM;
            bpm[6] = (IBI1024 & 0xFF00) >> 8;
            bpm[7] = (IBI1024 & 0xFF);
            */
            // update bpm
            if(ble.gap().getState().connected == 1)
            {
                hrServiceBPM.updateHeartRate(BPM);
//                hrServiceIBI.updateHeartRate(IBI);
            }
            
//            printf("B%d\n", BPM);
//            printf("Q%d\n", IBI);

//            nMsgLen = sprintf(msg,"B%d\0", BPM); 
//            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
            
            nMsgIBILen = sprintf(msgIBI,"Q%d\0", IBI); 
//            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
            
            // 平均値算出用配列に値を詰める
            for(i = 0; i < MAXHRVARRAY ; i++)
            {
                // 最後尾以外の場合
                if(i != MAXHRVARRAY - 1)
                {
                    // 次の要素の値をコピーする
                    intervalmillisHRVArray[i] = intervalmillisHRVArray[i + 1];
                }
                // 最後尾の場合
                else {
                    // 最新値を詰める
                    intervalmillisHRVArray[i] = intervalmillis;
                }
            }

            // 平均値算出
            int nNNInterval;
            unsigned long ulIntervalSum = 0;
            int nSampleCount = 0;
            for(i = 0; i < MAXHRVARRAY ; i++)
            {
                if(intervalmillisHRVArray[i] > 0)
                {
                    ulIntervalSum += intervalmillisHRVArray[i];
                    nSampleCount++;
                }
            }
            
            float IntervalAverage = 0.0;
            if(nSampleCount > 0){
                IntervalAverage = (float)ulIntervalSum / (float)nSampleCount;
            }
            
            nNNInterval = (int)((float)IntervalAverage * PNNX_FACTOR);            
            
            
            bool bContinue = false;
            int nNNxCount = 0;
            for(i = 0; i < MAXHRVARRAY ; i++)
            {
                // if not end
                if(i < MAXHRVARRAY - 1) {
                    if(abs((int)(intervalmillisHRVArray[i] - intervalmillisHRVArray[i + 1])) >= nNNInterval)
                    {
                        if(bContinue == false)
                        {
                            nNNxCount++;
                        }
                        nNNxCount++;
                        bContinue = true;
                    }
                    else {
                        bContinue = false;
                    }
                } 
            }
        
            float diffRatio = (float)nNNxCount / (float)nSampleCount;
        
            if(diffRatio > 1)
            {
                diffRatio = 1;
            }

            // リラックスの係数を算出する
            relaxRatio = diffRatio;

            nMsgRelaxLen = sprintf(msgRelax,"R%0.2f\0", relaxRatio); 
            
//            nMsgLen = sprintf(msg,"R%0.2f\0", relaxRatio); 
//            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
            
            // 現在値から最低値を引く
//            relaxRatio = relaxRatio - RELAX_RATIO_MIN;
            
            // 1.0 / 最低値 × 現在値
//            relaxRatio = relaxRatio * (1.0 / (1.0 - RELAX_RATIO_MIN));

            int x1, y1, x2, y2;
            int tempx1, tempy1, tempx2, tempy2, tempx3, tempy3;
            int longx, shortx, longy, shorty;
            long xx1, xx2, yy1, yy2;
            
            int movex, movey;
            
            double dist1, dist2;
            double conRatioRatio;
            
            /*
            // 最後尾の3データから二つのx, y地点を作る。
            x1 = intervalmillisArray[nSampleCount - 1];
            y1 = intervalmillisArray[nSampleCount - 2];
            x2 = intervalmillisArray[nSampleCount - 2];
            y2 = intervalmillisArray[nSampleCount - 3];
            */
            
            x1 = intervalmillis;
            y1 = prevInterval;
            x2 = prevInterval;
            y2 = prevprevInterval;
            
            // ２地点を結ぶ直線の長さを算出する。
            if(x1 > x2)
            {
                longx = x1;
                shortx = x2;
            }
            else {
                longx = x2;
                shortx = x1;
            }
            
            if(y1 > y2)
            {
                longy = y1;
                shorty = y2;
            }
            else {
                longy = y2;
                shorty = y1;
            }
            
            xx1 = longx - shortx;
            yy1 = longy - shorty;
            
            dist1 = sqrt((double)((xx1 * xx1) + (yy1 * yy1)));
            
            dist2 = longx + longy;
            float distRatio = dist1 / dist2;

            conRatioNow = 1 - distRatio;
            
            // 平均値算出用配列に値を詰める
            float averageConRatio = 0.5;
            for(i = 0; i < MAXCONARRAY ; i++)
            {
                // 最後尾以外の場合
                if(i != MAXCONARRAY - 1)
                {
                    // 次の要素の値をコピーする
                    conArray[i] = conArray[i + 1];
                }
                // 最後尾の場合
                else {
                    // 最新値を詰める
                    conArray[i] = distRatio;
                }
            }
            // 先頭が0の間は何もしない
            if (conArray[0] != 0)
            {
                // 平均値算出
                averageConRatio = 0;
                for(i = 0; i < MAXCONARRAY ; i++)
                {
                    averageConRatio += conArray[i];
                }
                averageConRatio = averageConRatio / (double)MAXCONARRAY;  
            }
/*
            float averageStressRatio = 0.5;
            for(i = 0; i < MAXSTRESSARRAY ; i++)
            {
                // 最後尾以外の場合
                if(i != MAXSTRESSARRAY - 1)
                {
                    // 次の要素の値をコピーする
                    stressArray[i] = stressArray[i + 1];
                }
                // 最後尾の場合
                else {
                    // 最新値を詰める
                    if(x1 > y1) {
                        stressArray[i] = y1 / x1;
                    }
                    else {
                        stressArray[i] = x1 / y1;
                    }
                }
            }
            // 先頭が0の間は何もしない
            if (stressArray[0] != 0)
            {
                // 平均値算出
                averageStressRatio = 0;
                for(i = 0; i < MAXSTRESSARRAY ; i++)
                {
                    averageStressRatio += stressArray[i];
                }
                averageStressRatio = averageStressRatio / (double)MAXSTRESSARRAY;  
            }
  */          

/*            float rcFilter = RCFILTER2;
            float filteredDistRatio = 0.0;
            float a;
            if(prevFilteredDistRatio != 0) {
                a = rcFilter * prevFilteredDistRatio;
                filteredDistRatio = a + (1 - rcFilter) * distRatio;
            }
            else {
                filteredDistRatio = (1 - rcFilter) * distRatio;
            }            
            prevFilteredDistRatio = filteredDistRatio ;
            
            conRatio = filteredDistRatio ;
 */                   
            conRatio = averageConRatio;
            conRatio = 1 - conRatio;

            nMsgConcentrationLen = sprintf(msgConcentration,"C%0.2f\0", conRatio); 

//            nMsgLen = sprintf(msg,"C%0.2f\0", conRatio); 
//            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
            
            
            prevprevInterval = prevInterval;
            prevInterval = intervalmillis; 
            
            // (x2, y2)が原点となるように座標を変換する
            movex = x2 * -1;
            movey = y2 * -1;
            
            tempx1 = x1 - movex;
            tempy1 = y1 - movey;
            tempx2 = x2 - movex;
            tempy2 = y2 - movey;
            tempx3 = x3 - movex;
            tempy3 = y3 - movey;
            
            // (x2, y2)を中心とした円を考える。
                        
            // (x3, y3)を(x2, y2)を中心に180°回転する。→(rotx3, roty3)
            double theta = DegreeToRadian(180);
            
            double rotx3 = (tempx3) * cos(theta) - (tempy3) * sin(theta);
            double roty3 = (tempx3) * sin(theta) + (tempy3) * cos(theta);
            
            // ベクトル1とベクトル3の内積を求める
            double vecInt = tempx1 * rotx3 + tempy1 * roty3;
            
            // ベクトル1とベクトル3の外積を求める
            double vecExt = tempx1 * roty3 - tempy1 * rotx3;
            
            // ベクトル1とベクトル3の角度を求める
            theta = atan2(vecExt, vecInt);
            
            double deg = RadianToDegree(theta);
            
            midPoint = degRatio;
            
            if( deg <= 11.25 && deg >= -11.25) {
                degPoint = 0.5;
            }
            else if(deg >= 11.25 && deg <= 33.75) {
                degPoint = 0.375;
            }
            else if(deg >= 33.75 && deg <= 56.25) {
                degPoint = 0.25;
            }
            else if(deg >= 56.25 && deg <= 78.75) {
                degPoint = 0.125;
            }
            else if(deg >= 78.75 && deg <= 101.25) {
               degPoint = 0;
            }
            else if(deg >= 101.25 && deg <= 123.75) {
                degPoint = 0.125;
            }
            else if(deg >= 123.75 && deg <= 146.25) {
                degPoint = 0.25;
            }
            else if(deg >= 146.25 && deg <= 168.75) {
                degPoint = 0.375;
            }
            else if(deg >= 168.75 && deg <= 180 || deg <= 0 && deg >= -11.25) {
                degPoint = 0.5;
            }
            else if(deg <= -11.25 && deg >= -33.75) {
                degPoint = 0.625;
            }
            else if(deg <= -33.75 && deg >= -56.25) {
                degPoint = 0.75;
            }
            else if(deg <= -56.25 && deg >= -78.75) {
                degPoint = 0.875;
            }
            else if(deg <= -78.75 && deg >= -101.25) {
                degPoint = 1;
            }
            else if(deg <= -101.25 && deg >= -123.75) {
                degPoint = 0.875;
            }
            else if(deg <= -123.75 && deg >= -146.25) {
                degPoint = 0.75;
            }
            else if(deg <= -146.25 && deg >= -168.75) {
                degPoint = 0.625;
            }
            else {
                degPoint = 0;
            }
            
            happyRatioNow = degPoint;
            
            degRatio = (deg + 180.0) / 360.0;

            float rcFilter = RCFILTER;
            float filteredAngle = rcFilter * prevFilteredAngle + (1 - rcFilter) * degPoint;            
            prevFilteredAngle = filteredAngle ;

            happyRatio = filteredAngle - 0.5;
            
            if(happyRatio <= 0){
                happyRatio = abs(happyRatio);
            }
            else {
                happyRatio = 0;
            }
            
//            happyRatio = averageStressRatio ;// @@@

            nMsgHappyLen = sprintf(msgHappy,"H%0.2f\0", happyRatio); 

//            nMsgLen = sprintf(msg,"H%0.2f\0", happyRatio); 
//            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
            
//            happyRatio = happyRatio * happyRatio * happyRatio;
              
//            happyRatio *= 10;
            
            x3 = x2;
            y3 = y2;
                
            int HR_range = 0;
            
            int HR_tempval = currentBPM;
            
            
            // 心拍の係数を算出する
            
            // 最大心拍数以上→最大心拍数に
            if( HR_MAX < HR_tempval)
            {
                HR_tempval = HR_MAX;
            }
            
            // 最少心拍数以下→最少心拍数に
            if( HR_MIN > HR_tempval)
            {
                HR_tempval = HR_MIN;
            }
            
            // 心拍の幅を計算
            HR_range = HR_MAX - HR_MIN;
            
            // 現在値－最少心拍数
            HR_tempval = HR_tempval - HR_MIN;
            
            // 心拍の係数を算出。0 ～ 1.0
            HR_ratio = (double)HR_tempval / (double)HR_range;
            
            if(conRatioNow >= 0.95 /* && averageAngle >= 0.7*/)
            {
                bConcentration = true;
            }
            else {
                bConcentration = false;
            }  
            prevRelaxRatio = relaxRatio;

            int hval = 1023 * happyRatio;
            int rval = 1023 * relaxRatio;
            int cval = 1023 * conRatio;

//            printf("H%d\n", hval);
//            printf("R%d\n", rval);
//            printf("C%d\n", cval);

            sprintf(msg,"%s%s%s%s\0", msgIBI, msgHappy, msgRelax, msgConcentration); 
            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),
             (uint8_t*)msg, nMsgIBILen + nMsgHappyLen  + nMsgRelaxLen + nMsgConcentrationLen);
//             (uint8_t*)msg, nMsgIBILen + 1 + nMsgHappyLen + 1 + nMsgRelaxLen + 1 + nMsgConcentrationLen);

            // determine color range

            conRatio = conRatio - 0.5;
            
            if(conRatio < 0) {
                conRatio = 0;
            }
            
            conRatio = conRatio * 2;

//            happyRatio = happyRatio * 2;
//            happyRatio = happyRatio * 2;
            if(happyRatio > 1) {
                happyRatio = 1;
            }
        /*    
            if(BPM >= HR_MIN) {
                happyRatio /= ((float)(BPM * 2) / 255.0) ;
            }
          */          
        }

        if(BPM > 0 && bPulseEvent) {
            int NewMood = getMood(BPM, relaxRatio, happyRatio, conRatio) ;
            
            for(i = 0 ; i < NEOPIXEL_COUNT; i++) {
                TargetColorArrayR[i] = MoodColorR[i][NewMood];
                TargetColorArrayG[i] = MoodColorG[i][NewMood];
                TargetColorArrayB[i] = MoodColorB[i][NewMood];
            }
        }
        
//        for(i = NEOPIXEL_COUNT - 1; i >= 0; i--) {
            if(ColorArrayR[pixelIndex] < TargetColorArrayR[pixelIndex]) {
                ColorArrayR[pixelIndex]++;
            }
            else if(ColorArrayR[pixelIndex] > TargetColorArrayR[pixelIndex]) {
                ColorArrayR[pixelIndex]--;
            }
/*            if(ColorArrayR[pixelIndex] < 200) {
                ColorArrayR[pixelIndex]++;
            }
            else if(ColorArrayR[pixelIndex] > 200) {
                ColorArrayR[pixelIndex]--;
            }*/
              if(ColorArrayG[pixelIndex] < TargetColorArrayG[pixelIndex]) {
                ColorArrayG[pixelIndex]++;
            }
            else if(ColorArrayG[pixelIndex] > TargetColorArrayG[pixelIndex]) {
                ColorArrayG[pixelIndex]--;
            }
            if(ColorArrayB[pixelIndex] < TargetColorArrayB[pixelIndex]) {
                ColorArrayB[pixelIndex]++;
            }
            else if(ColorArrayB[pixelIndex] > TargetColorArrayB[pixelIndex]) {
                ColorArrayB[pixelIndex]--;
            }
            
            neopixel_set_color(
                pixels[pixelIndex],
                0,
                short(float(ColorArrayR[pixelIndex]) * DIM_LED),
                short(float(ColorArrayG[pixelIndex]) * DIM_LED),
                short(float(ColorArrayB[pixelIndex]) * DIM_LED));
            neopixel_show(pixels[pixelIndex]);
//        }
        pixelIndex++;
        if(pixelIndex >= NEOPIXEL_COUNT)
        {
            pixelIndex = 0;
        }
        //wait_ms(1);
        wait_us(500);
    }
}