/***************************************************************************************************
  Filename:       cav_main.c
  Revised:        $Date: $
  Revision:       $Revision: $

   Description:    Cavalier App main() function

**************************************************************************************************/

/***************************************************************************************************
*                                             INCLUDES
***************************************************************************************************/

#include <msp430.h> 
#include <stdio.h>
#include <stdlib.h>
#include "ComTypeDefs.h"
#include "hal_nv_ti.h"
#include "hal_uart_ti.h"
#include "hal_adc12.h"
#include "cavalier_board_cfg_rev2.h"
#include "sys_tick.h"
#include "ucs.h"
#include "Versions.h"
#include "radio_api.h"
#include "pixl_sensor.h"
#include "analog_sensors.h"
#include "sample_mngr.h"
#include "sys_utils.h"
#include "radio_api.h"
#include "cavalier_intfc.h"

/***************************************************************************************************
*                                             ENUMS
***************************************************************************************************/

#define SYSTATE_SLEEPY              0
#define SYSTATE_SAMPLY              1
#define SYSTATE_SENDY               2
#define RING_SIZE                   18

bool_t      reset_event = FALSE;
float       avg_sampling_time;
uint8_t	    sys_state = SYSTATE_SAMPLY;
uint8_t     last_state = 0xff;
uint8_t     new_state = 0;
bool_t      last_radio_state = FALSE;
bool_t      current_radio_state = FALSE;
uint32_t    packets_sampled = 0;
uint16_t    radio_awake_sec = 0;
uint16_t    xbee_hang_alive_counter;
uint16_t    xbee_reset_counter = 0;
uint16_t    err_buffer = 0;
uint32_t    radio_woke_up_time = 0;
uint32_t    last_sample_time = 0;
uint32_t    next_sampling_time = 2;
uint32_t    sampling_time;
uint32_t    toggle_offset = 0;
uint16_t    mesh_sent_successfully = 0;
uint16_t    mesh_failed_to_send = 0;
uint32_t    last_state_time = 0;
uint8_t     cavalier_seq_num = 0;
uint8_t     ota_packet[RING_SIZE][ MAX_CAV_PACKET_SIZE+1 ];
uint16_t    sendPktlen[RING_SIZE];
uint8_t     transmit_fails[RING_SIZE];
uint8_t     pkts_in_ring = 0;
uint16_t    transmit_status;
uint8_t     head = 0;
uint8_t     tail = 0;  // for the transmit buffer
uint8_t     dest[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff }; // start with broadcast, update with ATAG cmd from base
uint32_t    os;
uint32_t    ow;
int8_t      db_val    = 0x00;  // initial value 0x00, until loaded by a reg read
uint16_t    wake_time = 0;
uint16_t    sleep_time = 0;
uint16_t    ms_val = 0x0000;  // initial value 0x0000, until loaded by a reg read
uint16_t    sq_val = 0x0000;  // initial value 0x0000, until loaded by a reg read
uint8_t     sm_val = 0xff;
uint8_t     query_choice = 0;
bool_t      atcmd_pending = FALSE;
bool_t      frame_was_broadcast = FALSE;
bool_t      atag_received = FALSE;
uint8_t     wake_up_task = 0;
bool_t      rev3dot3;
bool_t      do_dhdl_broadcast = FALSE;

extern bool_t   good_PIXL_CRCs;
extern bool_t   good_DAFE_CRC;
extern bool_t   good_FLOW_CRC;
extern bool_t   PIXL_found;
extern bool_t   DAFE_found;
extern bool_t   AQUA_found;
extern bool_t   FLOW_found;
extern uint8_t  max_flow_sens;

/***************************************************************************************************
*                                             PROTOTYPES
***************************************************************************************************/

// main declarations
void do_state_sleepy(void);
void do_state_samply(void);
void do_state_sendy(void);
void main_init_stuff(void);
void check_radio_woke_up(void);
Result_t mesh_send_packet( const addr64 *ieee_addr_be, void *data, uint16_t length );
Result_t dequeueSensorPacket( void );
Result_t enqueueSensorPacket( void );
void query_mesh_regs(void);
extern Result_t set_dhdl_broadcast(void);

/**********************************************************************************************//**
* @brief   You da main()!
* @param   none
* @return  - DOES NOT RETURN
**************************************************************************************************/
int main( void )
{
    main_init_stuff();  // initialize all system variables, and setup hardware for operation

    while( TRUE ) {
        if( timerTickEvent.tick_1s == 1 ) { // check 1s tick bit
            timerTickEvent.tick_1s = 0;     //    & clear the tick event
            RESET_WATCHDOG();       // reset watchdog for 16 second timeout
            check_radio_woke_up();  // transitioning on 1s boundaries

            switch (sys_state) {
                case SYSTATE_SLEEPY:
                    do_state_sleepy();
                    break;
                case SYSTATE_SAMPLY:
                    do_state_samply();
                    break;
                case SYSTATE_SENDY:
                    do_state_sendy();
                    break;
            }
            // determine next state for master state machine
            if (radio_isAwake()) {
                sys_state = SYSTATE_SENDY;
            } else if (tick_GetUptime() >= next_sampling_time) {
                sys_state = SYSTATE_SAMPLY;
            } else {
                sys_state = SYSTATE_SLEEPY;
            }
        }
    }	// while(TRUE)  // Shouldn't never not no not get here.
}

void do_state_sleepy(void)
{
    if( sys_state == SYSTATE_SLEEPY ) { // Snooze fest if didn't state transition to another state
       LPM3;
        _nop();
    }
}

void check_radio_woke_up(void)
{
    timerTickEvent.radio_woke_up = 0;
    last_radio_state = current_radio_state;
    current_radio_state = radio_isAwake();
    if (!last_radio_state && current_radio_state && (tick_GetUptime() > 10)) { // give time for powerup, and a first sample
        radio_woke_up_time = tick_GetUptime();
        debug_printf("%8ld: Up, to Tx: %d\n", (unsigned long) tick_GetUptime(), pkts_in_ring);
    }
    clear_spi_if();
}

void do_state_sendy(void)
{
    radio_awake_sec = tick_GetUptime() - radio_woke_up_time;

    if ((radio_awake_sec > (wake_time+50)) && (tick_GetUptime() > sampling_time*4)) {
        debug_printf("\t\t\tMesh Awake Too Long!!!  Resetting Mesh\n");
        debug_printf("\t\t\tWake Time: %d, Radio Awake: %d\n", wake_time, radio_awake_sec);
        radio_woke_up_time = tick_GetUptime() + wake_time;  // don't reset quickly, give it time to complete
        // not sure mesh init is smart, I will do a warm reset instead right now
        FORCE_WATCHDOG(); // force a watchdog timeout, and warm reset
    } else {
        if (radio_isAwake()) {
            radio_tick();
            radio_awake_sec = tick_GetUptime() - radio_woke_up_time;
            if ((wake_up_task != 5) && !do_dhdl_broadcast) { // get the dB within the first hour of operation
                if ((radio_awake_sec < (wake_time-2)) && (radio_awake_sec >= 1)) {
                    if (pkts_in_ring > 0) {
                        if (dequeueSensorPacket() == RESULT_SUCCESS) {
                            mesh_sent_successfully++;  // successful transmit end-to-end
                            blink_led(1, 75, 100);
                        }
                    }
                }
            } else {
                if (radio_awake_sec == 1) {  // arbitrary time, with time to complete atcmd
                    if (do_dhdl_broadcast) {
                        if (set_dhdl_broadcast() == RESULT_SUCCESS)
                            do_dhdl_broadcast = FALSE;
                    } else {
                        debug_printf("%8ld: Query Mesh regs using ATCMDs, choice: %d\n",
                            tick_GetUptime(), query_choice);
                        query_mesh_regs();
                    }
                }
            }
        } else {
            wake_up_task = (wake_up_task + 1) % 8; // every eight awake cycle, read mesh stats instead
            debug_printf("%8ld: Down, sleep\n", (unsigned long) tick_GetUptime());
            debug_printf("%8ld: %ld samples: avg rate: %6.3f, exp rate: %6.3f\n",
                (unsigned long) tick_GetUptime(), (unsigned long) (packets_sampled),
                avg_sampling_time, (float) sampling_time);
            debug_printf("%8ld: Mesh: Tx: %d, reTx: %d, drop: %d\n",
                (unsigned long) tick_GetUptime(), mesh_sent_successfully, mesh_failed_to_send, err_buffer);
        }
    }
}

void query_mesh_regs(void)
{
    uint8_t timeout;

    timeout = 3;
    atcmd_pending = TRUE;
    switch (query_choice) {
        case 0:
            atcmd_db_read();
            break;
        case 1:
            atcmd_os_read();
            break;
        case 2:
            atcmd_ow_read();
            break;
    }
    while ((--timeout) && (atcmd_pending)) {
        radio_tick();
    }
    if (!atcmd_pending)  // only advance to next stats if last one successfully read
        query_choice = (query_choice +1) % 3;
}

void do_state_samply(void)
{
#ifdef BUILDTYPE_Debug
    if (packets_sampled != 0)
        avg_sampling_time = (float) tick_GetUptime()/packets_sampled;  // for diagnostics
    else
        avg_sampling_time = (float) 0;
#endif
    sampling_time = (wake_time + sleep_time); // sampling time: calculated from sleep/wake times
    debug_printf("%8ld: Sample\n", (unsigned long) tick_GetUptime());

    if ((radio_woke_up_time + wake_time + 2) > ((uint32_t) tick_GetUptime())
            && (((uint32_t) tick_GetUptime()) > sampling_time)) { // is radio on near sampling time
        debug_printf("%8ld: \tMesh near, adjust timing (radio_woke_up + wake_time + 2): %ld)\n",
                     (unsigned long) tick_GetUptime(), (radio_woke_up_time + wake_time + 2) );
        if (toggle_offset == 0) {
            toggle_offset = (sampling_time >> 1);  // shift half of the wake/sleep cycle to move timing
        } else {
            toggle_offset = 0;
        }
    } else {
        read_sensors();     // read the sensors and load the intermediate data buffers
        packets_sampled++;  // one more sensor packet ready
        blink_sensors();  // indicate sensors found on LED
        if (enqueueSensorPacket() == RESULT_FAIL) {
            debug_printf("%8ld: No room to enqueue packet, warm reset\n", \
                   (unsigned long) tick_GetUptime());
            FORCE_WATCHDOG(); // force a warmstart by forcing a watchdog timeout
        }
    }

    if (tick_GetUptime() < (sampling_time>>1)) {
        next_sampling_time = sampling_time;
    } else {
        last_sample_time = (tick_GetUptime() / sampling_time) * sampling_time;
        next_sampling_time = last_sample_time + sampling_time + toggle_offset;
    }
    debug_printf("%8ld: next Sample: %ld\n",
         (unsigned long) tick_GetUptime(), (unsigned long) next_sampling_time);
}

Result_t dequeueSensorPacket( void )
{
    uint16_t len;
    uint8_t retries;

    if ((pkts_in_ring > (RING_SIZE-3)) && radio_isAwake()) {
        radio_woke_up_time = tick_GetUptime() + wake_time;  // give it time to complete
// not sure the mesh_init() is yet a smart idea...  will debug soon
//        mesh_init();  // ring nearly full - let's reset mesh and see if that makes it better
    }

    if ((pkts_in_ring != 0) && (radio_isAwake())) {
        len = sendPktlen[head];
        if( len <= sizeof(CavalierOTAHeader_t) ) {   // Check if we any real data to send
            return RESULT_FAIL;  // not enough data in buffer for legitimate packet
        } else {
            if ((dest[0] == 0x00) && (dest[1] == 0x00)
                    && (dest[2] == 0x00) && (dest[3] == 0x00)
                    && (dest[4] == 0x00) && (dest[5] == 0x00)
                    && (dest[6] == 0xff) && (dest[7] == 0xff)) {
                debug_printf("%8ld: \tDeQ BC : %02x\n", \
                       (unsigned long) tick_GetUptime(), ota_packet[head][2]);
                frame_was_broadcast = TRUE;
                atag_received = FALSE;
            } else {
                debug_printf("%8ld: \tDeQ Uni: %02x\n", \
                       (unsigned long) tick_GetUptime(), ota_packet[head][2]);
                frame_was_broadcast = FALSE;
            }

            // library function was failing, as it sent data in different sends to driver
            //   header was different than payload.  It caused occasional underruns at radio.
            // so wrote this function to aggregate all the data into one push to the radio.
            transmit_status = 0x8000;
            milli_wait(20+(rand() % 24) * 40);  // dither random 0.020 to 0.940 seconds
            mesh_send_packet( (const addr64 *) &dest, ota_packet[head], len );

            retries = 40; // 2 second timeout (40 times 50 ms)
            while (((transmit_status == 0x8000)
                    || (frame_was_broadcast && !atag_received))
                    && (retries--)) {
                radio_tick();
                milli_wait(50);
            }
            if (((transmit_status == 0x0000) && !frame_was_broadcast)
                    || (frame_was_broadcast && atag_received)) {
                if (frame_was_broadcast) {
                    do_dhdl_broadcast = TRUE; // reset DH, DL when atag received
                    atag_received = FALSE;
                    frame_was_broadcast = FALSE;
                }
                debug_printf("%8ld: \t\tSuccess Tx\n", (unsigned long) tick_GetUptime());
                transmit_fails[head] = 0;  // I don't think I need this, as I reset it otherwise

                pkts_in_ring--;  // one less packet in the ring
                head = (head + 1) % RING_SIZE;  // move the head of the queue
                return RESULT_SUCCESS;
            } else {
                if (frame_was_broadcast) {
                    transmit_status = 0x4000;
                    do_dhdl_broadcast = TRUE;
                }
                debug_printf("%8ld:\t\tFailed Tx, fails on buf: %02x: %d, status: %04X\n",
                       (unsigned long) tick_GetUptime(),
                       head, transmit_fails[head], transmit_status);
                transmit_fails[head]++;
                mesh_failed_to_send++;
                if ((transmit_status == 0x2502) || (transmit_status == 0x0002)) {
                    // route not found, discovery: force route lookup by sending a broadcast
                    dest[0] = dest[1] = dest[2] = dest[3] = dest[4] = dest[5] = 0x00;
                    dest[6] = dest[7] = 0xff;
                }
                if (transmit_fails[head] > (RING_SIZE-1)) {
                    // may be a corrupt buffer in ring, and transmit failing accordingly
                    // do I restart sending broadcasts??  I do that for route discovery errors
                    debug_printf("%8ld:\t\tMany fails on buf: %02x\n",
                                 (unsigned long) tick_GetUptime(), head);
                    debug_printf("\t\t\tNot doing anything re-active about it yet\n");
                }
                return RESULT_FAIL;
            }
        }
    }
    return RESULT_FAIL; // nothing in ring to send
}

Result_t enqueueSensorPacket( void )
{
    uint8_t *pkt_pt;
    uint16_t bytes_rem;
    CavalierOTAHeader_t *hdr_pt;

    if ((pkts_in_ring <= RING_SIZE)) {  // is there room to enqueue another packet?
        pkt_pt = ota_packet[tail] + sizeof(CavalierOTAHeader_t);
        hdr_pt = (CavalierOTAHeader_t *)ota_packet[tail];
        bytes_rem = sizeof( ota_packet[tail] ) -sizeof(CavalierOTAHeader_t);

        // Populate Packet Header
        hdr_pt->fw_ver = FIRMWAREVERSION_MAJOR   << 12 | FIRMWAREVERSION_MINOR << 8 |
                         FIRMWAREVERSION_REVISION << 4 | FIRMWAREVERSION_BUILD;
        hdr_pt->seq_num =  cavalier_seq_num++;    // packet sequence number
        hdr_pt->hw_ver = rev3dot3 ? 0x33 : 0x32;  // current hardware revision

        // Populate Subpacket Payloads
        if (good_DAFE_CRC && DAFE_found) {
            dafe_populateSubPacket( &pkt_pt, &bytes_rem );  // add DAFE data
        }
        if (good_FLOW_CRC && FLOW_found) {
            flow_populateSubPacket( &pkt_pt, max_flow_sens, &bytes_rem );// add FLOW data
        }
        if (good_PIXL_CRCs && PIXL_found) {
            pixl_populateSubPacket( &pkt_pt, &bytes_rem );  // add PIXL data
        }
        if (AQUA_found) {
            aqua_populateSubPacket( &pkt_pt, &bytes_rem );  // add AquaCheck data
        }
        // debug_printf("%8ld:       Adding battery voltage, temperature\n",
        //    (unsigned long) tick_GetUptime());
        ana_populateSubPacket( &pkt_pt, &bytes_rem ); // add the xbee battery data

        sendPktlen[tail] = MAX_CAV_PACKET_SIZE - bytes_rem + 1;
        debug_printf("%8ld: EnQ, %02x\n", (unsigned long) tick_GetUptime(), tail);

        pkts_in_ring++;  // one more packet in the ring
        transmit_fails[tail] = 0;  // reset fail counter on the tail
        tail = (tail + 1) % RING_SIZE;  // move the tail to use for the next frame enqueue
        return RESULT_SUCCESS;
    } else {
        return RESULT_FAIL;      // no room in the packet queue
    }
}

void main_init_stuff(void)
{
    STOP_WATCHDOG(); // hold watchdog until init complete
    HAL_BOARD_INIT();  // Initialization for board related stuff

    __disable_interrupt();  // drivers not initialized yet, let's keep interrupts off
    ana_powerOff();     // turning off sensor power
    _waitForPowerup();  // wait until power is above 2.8 volts for operating

    rev3dot3 = get_cav_rev();  // read voltage level on sensor power pin to determine revision
    debug_printf("%8ld: Cav Sensor, 18 Sep 18, WEJ, Cav rev: 3.%s, FW rev: %d.%d.%d.%d\n",
           (unsigned long) tick_GetUptime(), rev3dot3 ? "3" : "2", FIRMWAREVERSION_MAJOR,
           FIRMWAREVERSION_MINOR, FIRMWAREVERSION_REVISION, (FIRMWAREVERSION_BUILD | (rev3dot3 ? 1: 0)));

    tick_Init();  // start one second timer tick
    __enable_interrupt();

    mesh_init();  // this is time sensitive, needs to occur before mesh sends its sync poll message
    blink_rev();  // blink the minor revision version on LED at bootup

    STOP_WATCHDOG(); // make sure clock is being held, because we are changing clock source for watchdog
    RESET_WATCHDOG(); // set watchdog for 16 second timeout
}
