/**
 *  @file       Main.cpp
 *  @brief      Send 1ch waveform data to PC via USB serial
 *  @date       2015.02.22
 *  @version    1.0.2
 */
#include "mbed.h"
#include "USBSerial.h"
#include "FilterTest.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 SAMPLING_RATE (0.001)   /* A/D sampling rate (1ms) */

#define PACKET_HEADER (0xAA)
#define WAVEFORM_ID   (0x01)
#define PACKET_SIZE   (34)
#define DATA_NUM      (30)
#define BUF_NUM       (2)
#define CNTR_SIZE     (100)
/*  [ Packet format ]                                           */
/*        ----------------                                      */
/*  0x00 | Header byte    | Fixed value (PACKET_HEADER:0xAA)    */
/*  0x01 | Data type      | ID value    (0x01:Waveform)         */
/*  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)|                                     */
/*        ----------------                                      */

Ticker      sampling;           /* Interval timer for A/D sampling */
AnalogIn    wave_in(p20);       /* A/D port */
USBSerial   serial;
FilterTest  filter;

DigitalIn   hpf_on(p28);        /* Hiph pass filter */
DigitalIn   lpf_on(p27);        /* Low pass filter */
DigitalIn   brf_on(p26);        /* Notch (Band reject) filter */
/*  [ 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.                        */

DigitalOut  dbg_pin1(p21);      /* for debug */
DigitalOut  dbg_pin2(p22);      /* for debug */
DigitalOut  dbg_led1(LED1);     /* for debug */
DigitalOut  dbg_led2(LED2);     /* for debug */

int32_t idx, toggle, send_flag;
uint8_t cntr;
uint8_t buf[BUF_NUM][PACKET_SIZE];


/** Initialize buffer
 *  @param      tgl    buffer toggle index
 *  @param      ctr    packet counter
 */
void init_buf(int32_t tgl, uint8_t ctr)
{
    int i;
    if(tgl>BUF_NUM) {
        return;
    }
    buf[tgl][0] = PACKET_HEADER;
    buf[tgl][1] = WAVEFORM_ID;
    buf[tgl][2] = ctr;              /* Packet counter */
    buf[tgl][3] = DATA_NUM;         /* Payload size */
    idx = 4;                        /* Start index of waveform */
    for(i=4; i<PACKET_SIZE; i++) {  /* Initialize array just in case */
        buf[tgl][i] = 0;
    }
}

/** Interval timer for read A/D value
 */
void ad_sampling()
{
    int32_t wav_temp;
    dbg_pin1 = ON;      /* Calculation time for filter for debug */
    dbg_led1 = hpf_on;  /* High pass filter enable status for debug */
    dbg_led2 = lpf_on;  /* Low pass filter enable status 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 );
    wav_temp = (wav_temp > INT16_MAX) ? INT16_MAX : wav_temp;   /* Clip ceiling */
    wav_temp = (wav_temp < INT16_MIN) ? INT16_MIN : wav_temp;   /* Clip floor */
    buf[toggle][idx] = (uint8_t)((wav_temp >> 8 ) & BYTE_MASK); /* MSB side (big endian) */
    idx++;
    buf[toggle][idx] = (uint8_t)(wav_temp & BYTE_MASK);         /* LSB side */
    idx++;

    dbg_pin1 = OFF; /* for debug */

    /* Switch buffer */
    if(idx >= PACKET_SIZE) {    /* Counter reached */
        toggle = !toggle;
        cntr = (cntr + 1 ) % CNTR_SIZE;
        init_buf(toggle, cntr);
        send_flag = ON;          /* Set flag */
    }
}

/** 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_pin2 = ON;   /* for debug */
        serial.writeBlock (p_buf, size);    /* Send data via USB */
        dbg_pin2 = OFF;  /* for debug */
        return true;
    } else {
        return false;
    }
}

/** Main function
 */
int main()
{
    /* Initialization */
    idx = 0;
    toggle = 0;
    cntr = 0;
    send_flag = 0;
    init_buf(toggle, cntr);
    init_buf(!toggle, cntr);

    dbg_pin1 = OFF;
    dbg_pin2 = OFF;
    dbg_led1 = LED_OFF;
    dbg_led2 = LED_OFF;

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

    /* Main loop */
    while(1) {
        if(send_flag != OFF) {
            /* Send data */
            send_packet(buf[!toggle], (uint16_t)PACKET_SIZE);

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