/******************************************************************************
* File name:   main.c
* Version:     1.0.0
* Discription:
*   This is the trial of classification of ADL (Activities of daily living)
*    by means of LSTM.
** History:
*   1.0.0  2018.03.28  Initial coding
*
* Copyright 2018 METOOL Inc. All Rights Reserved
******************************************************************************/

/******************************************************************************
* Included headers
******************************************************************************/
#include "main.h"

/******************************************************************************
* Static variables 
******************************************************************************/
char ACTIVITIES[OUTPUT_UNIT_SIZE][10] = {
    "WALKING  ",
    "WALK_UP  ",
    "WALK_DOWN",
    "SITTING  ",
    "STANDING ",
    "LAYING   "
};

int volatile data_cntr;
int volatile res_idx;
int volatile max_idx;
int32 volatile max_val;
int volatile rdy_flag;

int32 in_data[TEST_DATA_LEN * INPUT_UNIT_SIZE];

int32 X[INPUT_UNIT_SIZE];

int32 IG[HIDDEN_UNIT_SIZE];
int32 FG[HIDDEN_UNIT_SIZE];
int32 OG[HIDDEN_UNIT_SIZE];
int32 CX[HIDDEN_UNIT_SIZE];
int32 CEC[HIDDEN_UNIT_SIZE];
int32 HL[HIDDEN_UNIT_SIZE];

int32 Y[OUTPUT_UNIT_SIZE];

/******************************************************************************
* Function Name: hard_tanh
*******************************************************************************
* Summary:
* This function returns result of tanh of broken line approximation.
*
* Parameters:
*  int32 x  : input value
*
* Return:
*  int32    : result of hard_tanh
*
******************************************************************************/
int32 hard_tanh(int32 x)
{
    // Q12 format
    int32 z = 0;
    if(x < -9625) { z = -4096; } else                           // x < -2.35 : -1.0
    if(x < -4710) { z = ((512 * x) >> DAT_Q_FMT) - 2893; } else // x < -1.15 : 0.125 * x - 0.70625
    if(x < -1638) { z = ((2458 * x) >> DAT_Q_FMT) - 655; } else // x < -0.4  : 0.6   * x - 0.16
    if(x <  1638) { z = x;     } else                           // x < 0.4   : x
    if(x <  4710) { z = ((2458 * x) >> DAT_Q_FMT) + 655; } else // x < 1.15  : 0.6   * x + 0.16
    if(x <  9625) { z = ((512 * x) >> DAT_Q_FMT) + 2893; } else // x < 2.35  : 0.125 * x + 0.70625
                  { z = 4096;  }                                // x >= 2.35 : 1.0
    return z;
}

/******************************************************************************
* Function Name: hard_sigmoid
*******************************************************************************
* Summary:
* This function returns result of sigmoid (logistic) function of broken line approximation.
*
* Parameters:
*  int32 x  : input value
*
* Return:
*  int32    : result of hard_sigmoid
*
******************************************************************************/
int32 hard_sigmoid(int32 x)
{
    int32 z = ((x * 819) >> DAT_Q_FMT) + (1 << (DAT_Q_FMT-1));  // x * 0.2 + 0.5
    z = MAX(0, z);                                              // Clipping 0
    z = MIN((1 << DAT_Q_FMT), z);                               // Clipping 1
    return z;
}

/******************************************************************************
* Function Name: calc_conv
*******************************************************************************
* Summary:
* This function calculates convolution function res = w * x + u * h + b.
*
* Parameters:
*  int32 *res_ptr       : pointer to return value buffer
*  const int32 *w_ptr   : pointer to weight w
*  const int32 *u_ptr   : pointer to weight u
*  const int32 *b_ptr   : pointer to bias b
*
* Return:
*  void
*
******************************************************************************/
void calc_conv(int32 *res_ptr, const int32 *w_ptr, const int32 *u_ptr, const int32 *b_ptr)
{
    int i, j;
    int64 temp1, temp2;
    int32 *x_ptr = X;
    int32 *h_ptr = HL;
    
    for(i=0; i<HIDDEN_UNIT_SIZE; i++) {
        for(j=0, temp1 = 0, x_ptr = X; j<INPUT_UNIT_SIZE; j++) {
            temp1 += (*w_ptr++) * (*x_ptr++);         // Never exceeding int32
        }
        for(j=0, temp2 = 0, h_ptr = HL; j<HIDDEN_UNIT_SIZE; j++) {
            temp2 += (*u_ptr++) * (*h_ptr++);         // Never exceeding int32
        }
        *res_ptr++ = (((temp1 + temp2) >> DAT_Q_FMT) + *b_ptr++);
    }
    return;
}

/******************************************************************************
* Function Name: timer_isr
*******************************************************************************
* Summary:
* Timer interrupt routine. Loading data and predict activity.
*
* Parameters:
*  void
*
******************************************************************************/
CY_ISR(timer_isr)
{    
    int i, j;
    int32 *res_ptr;
    const int32 *w_ptr, *u_ptr, *b_ptr;

     Pin_GPIO_2_Write(IO_HIGH);

    // Load ADL data
    data_cntr = (data_cntr < TEST_SAMPLES)? data_cntr : TEST_SAMPLES - 1;
    for(i=0; i<TEST_DATA_SIZE; i++) {
        in_data[i] = ADL_DAT[data_cntr * TEST_DATA_SIZE + i];
    }
    res_idx = data_cntr;
    data_cntr = (data_cntr + 1) % TEST_SAMPLES;

    // Initilize array
    for(i=0; i<HIDDEN_UNIT_SIZE; i++) {
        CEC[i] = 0;
        HL[i] = 0;
    }

    // Start calculating LSTM
    Pin_GPIO_1_Write(IO_HIGH);
    
    // LSTM layer
    for(i=0; i<TEST_DATA_LEN; i++) {
        for(j=0; j<INPUT_UNIT_SIZE; j++) {
            X[j] = in_data[i*INPUT_UNIT_SIZE + j];
        }
        // Input gate
        res_ptr = IG;
        w_ptr = W1 + IN_GATE_INDEX * INPUT_UNIT_SIZE;
        u_ptr = W2 + IN_GATE_INDEX * HIDDEN_UNIT_SIZE;
        b_ptr = B1 + IN_GATE_INDEX;
        calc_conv(res_ptr, w_ptr, u_ptr, b_ptr);        
        
        // CEC candidate
        res_ptr = CX;
        w_ptr = W1 + CEC_IN_INDEX * INPUT_UNIT_SIZE;
        u_ptr = W2 + CEC_IN_INDEX * HIDDEN_UNIT_SIZE;
        b_ptr = B1 + CEC_IN_INDEX;
        calc_conv(res_ptr, w_ptr, u_ptr, b_ptr);

        // Froget gate
        res_ptr = FG;
        w_ptr = W1 + FGT_GATE_INDEX * INPUT_UNIT_SIZE;
        u_ptr = W2 + FGT_GATE_INDEX * HIDDEN_UNIT_SIZE;
        b_ptr = B1 + FGT_GATE_INDEX;
        calc_conv(res_ptr, w_ptr, u_ptr, b_ptr);   

        // Activation function & update CEC
        for(j=0; j<HIDDEN_UNIT_SIZE; j++) {
            IG[j] = hard_sigmoid(IG[j]);
            CX[j] = hard_tanh(CX[j]);
            FG[j] = hard_sigmoid(FG[j]);
            CEC[j] = ((IG[j] * CX[j]) + (FG[j] * CEC[j])) >> DAT_Q_FMT;     // Never exceeding int32
        }

        // Output gate
        res_ptr = OG;
        w_ptr = W1 + OUT_GATE_INDEX * INPUT_UNIT_SIZE;
        u_ptr = W2 + OUT_GATE_INDEX * HIDDEN_UNIT_SIZE;
        b_ptr = B1 + OUT_GATE_INDEX;
        calc_conv(res_ptr, w_ptr, u_ptr, b_ptr);
        
        // Activation function & update LSTM layer
        for(j=0; j<HIDDEN_UNIT_SIZE; j++) {
            OG[j] = hard_sigmoid(OG[j]);
            HL[j] = ((OG[j] * hard_tanh(CEC[j])) >> DAT_Q_FMT);
        }
    }

    // Output layer
    for(i=0; i<OUTPUT_UNIT_SIZE; i++) {
        Y[i] = 0;
        for(j=0; j<HIDDEN_UNIT_SIZE; j++) {
            Y[i] += ((W3[i*HIDDEN_UNIT_SIZE + j] * HL[j]) >> DAT_Q_FMT);
        }
        Y[i] += B2[i];
    }

    Pin_GPIO_1_Write(IO_LOW);
    
    // Predict
    max_idx = 0;
    max_val = Y[0];
    for(i=0; i<OUTPUT_UNIT_SIZE; i++) {
        if(max_val < Y[i]) {
            max_idx = i;
            max_val = Y[i];
        }
    }
    
    Pin_GPIO_2_Write(IO_LOW);

    rdy_flag = TRUE;
}

/******************************************************************************
* Function Name: main
*******************************************************************************
* Summary:
* System entrance point.
* This processes initializing, predicting and sending result via USB serial.
*
* Parameters:
*  void
*
* Return:
*  int
*
******************************************************************************/
int main(void)
{
    char str[128];
    
    // Initiaize
    data_cntr = 0;
    res_idx = 0;
    max_idx = 0;
    max_val = 0;
    rdy_flag = FALSE;

    Pin_GPIO_1_Write(IO_LOW);
    Pin_GPIO_2_Write(IO_LOW);
    isr_1_StartEx(timer_isr);
    Timer_1_Start();

    CyGlobalIntEnable;

    USBUART_1_Start(0u, USBUART_1_5V_OPERATION);
    while(!USBUART_1_GetConfiguration()){};
    USBUART_1_CDC_Init();
        
    for(;;) {
        if(rdy_flag) {
            // Output result to PC via USB serial
            if((res_idx%OUTPUT_UNIT_SIZE) == 0) {
                while(USBUART_1_CDCIsReady() == 0u) {};
                USBUART_1_PutCRLF();
            }
            sprintf(str, "Data %d pred:\t %d %s",
                res_idx, max_idx, ACTIVITIES[max_idx]);
            while(USBUART_1_CDCIsReady() == 0u) {};
            USBUART_1_PutString(str);
            while(USBUART_1_CDCIsReady() == 0u) {};
            USBUART_1_PutCRLF();
                        
            // Clear flag
            rdy_flag = FALSE;
        }
    }
}

/* [] END OF FILE */
