float y;

unsigned long cur_time = 0;
unsigned long start_time = 0;
unsigned long end_time = 0;
float latency = 0;


const char* GESTURES[] = {
  "ADL",
  "Fall"
};


// Globals, used for compatibility with Arduino-style sketches.
namespace
{
    tflite::ErrorReporter* error_reporter = nullptr;
    const tflite::Model* tflModel = nullptr;
    tflite::MicroInterpreter* tflInterpreter = nullptr;
    TfLiteTensor* tflInputTensor = nullptr;
    TfLiteTensor* tflOutputTensor = nullptr;
    int inference_count = 0;

    const int kModelArenaSize = 1024;
    const int kTensorArenaSize = 120 * 1024;
    uint8_t tensor_arena[kTensorArenaSize];
    //byte tensorArena[kTensorArenaSize] __attribute__((aligned(16)));

}  // namespace


list<int> listAccX;
list<int> listAccY;
list<int> listAccZ;
list<int> listGyroX;
list<int> listGyroY;
list<int> listGyroZ;

void setup()
{
    // ·Î±ë ¼³Á¤
    static tflite::MicroErrorReporter micro_error_reporter;
    error_reporter = &micro_error_reporter;

    // ¸ðµ¨À» »ç¿ë °¡´ÉÇÑ µ¥ÀÌÅÍ ±¸Á¶¿¡ ¸ÅÇÎ, ÀÌ´Â º¹»ç³ª ÆĽÌÀ» Æ÷ÇÔÇÏÁö ¾Ê´Â ¸Å¿ì °¡º­¿î ÀÛ¾÷
    tflModel = tflite::GetModel(g_model);
    if (tflModel->version() != TFLITE_SCHEMA_VERSION) {
        TF_LITE_REPORT_ERROR(error_reporter,
            "Model provided is schema version %d not equal "
            "to supported version %d.",
            tflModel->version(), TFLITE_SCHEMA_VERSION);
        return;
    }
    
    // ÇÊ¿äÇÑ ¸ðµç Op ±¸ÇöÀ» °¡Á®¿Â´Ù. 
    static tflite::AllOpsResolver tflOpsResolver;

    // ¸ðµ¨À» ½ÇÇàÇÒ ÀÎÅÍÇÁ¸®ÅÍ ºôµå
    static tflite::MicroInterpreter static_interpreter(
        tflModel, tflOpsResolver, tensorArena, kTensorArenaSize, error_reporter);
    tflInterpreter = &static_interpreter;

    // ¸ðµ¨ ÅÙ¼­¸¦ tensor_arenaÀÇ ¸Þ¸ð¸®¿¡ ÇÒ´çÇÑ´Ù. 
    TfLiteStatus allocate_status = tflInterpreter->AllocateTensors();
    if (allocate_status != kTfLiteOk) {
        TF_LITE_REPORT_ERROR(error_reporter, "AllocateTensors() failed");
        return;
    }

    // ¸ðµ¨ÀÇ ÀԷ°ú Ãâ·Â¿¡ ´ëÇÑ Æ÷ÀÎÅÍ È¹µæ
    tflInputTensor = tflInterpreter->input(0);
    tflOutputTensor = tflInterpreter->output(0);

    // Ãß·ÐÀ» ½ÇÇàÇÑ È½¼ö¸¦ ±â·ÏÇϱâ À§ÇÑ º¯¼ö
    inference_count = 0;

    // ¡Ú ½½¶óÀ̵ù À©µµ¿ì¸¦ À§ÇÑ ¸®½ºÆ® ÃʱâÈ­
    
    int nListSize = listAccX.size();
    for (int i = 0; i < 50; i++)
    {
        listAccX.push_back(332);
        listAccY.push_back(-4936);
        listAccZ.push_back(6279);
        listGyroX.push_back(-5);
        listGyroY.push_back(443);
        listGyroZ.push_back(20);
    }
}


void loop()
{
    //// ¡Ú ¸ðµ¨¿¡ °ø±ÞÇÒ x°ª °è»ê
    //// ÇöÀçÀÇ inference_count¸¦ ÁÖ±â´ç Ã߷РȽ¼ö¿Í ºñ±³ÇÏ¿© 
    //// ¸ðµ¨ÀÌ ÇнÀµÈ ÁöÁ¤ °¡´É¤¾³ª x°ª ¹üÀ§ ³»¿¡¼­ À§Ä¡¸¦ °áÁ¤ÇÏ°í À̸¦ »ç¿ëÇÏ¿© °ªÀ» °è»êÇÑ´Ù.
    //// Áï ¹üÀ§ ³»ÀÇ ÇöÀç À§Ä¡
    //float position = static_cast<float>(inference_count) / static_cast<float>(kInferencesPerCycle);
    //// ¸ðµ¨¿¡ Àü´ÞÇÒ °ª
    //float x_val = position * kXrange; // ¡Ú


    // °è»êÇÑ x°ªÀ» ¸ðµ¨ÀÇ ÀÔ·Â ÅÙ¼­¿¡ ³Ö±â
    int g = 0;

    std::list<int>::iterator it5, it6, it7, it8, it9, it10;

    // ÀÔ·Â 6 x 50 °³
    for (it5 = listAccX.begin(), it6 = listAccY.begin(), it7 = listAccZ.begin(),
        it8 = listGyroX.begin(), it9 = listGyroY.begin(), it10 = listGyroZ.begin();
        it5 != listAccX.end();
        ++it5, ++it6, ++it7, ++it8, ++it9, ++it10)
    {
        tflInputTensor->data.f[g * 6 + 0] = (*it5);
        tflInputTensor->data.f[g * 6 + 1] = (*it6);
        tflInputTensor->data.f[g * 6 + 2] = (*it7);
        tflInputTensor->data.f[g * 6 + 3] = (*it8);
        tflInputTensor->data.f[g * 6 + 4] = (*it9);
        tflInputTensor->data.f[g * 6 + 5] = (*it10);
    }

    g++;

}







// ¸ðµ¨ µ¹¸®±â
if (bSensorReadToggle == true) // 20ms ÆÇ´ÜÁֱ⠸ÂÃß±â À§Çؼ­.
{

    ReadSensorData2(&sData);
    // UART5_Printf("%04d,%04d,%04d,%04d,%04d,%04d,%04d>\r\n", seqNum++, sData.accX, sData.accY, sData.accZ,  sData.gyroX,sData.gyroY,sData.gyroZ);

    txPacket.n1 = (int)(((float)sData.accX / (float)100) + 0.1);
    txPacket.n2 = (int)(((float)sData.accY / (float)100) + 0.1);
    txPacket.n3 = (int)(((float)sData.accZ / (float)100) + 0.1);
    txPacket.n4 = (int)(((float)sData.gyroX / (float)100) + 0.1);
    txPacket.n5 = (int)(((float)sData.gyroY / (float)100) + 0.1);
    txPacket.n6 = (int)(((float)sData.gyroZ / (float)100) + 0.1);

    txPacket.seqNum = seqNum++;
    txPacket.end = 0x3E;
    txPacket.end2 = 0x0D;
    txPacket.end3 = 0x0A;

#if ( PROCESS_TRIGGER == 1)             

    listAccX.push_back((int)sData.accX);
    listAccY.push_back((int)sData.accY);
    listAccZ.push_back((int)sData.accZ);
    listGyroX.push_back((int)sData.gyroX);
    listGyroY.push_back((int)sData.gyroY);
    listGyroZ.push_back((int)sData.gyroZ);



    int nListSize = listAccX.size();
    if (nListSize > 50)
    {
        listAccX.pop_front();
        listAccY.pop_front();
        listAccZ.pop_front();
        listGyroX.pop_front();
        listGyroY.pop_front();
        listGyroZ.pop_front();
    }


    loop();

    TfLiteStatus invoke_status = tflInterpreter->Invoke();
    // Ãß·ÐÀ» ½ÇÇàÇÏ°í ¿À·ù°¡ ÀÖÀ¸¸é º¸°í
    if (invoke_status != kTfLiteOk)
    {
        TF_LITE_REPORT_ERROR(error_reporter, "Invoke failed on x: %f\n",
            static_cast<double>(x1));
    }


    for (int i = 0; i < 2; i++) {
        if (i == 1)
            start_time = clock();

        y = tflOutputTensor->data.f[i];
        end_time = clock();

        UART5_Printf("%c : %lf\n", GESTURES[i], tflOutputTensor->data.f[i])

        if (y > 0.7)
            retValCounter++;
        else 
            retValCounter = 0;
       


        if (retValCounter >= 2)  // 1.0005 0.99888
        {
            // Æ®¸®°Å ¸®ÅÏ
            if (g_bTriggerON == false)
            {
                HAL_GPIO_WritePin(GPIOC, LED_G_Pin, GPIO_PIN_SET);
                HAL_GPIO_WritePin(GPIOC, SHOOT_EN_Pin, GPIO_PIN_SET);

                g_bResetMsgProcess = true;
                {
                    PACKET txPacketFire = { 0x3C, 't','r','i','g','g','e','r','3', 0x3C,0x0D,0x0A };
                    HAL_UART_Transmit(&huart3, (uint8_t*)(&txPacketFire), sizeof(txPacketFire), 1000);
                }

                HAL_GPIO_WritePin(GPIOC, SHOOT_EN_EXT_Pin, GPIO_PIN_SET);

                g_nTriggerONCount = 0;
                g_bResetMsgProcess = false;
                g_bTriggerON = true;
            }
        }

    }
    end_time = clock();
    latency = end_time - start_time;

    UART5_Printf("latency : %04d ms\n\n", latency)

    bSensorReadToggle = !bSensorReadToggle;

}