/**********************************************************************************************//**
  @file       pixl_sensor.c
  @date       $Date$
  @revision   $Revision$

  @brief      Profile Soil Sensor Interface
**************************************************************************************************/

#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 BOARD_HEADER
#include "cavalier_board_cfg_rev2.h"
#include "sys_tick.h"
#include "ucs.h"
#include "Monitor.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 "crc.h"
#include "radio_api.h"
#include "analog_sensors.h"
#include "pixl_sensor.h"
#include "pixl_intfc.h"
#include "cavalier_intfc.h"

#define SENS_BUF_SIZE               256
#define DIG_SENSOR_TIMEOUT          1200

#define MB_MAX_NUM_SENSORS          1
#define MB_MAX_CMDS                 3

#define PRESSURE_SENSOR_SUBTYPE     1
#define DENDROMETER_SENSOR_SUBTYPE  2
#define INFRARED_SENSOR_SUBTYPE     3
#define FROST_SENSOR_SUBTYPE        4
#define FLOW_MOTOR_SENSOR_SUBTYPE   5

DetailedAQUADataWHdr_t mb_data[ MB_MAX_NUM_SENSORS*MB_MAX_CMDS ];
DetailedPIXLDataWHdr_t pixl_data[ PIXL_MAX_NUM_SENSORS ];
DetailedDAFEDataWHdr_t dafe_data[ PIXLN_MAX_NUM_SENSORS ];

uint8_t     pixl_buf[4][36];

struct aquamesh a;
struct aquamesh {
    uint8_t nodeid[5];
    int16_t temp[6];
    int16_t moist[6];
};

//uint32_t conv24to32( PckdDetailedDataPoint_t * dp );

// aquacheck declarations
void    aqua_zero(struct aquamesh * a);
void    read_aquacheck (struct aquamesh *a);
Result_t aqua_temp (struct aquamesh *a);
Result_t aqua_moist (struct aquamesh *a);
Result_t aqua_nodeid(struct aquamesh * a);
void    aqua_command (uint8_t *request, int16_t request_size);
Result_t aqua_response (uint8_t *response, int16_t response_size);
Result_t check_crc (uint8_t *buf, int16_t buflen);
int16_t ModRTU_CRC (uint8_t *buf, int16_t buflen);

bool_t          good_PIXL_CRCs;
bool_t          good_DAFE_CRC;
bool_t          good_FLOW_CRC;
bool_t          PIXL_found;
bool_t          DAFE_found;
bool_t          AQUA_found;
bool_t          FLOW_found;
uint8_t         max_flow_sens;
uint8_t         *pt_data;
uint8_t         pkt_buffer[SENS_BUF_SIZE];
uint16_t        good_crc = 0;
uint16_t        bad_crc = 0;

Result_t read_sensors(void) {
    AQUA_found = FALSE;
    PIXL_found = FALSE;
    DAFE_found = FALSE;
    FLOW_found = FALSE;

    // set baud rate to 9,600 baud and flush port...
    Usart_Init(HAL_UART_BR_9600);
    CLR_RS422_nRE_PIN();        // receiver RS422 turned on
    CLR_RS422_DE_PIN();  // turn off transmitter
    HalUARTRxFlush( PIXL_SERIAL_PORT );  // empty any pending bytes from earlier
    ana_powerOn();                      // turn on the power to the sensor(s)
    debug_printf("\t\tLooking for PIXL/DAFE/FLOW Sensor(s)\n");
    dig_sensors();

    if (PIXL_found || FLOW_found) {
        CLR_RS422_DE_PIN(); // transmit enable RS422 turned off
        SET_RS422_nRE_PIN();        // receiver RS422 turned off
        ana_powerOff();  // done reading the sensor(s), turn off the power
        return RESULT_SUCCESS;
    }

    if (DAFE_found) {
        milli_wait(600);  // if DAFE was found, we didn't timeout, and need more time for AQ
    }
    // power is still on from above, and it's been at least a second, so aqua should respond
    // set baud rate to 19,200 baud and flush port...
    Usart_Init(HAL_UART_BR_19200);
    HalUARTRxFlush( PIXL_SERIAL_PORT );  // empty any pending bytes from earlier
    debug_printf("\t\tLooking for an AquaCheck Sensor \n");


//    milli_wait(1000);
    aqua_nodeid(&a); // try reading MODBUS Node ID
    if (AQUA_found) { // if found, then there is an aquacheck
        debug_printf("\t\t\tAquacheck found with NodeID: %02x:%02x:%02x:%02x:%02x\n",
               a.nodeid[0], a.nodeid[1], a.nodeid[2], a.nodeid[3], a.nodeid[4]);
        aqua_moist(&a);
        aqua_temp(&a);
        show_aquacheck();
    } else {
        debug_printf("\t\t\tif expected, and not found: check DB9 loopback plug\n");
    }
    if (!(DAFE_found || PIXL_found || AQUA_found || FLOW_found)) {
    //     Cavalier is a repeater
        debug_printf("\t\tNo sensors found: so Cavalier is a Repeater\n");
    }
    //     Turn Off power, turn off RS422 transceiver, return success
    CLR_RS422_DE_PIN(); // transmit enable RS422 turned off
    SET_RS422_nRE_PIN();        // receiver RS422 turned off
    ana_powerOff();
    return RESULT_SUCCESS;
}

void show_aquacheck(void) {
    float moist[6], temp[6];
    uint16_t i;

    for (i = 0; i < 6; i++)
        moist[i] = ((float) ((a.moist[i] - 1600) / 320.));
    debug_printf("\t\t\t\tMoist: %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", \
           moist[0], moist[1], moist[2], moist[3], moist[4], moist[5]);
    for (i = 0; i < 6; i++)
        temp[i] = ((float) (a.temp[i] / 256.)) - 50.;
    debug_printf("\t\t\t\tTemp:  %4.3f, %4.3f, %4.3f, %4.3f, %4.3f, %4.3f\n", \
                 temp[0], temp[1], temp[2], temp[3], temp[4], temp[5]);
}

Result_t aqua_nodeid(struct aquamesh * a)
{
// routine to read aquacheck nodeID using MODBUS over RS485 on unit #2
// checks CRC and handles timeouts
// fills in data structure if successful reads without CRC errors
    uint8_t buf[80];
    uint8_t nodeid_req[] = "\x02\x00\x00\x10\x00\x50";
    int16_t i, misses = 0;

    do {
        /* aqua nodeid request, simple output */
        aqua_command((uint8_t *) &nodeid_req, sizeof(nodeid_req)-1);
        /* aqua nodeid response, simple noncanonical input */
        if (aqua_response((uint8_t *) &buf, 12) == RESULT_FAIL) {
            if (++misses == 4) {
                return RESULT_FAIL;
            } else {
                milli_wait(50); // one twentieth of a second
            }
        }
    } while (check_crc(buf, 12) == RESULT_FAIL);

    AQUA_found = 1;
    for (i = 0; i < 5; i++)
        a->nodeid[i] = buf[5+i];

    return RESULT_SUCCESS;
}

Result_t aqua_temp (struct aquamesh * a)
{
// routine to read aquacheck temperature using MODBUS over RS485 on unit #2
// checks CRC and handles timeouts
// fills in data structure if successful reads without CRC errors
    uint8_t buf[80];
    uint8_t temp_req[] = "\x02\x04\x00\x64\x00\x10\xb0\x2a";
    int16_t i, misses = 0;

    do {
        /* aqua temperature request, simple output */
        aqua_command((uint8_t *) &temp_req, sizeof(temp_req)-1);
        /* aqua temperature response, simple noncanonical input */
        if (aqua_response((uint8_t *) &buf, 17) == RESULT_FAIL) {
            if (++misses == 4) {
                return RESULT_FAIL;
            } else {
                milli_wait(50); // one twentieth of a second
            }
        }
    } while (check_crc(buf, 17) == RESULT_FAIL);
    // transfer temperature into structure
    for (i = 0; i < 6; i++)
        a->temp[i] = buf[3+(i<<1)]<<8 | buf[4+(i<<1)];

    return RESULT_SUCCESS;
}

Result_t aqua_moist(struct aquamesh * a)
{
// routine to read aquacheck moisture using MODBUS over RS485 on unit #2
// checks CRC and handles timeouts
// fills in data structure if successful reads without CRC errors
    uint8_t buf[80];
    uint8_t moist_req[] = "\x02\x04\x00\x00\x00\x10\xf1\xf5";
    int16_t i, misses = 0;

    do {
        /* aqua moisture request, simple output */
        aqua_command((uint8_t *) &moist_req, sizeof(moist_req)-1);
        /* aqua moisture response, simple noncanonical input */
        if (aqua_response((uint8_t *) &buf, 17) == RESULT_FAIL) {
            if (++misses == 4) {
                return RESULT_FAIL;
            } else {
                milli_wait(50); // one twentieth of a second
            }
        }
    } while (check_crc(buf, 17) == RESULT_FAIL);
    /* assign moisture to structure */
    for (i = 0; i < 6; i++)
        a->moist[i] = ((buf[3+(i<<1)]<<8) | (buf[4+(i<<1)]));

    return RESULT_SUCCESS;
}

void aqua_command(uint8_t *request, int16_t request_size)
{
// Sends MODBUS command over RS485
    SET_RS422_DE_PIN(); // transmit enable RS422 turned on
    SET_RS422_nRE_PIN();   // receiver RS422 turned off
    while (request_size--) {
        Usart_TX(*request++);
    }
    milli_wait(3);
    CLR_RS422_DE_PIN(); // transmit enable RS422 turned off
    CLR_RS422_nRE_PIN();   // receiver RS422 turned on
}

Result_t aqua_response(uint8_t *response, int16_t response_size)
{
// Collects MODBUS response, or timesouts if no response
// if correct length response (no CRC or data check), return RESULT_SUCCESS
// if failure (timeout), return RESULT_FAIL
    uint8_t ch;
    int16_t i, rdlen;
    int16_t misses = 0;

    for (i = 0; i < response_size; ) {
        rdlen = HalUARTRead(MONITOR_SERIAL_PORT, &ch, 1);
        if (rdlen == 1) {
            response[i++] = ch;
            misses = 0;
        } else {
            milli_wait(20);
            misses++;
        }
        if (misses > 60) {
            return RESULT_FAIL;
        }
    }

    return RESULT_SUCCESS;
}

/**********************************************************************************************//**
 * @brief   Populate AquaCheck Send Soil Sensor SubPacket Data
 *
 * @param
 *
 * @return  1= if more data to send (won't all fit)
 ***************************************************************************************************/
uint8_t aqua_populateSubPacket( uint8_t **pkt_pt, uint16_t *bytes_rem )
{
    uint16_t    len;
    uint8_t     i;
    SoilSensorDataSubPacketHdr_t * hdr_pt = (SoilSensorDataSubPacketHdr_t *) *pkt_pt;
    uint8_t     *data_pt;
    uint16_t    temp, moist;

    // Calculate subpacket length
    len = sizeof(SoilSensorDataSubPacketHdr_t);
    for( i=0; i<3; i++ ) { // each modbus command has it's own slot
        len += sizeof( DetailedAQUADataWHdr_t);
    }
    if( len > *bytes_rem ) { // No room at the inn...
        return 1;       // Indicate we have mode data that still needs sending
    }

    // just for good measure
    memset( *pkt_pt, 0, len );

//    Packet captured on a Dalmatian, for AquaCheck data from cav rev 1.5.6.0
//    33 3c 33                            ... header: match these values: 0x33, 0x3c, 0x33
//    aa cc bb ee dd                      ... header: match these values: 0xaa, 0xcc, 0xbb, 0xee, 0xdd
//    02 04 12 50                         ... create this as a constant: 0x02, 0x04, 0x12, 0x50
//    0f a2 0f 38 0d 3c 23 88 11 9d 0d 17 ... a.moist
//    c2 73                               ... (unchecked crc): pad with two bytes: 0x00, 0x00
//    02 04 12 51                         ... create this as a constant: 0x02, 0x04, 0x12, 0x51
//    48 20 48 20 48 20 48 10 48 10 47 e0 ... a.temp
//    49 6b                               ... (unchecked crc): pad with two bytes: 0x00, 0x00
//    02 00 12 52                         ... create this as a constant: 0x02, 0x00, 0x12, 0x52
//    10 02 00 00 03 0f 52 b5 e3 10       ... create by adding 0x10, 0x02, a.nodeid, 0x00, 0x00, 0x00
//    47 e0                               ... (unchecked crc): pad with two bytes: 0x00, 0x00

    hdr_pt->subpkt_type = SUBTYPE_AquaData; // 0x33
    hdr_pt->subpkt_len = len - 2*sizeof(uint8_t); // maintain consistency with other uploads
    hdr_pt->num_sensors = 0x33;  // number of multi-bus commands expected, actual in upper, lower nibble
    hdr_pt->samp_delay = 0xaa;      // ***TBD
    hdr_pt->sss_fwver  = 0xbbcc;     // ***TBD
    hdr_pt->sss_hwver  = 0xddee;     // ***TBD

    data_pt = *pkt_pt + sizeof(SoilSensorDataSubPacketHdr_t);

// moisture data
    *data_pt++ = 0x02;
    *data_pt++ = 0x04;
    *data_pt++ = 0x12;
    *data_pt++ = 0x50;
    for (i = 0; i < 6; i++) {
        moist = a.moist[i];
        *data_pt++ = (uint8_t) (moist >> 8);
        *data_pt++ = (uint8_t) (moist & 0xff);
    }
    *data_pt++ = 0x00;
    *data_pt++ = 0x00;

// temperature data
    *data_pt++ = 0x02;
    *data_pt++ = 0x04;
    *data_pt++ = 0x12;
    *data_pt++ = 0x51;
    for (i = 0; i < 6; i++) {
        temp = a.temp[i];
        *data_pt++ = (uint8_t) (temp >> 8);
        *data_pt++ = (uint8_t) (temp & 0xff);
    }
    *data_pt++ = 0x00;
    *data_pt++ = 0x00;

// nodeid data
    *data_pt++ = 0x02;
    *data_pt++ = 0x00;
    *data_pt++ = 0x12;
    *data_pt++ = 0x52;
    *data_pt++ = 0x10;
    *data_pt++ = 0x02;
    *data_pt++ = a.nodeid[0];
    *data_pt++ = a.nodeid[1];
    *data_pt++ = a.nodeid[2];
    *data_pt++ = a.nodeid[3];
    *data_pt++ = a.nodeid[4];
    *data_pt++ = 0x00;
    *data_pt++ = 0x00;
    *data_pt++ = 0x00;

    // Update parameters
    *bytes_rem -= len;
    *pkt_pt += len;
    return 0;  // no more data to send
}

Result_t check_crc(uint8_t * buf, int16_t buflen)
{
// the last two bytes are the crc, and before that, the buflen-2 bytes
// are the values to check against.  calculate the crc, and compare to
// the last two bytes.  if a good crc return 0, else return -1;
    uint16_t crc;

    crc = ModRTU_CRC(buf, buflen-2);
    return ((((crc & 0xff) == buf[buflen-2]) && ((crc>>8) == buf[buflen-1])) ? RESULT_SUCCESS : RESULT_FAIL);
}

int16_t ModRTU_CRC(uint8_t * buf, int16_t buflen)
{
// Calculate CRC for MODBUS buffer
    uint16_t crc = 0xFFFF;
    uint16_t i, pos;

    for (pos = 0; pos < buflen; pos++) {
        crc ^= (unsigned int) buf[pos];          // XOR byte into least sig. byte of crc

        for (i = 8; i != 0; i--) {    // Loop over each bit
            if ((crc & 0x0001) != 0) {      // If the LSB is set
                crc >>= 1;                    // Shift right and XOR 0xA001
                crc ^= 0xA001;
            } else                            // Else LSB is not set
                crc >>= 1;                    // Just shift right
        }
    }
  // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
  return crc;
}

void aqua_zero(struct aquamesh * a)
{
// zero all fields of the aqua data structure
    int16_t i;

    for (i = 0; i < 5; i++)
        a->nodeid[i] = 0;
    for (i = 0; i < 6; i++) {
        a->temp[i] = 0;
        a->moist[i] = 0;
    }
}

Result_t dig_sensors(void) {
    uint8_t digital_retries;
    uint16_t byte_cnt;

    digital_retries = 3;
    while (digital_retries--) {
        byte_cnt = read_digital_sensor();
        max_flow_sens = 0;
        parse_digital_sensor(byte_cnt);
        if (good_PIXL_CRCs && good_DAFE_CRC && good_FLOW_CRC) {
            digital_retries = 0;
        } else {
            debug_printf("%8ld: Bad Digital Sensor CRC, retrying: %d\n", (unsigned long) tick_GetUptime(), digital_retries);
        }
    }
    return (digital_retries != 0) ? RESULT_SUCCESS : RESULT_FAIL;
}

uint16_t read_digital_sensor(void)
{
    uint8_t rxByte;
    uint16_t timeout = 0;
    uint8_t rc;
    uint16_t byte_cnt;

    CLR_RS422_nRE_PIN();
    HalUARTRxFlush( PIXL_SERIAL_PORT );  // empty any pending bytes from earlier
    ana_powerOn();                      // turn on the power to the sensor
    milli_wait(250);

    timeout = DIG_SENSOR_TIMEOUT;  // PIXL responds in about 500 ms; pressure later, at about 1200 ms.  have to wait for pressure if no PIXL
    byte_cnt = 0;
    while (((rc = HalUARTRead( PIXL_SERIAL_PORT, &rxByte, 1 ) > 0) || (timeout-- > 0)) && (byte_cnt < SENS_BUF_SIZE)) {
        if (rc != 0) {
            pkt_buffer[byte_cnt++] = rxByte;
            timeout = 500;
        }
        milli_wait(1);
    }
    return byte_cnt;
}

void parse_digital_sensor(uint16_t byte_cnt)
{
    uint16_t i, pkt_len;

    // parsing sensor packet buffer, decoding each sub-frame and checking CRCs
    good_PIXL_CRCs = 1;
    good_DAFE_CRC  = 1;
    good_FLOW_CRC  = 1;

    i = 0;
    while (i < byte_cnt) {
        while ((pkt_buffer[i++] != 0x7e) && (i < byte_cnt)) { // looking for start of frame
            ;
        }
        if (pkt_buffer[i-1] == 0x7e) {
            pkt_len = pkt_buffer[i];
            if (pkt_len > 6) {
                process_digital_sensor(&pkt_buffer[i]);
                i += pkt_len;  // advance buffer by packet length
            }
            i++; // one more for crc, or seeking SOF after invalid len
        }
    }
}

void process_digital_sensor (uint8_t *pkt_buffer)
{
    uint8_t sensor_subtype;
    uint8_t flow_sensor_num;

//    print_digital_sensor_buf(&pkt_buffer[0]);

    if ((pkt_buffer[5] & 0xf0) == 0x00) { // PIXL
        PIXL_found = 1;
        process_pixl_sensor(&pkt_buffer[0]);
        memcpy(&pixl_buf[(pkt_buffer[5] & 0xf)][0], &pkt_buffer[0], pkt_buffer[0]+1);
#if 1
        if ((pkt_buffer[5] & 0x0f) == 0x00) {
            debug_printf("\t\t\tPIXL channel: 0");
        } else {
            debug_printf(", %x", pkt_buffer[5] & 0xf);
        }
        if ((pkt_buffer[5] & 0x0f) == 0x03) {
            debug_printf("\n");
        }
#endif
    }
    if ((pkt_buffer[5] & 0xf0) == 0x10) { // DAFE
        DAFE_found = 1;
        process_dafe_sensor(&pkt_buffer[0]);
        sensor_subtype = pkt_buffer[5] & 0x0F;
        if (sensor_subtype == FLOW_MOTOR_SENSOR_SUBTYPE) {
            FLOW_found = 1;
            good_FLOW_CRC = good_DAFE_CRC;
            DAFE_found = 0;
        }
#if 1
        switch (sensor_subtype) {
            case PRESSURE_SENSOR_SUBTYPE:
                printf("\t\t\tDAFE with Pressure\n");
                break;
            case INFRARED_SENSOR_SUBTYPE:
                printf("\t\t\tDAFE with Infrared\n");
                break;
            case FROST_SENSOR_SUBTYPE:
                printf("\t\t\tDAFE with Frost\n");
                break;
            case DENDROMETER_SENSOR_SUBTYPE:
                printf("\t\t\tDAFE with Dendrometer\n");
                break;
            case FLOW_MOTOR_SENSOR_SUBTYPE:
                printf("\t\t\tFlow Counter/Motor Sensor\n");
                break;
            default:
                printf("\t\t\tUnknown Type: %x\n", sensor_subtype);
        }
#endif
        if (pkt_buffer[5] == 0x15) { // FLOW
            flow_sensor_num = (pkt_buffer[4]&0xf0)>>4;
            max_flow_sens++;
            debug_printf("Number of Flow Sensors found: %d\n", max_flow_sens);
            memcpy(&pixl_buf[flow_sensor_num][0], &pkt_buffer[0], pkt_buffer[0]+1);

            print_digital_sensor_buf(&pixl_buf[flow_sensor_num][0]);
    #if 1
            if ((pkt_buffer[5] & 0x0f) == 0x00) {
                debug_printf("\t\t\tFLOW channel: 0");
            } else {
                debug_printf(", %x", (pkt_buffer[4]&0xf0)>>4);
            }
    #endif
        }
    }
}
#if 0
uint32_t conv24to32( PckdDetailedDataPoint_t * dp )
{
    uint32_t rc2, rc1, rc0, rc;

    rc2  = (uint32_t) dp->data[2];
    rc2 <<= 16;
    rc1  = (uint32_t) dp->data[1];
    rc1 <<= 8;
    rc0  = (uint32_t) dp->data[0];
    rc = rc2 | rc1 | rc0;

    return rc;
}
#endif

void process_pixl_sensor (uint8_t *pkt_buffer)
{
    uint16_t i, pkt_len;
    uint8_t  crc;
    uint8_t  slot;

    slot = pkt_buffer[5] & 0x0f;
    pixl_data[slot].hdr.frame_slot = slot;

    pkt_len = pkt_buffer[0];
    crc = compute_crc8(0, &pkt_buffer[1], pkt_len);

    pt_data = (uint8_t *) &pixl_data[slot];
    for (i = 0; i < pkt_len; i++) {
        *pt_data++ = pkt_buffer[i];
    }
#if 0
    debug_printf("\t\t\tfs: %d, fwv: %d.%d.%d, hw: 0x%04x, temp: %d.%dC, 50MHz_up: %6lu, 50MHz_lo: %6lu\n", pixl_data[slot].hdr.frame_slot,
            pixl_data[slot].hdr.fwver.maj, pixl_data[slot].hdr.fwver.min, pixl_data[slot].hdr.fwver.rev, pixl_data[slot].hdr.hwver,
            pixl_data[slot].data.temper/16, ((pixl_data[slot].data.temper%16)*625/10),
            (uint32_t) conv24to32( &pixl_data[slot].data.f50mhz_up[1] ),
            (uint32_t) conv24to32( &pixl_data[slot].data.f50mhz_low[1] )
            );
//    debug_printf("\t\t\tPacket CRC in slot %d: 0x%02x: ", pixl_data[pkt_buffer[5] & 0x0f].hdr.frame_slot, pkt_buffer[pkt_len+1]);
#endif
    if ((pkt_buffer[pkt_len+1] == crc)) {
        good_crc++;
    } else {
        bad_crc++;
        good_PIXL_CRCs = FALSE;
    }
}

void process_dafe_sensor (uint8_t *pkt_buffer)
{
    uint16_t i, pkt_len;
    uint8_t  crc;

    pkt_len = pkt_buffer[0];
    crc = compute_crc8(0, &pkt_buffer[1], pkt_len);

    pt_data = (uint8_t *) &dafe_data;
    for (i = 0; i < pkt_len; i++) {
        *pt_data++ = pkt_buffer[i];
    }
    if ((pkt_buffer[pkt_len+1] == crc)) {
        good_crc++;
    } else {
        bad_crc++;
        good_DAFE_CRC = FALSE;
        good_FLOW_CRC = FALSE;
    }
}

void print_digital_sensor_buf (uint8_t *pkt_buffer)
{
    uint16_t i, pkt_len;

    pkt_len = pkt_buffer[0];
    debug_printf("\t\t\t");
    for (i = 0; i <= (pkt_len); i++) {
        debug_printf("%02x ", pkt_buffer[i]);
        if ((i % 16) == 15) {
            debug_printf("\n\t\t\t");
        }
    }
    if ((i % 16) != 0) {
        debug_printf("\n");
    }
}


/**********************************************************************************************//**
 * @brief   Populate Send Soil Sensor SubPacket Data
 *
 * @param
 *
 * @return  1= if more data to send (won't all fit)
 ***************************************************************************************************/
uint8_t pixl_populateSubPacket( uint8_t **pkt_pt, uint16_t *bytes_rem )
{
    uint16_t    len = 0;
    uint8_t     i;
    SoilSensorDataSubPacketHdr_t * hdr_pt = (SoilSensorDataSubPacketHdr_t *) *pkt_pt;
    uint8_t     *data_pt;

    len = sizeof(SoilSensorDataSubPacketHdr_t); // size of header
    for( i=0; i<4; i++ ) { //initialized to num_sensors priorly
        len += (uint8_t) pixl_buf[i][0];  // adding in PIXL data
        len += sizeof(uint8_t);           // don't forget the length byte in the Pixl data
    }

    if( len > *bytes_rem ) { // No room at the inn...
        return 1;       // Indicate we have mode data that still needs sending
    }

    // zero structure, just for good measure
    memset( *pkt_pt, 0, len );

    hdr_pt->subpkt_type = SUBTYPE_PixlData;
    hdr_pt->subpkt_len  = len - 2*sizeof(uint8_t);
    hdr_pt->num_sensors = 0x44; // sensors expected, reporting
    hdr_pt->samp_delay  = 0;      // ***TBD
    hdr_pt->sss_fwver   = 0;      // ***TBD
    hdr_pt->sss_hwver   = 0;      // ***TBD

    data_pt  = *pkt_pt;
    data_pt += sizeof(SoilSensorDataSubPacketHdr_t); // skip over header before data copy
    for( i=0; i<4; i++ ) {
        memcpy( data_pt, &pixl_buf[i][0], (pixl_buf[i][0]+sizeof(uint8_t)) );
        data_pt += (uint8_t) pixl_buf[i][0];
        data_pt++; // include the length byte in the calculation
    }

    // Update parameters
    *bytes_rem -= len;
    *pkt_pt += len;

    //ok, all good.  Indicate where or not there are additional samples to send
    return(1);
}

/**********************************************************************************************//**
 * @brief   Populate FLOW connected SubPacket Data
 *
 * @param
 *
 * @return  1= if more data to send (won't all fit)
 ***************************************************************************************************/
uint8_t flow_populateSubPacket( uint8_t **pkt_pt, uint8_t num_of_flow_sens, uint16_t *bytes_rem )
{
    uint16_t    len;
    SoilSensorDataSubPacketHdr_t * hdr_pt = (SoilSensorDataSubPacketHdr_t *) *pkt_pt;
    uint8_t     *data_pt;
    uint8_t     i;

    // Calculate subpacket length
    len = sizeof(SoilSensorDataSubPacketHdr_t);
    for (i = 0; i < num_of_flow_sens; i++) {
         len += (uint8_t) pixl_buf[i][0];        // indicated sub-packet size
         len += sizeof(uint8_t);              // include length byte as part of sub-packet
    }

    if( len > *bytes_rem ) { // No room at the inn...
        return 1;       // Indicate we have mode data that still needs sending
    }

    // just for good measure
    memset( *pkt_pt, 0, len );

    hdr_pt->subpkt_type = SUBTYPE_FlowData;
    hdr_pt->subpkt_len  = len - 2*sizeof(uint8_t);
    hdr_pt->num_sensors = (num_of_flow_sens << 4) | num_of_flow_sens;
    hdr_pt->samp_delay  = 0;     // ***TBD
    hdr_pt->sss_fwver   = 0;     // ***TBD
    hdr_pt->sss_hwver   = 0;     // ***TBD

    data_pt  = *pkt_pt;
    data_pt += 8; // skip over 8 byte header
    for( i=0; i< num_of_flow_sens; i++ ) {
        memcpy( data_pt, &pixl_buf[i][0], pixl_buf[i][0]+sizeof(uint8_t) );
        data_pt += (uint8_t) pixl_buf[i][0];      // indicated sub-packet size
        data_pt += sizeof(uint8_t);            // include length byte as part of sub-packet
    }

    // Update parameters
    *bytes_rem -= len;
    *pkt_pt += len;

    //ok, all good.  Indicate where or not there are additional samples to send
    return( 0 );
}

/**********************************************************************************************//**
 * @brief   Populate DAFE connected SubPacket Data
 *
 * @param
 *
 * @return  1= if more data to send (won't all fit)
 ***************************************************************************************************/
uint8_t dafe_populateSubPacket( uint8_t **pkt_pt, uint16_t *bytes_rem )
{
    uint16_t    len;
    SoilSensorDataSubPacketHdr_t * hdr_pt = (SoilSensorDataSubPacketHdr_t *) *pkt_pt;
    uint8_t     *data_pt;

    // Calculate subpacket length
    len = sizeof(SoilSensorDataSubPacketHdr_t) + sizeof(DetailedDAFEDataWHdr_t);

    if( len > *bytes_rem ) {
        // No room at the inn...
        return 1;       // Indicate we have mode data that still needs sending
    }

    // just for good measure
    memset( *pkt_pt, 0, len );

    hdr_pt->subpkt_type = SUBTYPE_AnalogData;
    hdr_pt->subpkt_len = len - 2*sizeof(uint8_t);
    hdr_pt->num_sensors = 0x11;
    hdr_pt->samp_delay = 0;     // ***TBD
    hdr_pt->sss_fwver = 0;      // ***TBD
    hdr_pt->sss_hwver = 0;      // ***TBD

    data_pt = *pkt_pt +sizeof(SoilSensorDataSubPacketHdr_t);
    memcpy( data_pt, &dafe_data[0], sizeof(DetailedDAFEDataWHdr_t) );
    ((DAFEHeader_t *)data_pt)->length = sizeof(DetailedDAFEDataWHdr_t) -1;
    ((DAFEHeader_t *)data_pt)->type_slot |= (DAFE_DetailedSensor << 4);;
    data_pt += sizeof(DetailedDAFEDataWHdr_t);

    // Update parameters
    *bytes_rem -= len;
    *pkt_pt += len;

    //ok, all good.  Indicate where or not there are additional samples to send
    return( 0 );
}
