File content as of revision 14:698909b62531:

// 20170703 BRIGHTNESS_MAXを100に変更
//          シリアル通信のプロトコルを、文字列→バイト列に変更
// 20170619 DeviceInformationのSerialNumberにIND+Macアドレスを適用し、DEVICENAMEはINUPATHYとした
// 20170617 ストレス値を"R"(relax)から"S"(stress)に変更し、(1.0 - relaxRatio)を送るよう改変
// 20170617 輝度調整の方法をrgbの合計値を基準にするように調整した。

#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 BRIGHTNESS_MAX 100 // RGBLED１セットのpwm合計値の上限 ws2812b型のneopixelが６個の場合、150を超えた値にするとセンサーが誤作動する

#define DIM_LED ((float)(BRIGHTNESS_MAX) / (float)(255))   // R, G, Bそれぞれのpwm値がBRIGHTNESS_MAXを超えない様にする制限


// 心拍数の最低値、最高値
// 人間の場合は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


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

AnalogIn HRSigIn(P0_4);


const static char     DEVICE_NAME[]        = "INUPATHY";
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.9 // 0.85
#define RELAXMARGIN 0.35
#define HAPPYMARGIN 0.25 // 0.225
#define EXCITEMARGIN 1.5 // 1.15

#define HRM_AVERAGE_COUNT 100 // MAX 125
short HRM_Average[HRM_AVERAGE_COUNT] = {70};

// 気分判定フラグを索引にする
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[] = { 50,  50,  50, 127, 200, 255};
unsigned char HappyG[] = { 50, 127, 255, 127, 100,  50};
unsigned char HappyB[] = {255, 127,  50,  50,  50,  50};

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

unsigned char ExciteR[] = {255, 255, 255, 255, 255, 255};
unsigned char ExciteG[] = {100, 100, 100, 100, 100, 100};
unsigned char ExciteB[] = {  0,   0,   0,   0,   0,   0};

unsigned char StressR[] = {100, 100, 100, 100, 100, 100};
unsigned char StressG[] = {  0,   0,   0,   0,   0,   0};
unsigned char StressB[] = {255, 255, 255, 255, 255, 255};

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

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];
    }
}


float BPM_Average = 0.0;
int currentBPM = 0;

// 現在の気分の判定
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;
    }

    // 平均の心拍より1.2倍以上だと興奮
    // それ以外は安静
    if((float)currentBPM / BPM_Average >= EXCITEMARGIN) {
        return IS_EXCITED;
    }
    // もしくは一定以上の心拍数だと興奮
    else if(BPM >= HRThreshold[0]) {
        return IS_EXCITED;
    }
    else {
        return IS_RELAX;
    }
}

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

// MACアドレスを取得してBLEのSerialNumberに組み込む
void setBleSerialNumber(char *serialNumber)
{
    int i;
    
    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(serialNumber, "%s%02x", serialNumber, (mac_address.addr)[i]);
    }
    sprintf(serialNumber, "%s%s", serialNumber, "\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;
*/
    byte_t msgHeader[3]  = {0x49,0x4e,0x50}; 
    
//    byte_t msg[19];
    byte_t msg[10];
//    int nMsgLen = 0;

    int i;

    int r = 0;
    int g = 0;
    int b = 0;
    int brightnessSum = 0;

    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;
    
    

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

    
    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");
    
    char serialNumber[16] = "IND";    
    // SerialNumberStringにMACアドレスを組み込む
    setBleSerialNumber(serialNumber);  
    
    // ★
    DeviceInformationService deviceInfo(ble, "INUPATHY01", "0.0.1", serialNumber, "0.0.1", "0.0.1", "0.0.1");

    /* 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];
     
        TargetColorArrayR[i] = ColorArrayR[i];
        TargetColorArrayG[i] = ColorArrayG[i];
        TargetColorArrayB[i] = ColorArrayB[i];
        
        // 輝度調整
        r = ColorArrayR[i] * DIM_LED;
        g = ColorArrayG[i] * DIM_LED;
        b = ColorArrayB[i] * DIM_LED;
     
        brightnessSum = r + g + b;
        
        // 輝度の合計値が上限を超えた場合は補正する
        if(brightnessSum > BRIGHTNESS_MAX) {
            r = (int)(r * ((float)BRIGHTNESS_MAX / (float)brightnessSum));
            g = (int)(g * ((float)BRIGHTNESS_MAX / (float)brightnessSum));
            b = (int)(b * ((float)BRIGHTNESS_MAX / (float)brightnessSum));
        }
        
        neopixel_set_color(
            pixels[i],
            0,
            short(r),
            short(g),
            short(b));
        neopixel_show(pixels[i]);
    }
 
    t.start();
    
    prevmillis = t.read_ms();
    
    bool bPulseEvent = false;

    float degPoint = 0;

//    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;
        }
        
//        printf("S%d", sens);
//        printf("S%d\n", sens);

        sensorValue = currentValue;
        
        if(bPulseEvent) {
//            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];
                }
                
                averageIntervalMillis = (double)sumIntervalMillis / (double)MAXARRAY;
                                
                BPM = min32_of(60000 / averageIntervalMillis, 255);
//                if(intervalmillis < averageIntervalMillis - stdDev) { // || intervalmillis > averageIntervalMillis + stdDev) {
                
                currentBPM = min32_of(60000 / intervalmillis, 255);      
/*                }
                else {
                    currentBPM = BPM;
                } */
            }
            
            
            // BPMの平均値計算
            int BPM_sum = 0;
            for(i = 0; i < HRM_AVERAGE_COUNT ; i++)
            {
                BPM_sum += HRM_Average[i];
            }
            BPM_Average = (float)BPM_sum/(float)HRM_AVERAGE_COUNT;
            
            
            if(currentBPM > 0) {
                for(i = 0; i < HRM_AVERAGE_COUNT ; i++)
                {
                    // 最後尾以外の場合
                    if(i != HRM_AVERAGE_COUNT - 1)
                    {
                        // 次の要素の値をコピーする
                        HRM_Average[i] = HRM_Average[i + 1];
                    }
                    // 最後尾の場合
                    else {
                        // 最新値を詰める
                        HRM_Average[i] = currentBPM;
                    }
                }
            }
            
            /*
            int IBI1024 = IBI * (1024/1000);
            
            bpm[1] = BPM;
            bpm[6] = (IBI1024 & 0xFF00) >> 8;
            bpm[7] = (IBI1024 & 0xFF);
            */
            
//            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,"S%0.2f\0", (1-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, tempx3, tempy3;
            int longx, shortx, longy, shorty;
            long xx1, yy1;
            
            int movex, movey;
            
            double dist1, dist2;
            
            /*
            // 最後尾の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;
            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);
            
            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;
            
            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;

//            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); 

            unsigned char happyRatioChar = (unsigned char)(happyRatio * 100);
            unsigned char stressRatioChar = (unsigned char)((1 - relaxRatio) * 100);
            unsigned char conRatioChar = (unsigned char)(conRatio * 100);

            memcpy(msg, msgHeader, sizeof(msgHeader));
            memcpy(&(msg[3]), &IBI          , sizeof(IBI)       );
            memcpy(&(msg[7]), &happyRatioChar   , sizeof(happyRatioChar));
            memcpy(&(msg[8]), &stressRatioChar  , sizeof(stressRatioChar));
            memcpy(&(msg[9]), &conRatioChar     , sizeof(conRatioChar)  );
            ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),
            (uint8_t*)msg, sizeof(msg));
//             (uint8_t*)msg, nMsgIBILen + nMsgHappyLen  + nMsgRelaxLen + nMsgConcentrationLen);
//             (uint8_t*)msg, nMsgIBILen + 1 + nMsgHappyLen + 1 + nMsgRelaxLen + 1 + nMsgConcentrationLen);
            // update bpm

            if(ble.gap().getState().connected == 1)
            {
                hrServiceBPM.updateHeartRate(BPM);
//                hrServiceIBI.updateHeartRate(IBI);
            }

            // 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];
            }
        }
        
        int deltaVal = 2;
        for(i = NEOPIXEL_COUNT - 1; i >= 0; i--) {
            if(ColorArrayR[i] < TargetColorArrayR[i]) {
                ColorArrayR[i]+=deltaVal;
                if(ColorArrayR[i] > TargetColorArrayR[i]) {
                   ColorArrayR[i] = TargetColorArrayR[i];
                }
            }
            else if(ColorArrayR[i] > TargetColorArrayR[i]) {
                ColorArrayR[i]-=deltaVal;
                if(ColorArrayR[i] < TargetColorArrayR[i]) {
                   ColorArrayR[i] = TargetColorArrayR[i];
                }
            }
            if(ColorArrayG[i] < TargetColorArrayG[i]) {
                ColorArrayG[i]+=deltaVal;
                if(ColorArrayG[i] > TargetColorArrayG[i]) {
                   ColorArrayG[i] = TargetColorArrayG[i];
                }
            }
            else if(ColorArrayG[i] > TargetColorArrayG[i]) {
                ColorArrayG[i]-=deltaVal;
                if(ColorArrayG[i] < TargetColorArrayG[i]) {
                   ColorArrayG[i] = TargetColorArrayG[i];
                }
            }
            if(ColorArrayB[i] < TargetColorArrayB[i]) {
                ColorArrayB[i]+=deltaVal;
                if(ColorArrayB[i] > TargetColorArrayB[i]) {
                   ColorArrayB[i] = TargetColorArrayB[i];
                }
            }
            else if(ColorArrayB[i] > TargetColorArrayB[i]) {
                ColorArrayB[i]-=deltaVal;
                if(ColorArrayB[i] < TargetColorArrayB[i]) {
                   ColorArrayB[i] = TargetColorArrayB[i];
                }
            }
            
            // 輝度調整
            r = ColorArrayR[i] * DIM_LED;
            g = ColorArrayG[i] * DIM_LED;
            b = ColorArrayB[i] * DIM_LED;
         
            brightnessSum = r + g + b;
            
            // 輝度の合計値が上限を超えた場合は補正する
            if(brightnessSum > BRIGHTNESS_MAX) {
                r = (int)(r * ((float)BRIGHTNESS_MAX / (float)brightnessSum));
                g = (int)(g * ((float)BRIGHTNESS_MAX / (float)brightnessSum));
                b = (int)(b * ((float)BRIGHTNESS_MAX / (float)brightnessSum));
            }
            
            neopixel_set_color(
                pixels[i],
                0,
                short(r),
                short(g),
                short(b));
            neopixel_show(pixels[i]);
        }
        //wait_ms(1);
        wait_ms(10); // 500usまで下げてもセンサー共振は起こらないことを確認
    }
}