/**
 *  @file       Main.cpp
 *  @brief      Send 1ch waveform and FFT data to PC via USB serial
 *  @date       2015.02.24
 *  @version    1.0.2
 */
#include "mbed.h"
#include "USBSerial.h"
#include "FilterTest.h"
#include "FftTest.h"

#define ON      (1)
#define OFF     (0)

#define LED_ON  (0)
#define LED_OFF (1)

#define BYTE_MASK       (0xFF)
#define INT16_MAX       (32767)
#define INT16_MIN       (-32768)
#define UINT16_MAX      (65535)
#define UINT16_MIN      (0)

#define SAMPLING_RATE   (0.001)     /* A/D sampling rate (1kHz) */
#define DEC_COUNT       (5)         /* Decimation count (sampling 200Hz)*/

#define PACKET_HEADER   (0xAA)
#define WAVEFORM_ID     (0x01)
#define FFT_ID          (0x02)
#define PACKET_SIZE     (34)        /* 64 -> 34 for PC to receive waveform data smoothly */
#define PAYLOAD_SIZE    (30)        /* PACKET_SIZE - 4(Header size) */
#define WAV_BUF_NUM     (2)         /* Double buffer */
#define WV_PKT_CNTR_NUM (100)
/*  [ Packet format ]                                               */
/*        ----------------                                          */
/*  0x00 | Header byte    | Fixed value (PACKET_HEADER:0xAA)        */
/*  0x01 | Data type      | ID value    (0x01:Waveform, 0x02:FFT)   */
/*  0x02 | Packet counter | Count value (0to99 < CNTR_SIZE:100)     */
/*  0x03 | Payload size   | Fixed value (DATA_NUM:30)               */
/*  0x04 | Data0 (MSBside)| Short type  (signed, big endian)        */
/*  ...  | ...            |                                         */
/*  0x21 | Data29(LSBside)|                                         */
/*        ----------------                                          */

#define FFT_LENGTH      (256)       /* FFT data length */
#define FFT_INTERVAL    (200)       /* FFT calc interval */
#define FFT_OVERLAP_LEN (56)        /* FFT overlap length */
#define FFT_BUF_NUM     (2)         /* Double buffer */
#define FFT_SEND_LEN    (90)        /* FFT send data length */


Ticker      sampling;               /* Interval timer for A/D sampling */
AnalogIn    wave_in(p20);           /* Waveform input */
USBSerial   serial;
FilterTest  filter;
FftTest     fft;

/* DIP switch for filter ON/OFF */
DigitalIn   hpf_on(p28);
DigitalIn   lpf_on(p27);
DigitalIn   brf_on(p26);
/*  [ DIP switch ]                                  */
/*    TG-LPC11U35-501         +3.3V                 */
/*  | CN1         CN2 |        ---                  */
/*  |     mbed BD     |         |                   */
/*  |              13 | p28 -/ -+  Hiph pass filter */
/*  |              14 | p27 -/ -+  Low pass filter  */
/*  |              15 | p26 -/ -+  Notch filter     */
/*  |                 |                             */
/*  pull-down is default as below.                  */
/*  http://developer.mbed.org/handbook/DigitalIn    */
/*  It says "By default, the DigitalIn is setup     */
/*          with an internal pull-down resistor."   */
/*  Better to set the pins to pull-up mode and the  */
/*  switch connected to GND.                        */

/* For debug pins */
DigitalOut  dbg_p21(p21);
DigitalOut  dbg_p22(p22);
DigitalOut  dbg_led1(LED1);
DigitalOut  dbg_led2(LED2);

/* Variables */
int32_t wav_dec_cntr;                   /* Decimation counter */

uint8_t wav_pkt_buf[WAV_BUF_NUM][PACKET_SIZE];  /* Waveform packet buffer */
int32_t wav_pkt_idx;                    /* Packet buffer index */
int32_t wav_tgl;                        /* Packet buffer toggle */
int32_t wav_send_flag;                  /* Waveform sending flag */
uint8_t wav_pkt_cntr;                   /* Waveform packet counter */

float fft_buf[FFT_BUF_NUM][FFT_LENGTH]; /* FFT data buffer */
uint8_t pkt_buf_fft[PACKET_SIZE] = {0}; /* FFT packet buffer */
int32_t fft_dat_idx;                    /* FFT buffer index */
int32_t fft_tgl;                        /* FFT buffer toggle */
int32_t fft_send_flag;                  /* FFT sendign flag */

float fft_out_re[FFT_LENGTH] = {0.0f};  /* FFT result real */
float fft_out_im[FFT_LENGTH] = {0.0f};  /* FFT result imaginary */
uint16_t fft_res[FFT_SEND_LEN] = {0};   /* FFT result */

/** Initialize waveform packet buffer
 *  @param      tgl    buffer wav_tgl index
 *  @param      ctr    packet counter
 */
void init_wav_pkt_buf(int32_t tgl, uint8_t ctr)
{
    int i;
    if(tgl>WAV_BUF_NUM) {
        return;
    }
    wav_pkt_buf[tgl][0] = PACKET_HEADER;
    wav_pkt_buf[tgl][1] = WAVEFORM_ID;
    wav_pkt_buf[tgl][2] = ctr;              /* Packet counter */
    wav_pkt_buf[tgl][3] = PAYLOAD_SIZE;     /* Payload size */
    wav_pkt_idx = 4;                        /* Start index of waveform */
    for(i=4; i<PACKET_SIZE; i++) {
        wav_pkt_buf[tgl][i] = 0;
    }
}

/** Set FFT packet buffer
 *  @param      buf    Source data buffer
 *  @param      ctr    packet counter
 */
void set_fft_pkt_buf(uint16_t* buf, uint8_t ctr)
{
    int i;
    pkt_buf_fft[0] = PACKET_HEADER;
    pkt_buf_fft[1] = FFT_ID;
    pkt_buf_fft[2] = ctr;              /* Packet counter */
    pkt_buf_fft[3] = PAYLOAD_SIZE;     /* Payload size */
    i=0;
    while(i<PAYLOAD_SIZE) {
        pkt_buf_fft[i + 4] = (uint8_t)((buf[i / 2] >> 8 ) & BYTE_MASK);
        i++;
        pkt_buf_fft[i + 4] = (uint8_t)(buf[i / 2] & BYTE_MASK);
        i++;
    }
}

/** Interval timer for read A/D value
 */
void ad_sampling()
{
    int32_t wav_temp;
    dbg_led2 = lpf_on;  /* for debug */

    /* Read and filter data */
    wav_temp = (int32_t)filter.calc( (double)(wave_in.read_u16() - INT16_MAX), hpf_on, lpf_on, brf_on );

    /* Store data */
    wav_dec_cntr = (wav_dec_cntr + 1) % DEC_COUNT;
    if(wav_dec_cntr == 0) { /* Decimation */

        dbg_p21 = ON;  /* for debug */
        /* Waveform */
        wav_temp = (wav_temp > INT16_MAX) ? INT16_MAX : wav_temp;
        wav_temp = (wav_temp < INT16_MIN) ? INT16_MIN : wav_temp;
        wav_pkt_buf[wav_tgl][wav_pkt_idx] = (uint8_t)((wav_temp >> 8 ) & BYTE_MASK);
        wav_pkt_idx++;
        wav_pkt_buf[wav_tgl][wav_pkt_idx] = (uint8_t)(wav_temp & BYTE_MASK);
        wav_pkt_idx++;

        if(wav_pkt_idx >= PACKET_SIZE) {    /* Counter reached */
            wav_tgl = !wav_tgl;
            wav_pkt_cntr = (wav_pkt_cntr + 1 ) % WV_PKT_CNTR_NUM;
            init_wav_pkt_buf(wav_tgl, wav_pkt_cntr);
            wav_send_flag = ON;          /* Set flag */
        }

        /* FFT */
        fft_buf[fft_tgl][fft_dat_idx + FFT_OVERLAP_LEN] = (float)wav_temp;
        if(fft_dat_idx >= (FFT_INTERVAL - FFT_OVERLAP_LEN)) {
            fft_buf[!fft_tgl][fft_dat_idx + FFT_OVERLAP_LEN - FFT_INTERVAL] = (float)wav_temp;
        }

        fft_dat_idx = (fft_dat_idx + 1) % FFT_INTERVAL;
        if(fft_dat_idx == 0) {
            fft_tgl = !fft_tgl;
            fft_send_flag = ON;          /* Set flag */
        }

        dbg_p21 = OFF; /* for debug */
    }
}

/** Send data packet
 *  @param      p_buf   Data array
 *  @param      size    Data length
 */
bool send_packet(uint8_t *p_buf, uint16_t size)
{
    if(serial.writeable()) {
        dbg_p22 = ON;   /* for debug */
        serial.writeBlock (p_buf, size);    /* Send data via USB */
        dbg_p22 = OFF;  /* for debug */
        return true;
    } else {
        return false;
    }
}

/** Main function
 */
int main()
{
    int i;
    uint16_t fft_temp;

    /* Initialization */
    wav_dec_cntr = 0;

    init_wav_pkt_buf(wav_tgl, wav_pkt_cntr);
    init_wav_pkt_buf(!wav_tgl, wav_pkt_cntr);
    wav_pkt_idx = 0;
    wav_tgl = 0;
    wav_send_flag = OFF;
    wav_pkt_cntr = 0;

    fft_dat_idx = 0;
    fft_tgl = 0;
    fft_send_flag = OFF;

    dbg_p21 = OFF;
    dbg_p22 = OFF;
    dbg_led1 = LED_OFF;
    dbg_led2 = LED_OFF;

    /* Start interval timer */
    sampling.attach(&ad_sampling, SAMPLING_RATE);

    /* Main loop */
    while(1) {
        /* Waveform */
        if(wav_send_flag != OFF) {
            /* Send data */
            send_packet(wav_pkt_buf[!wav_tgl], (uint16_t)PACKET_SIZE);

            /* Disable interrupt */
            __disable_irq();
            wav_send_flag = OFF;    /* Clear flag */
            __enable_irq();
        }

        /* FFT */
        if(fft_send_flag != OFF) {
            dbg_led1 = LED_ON;      /* for debug */

            /* Calculate FFT and normalized amplitude spectrum */
            fft.apply_window(fft_buf[!fft_tgl], fft_buf[!fft_tgl]);
            fft.calc_fft(fft_buf[!fft_tgl], fft_out_re, fft_out_im);
            fft.calc_power(fft_out_re, fft_out_im, fft_out_re, FFT_SEND_LEN);
            fft.calc_amplitude(fft_out_re, fft_out_re, FFT_SEND_LEN);
            fft.norm_amplitude(fft_out_re, FFT_SEND_LEN);
            for(i=0; i<FFT_SEND_LEN; i++) {
                fft_temp = (fft_out_re[i] > UINT16_MAX) ? UINT16_MAX : (uint16_t)fft_out_re[i];
                fft_temp = (fft_out_re[i] < UINT16_MIN) ? UINT16_MIN : fft_temp;
                fft_res[i] = fft_temp;
            }

            /* Send data */
            i=0;
            while((PAYLOAD_SIZE * i) < FFT_SEND_LEN * sizeof(fft_res[0])) {
                set_fft_pkt_buf((fft_res + (PAYLOAD_SIZE/sizeof(fft_res[0])) * i), i);
                send_packet(pkt_buf_fft, (uint16_t)PACKET_SIZE);
                i++;
            }

            /* Disable interrupt */
            __disable_irq();
            fft_send_flag = OFF;    /* Clear flag */
            __enable_irq();

            dbg_led1 = LED_OFF;      /* for debug */
        }
    }
}
