Subversion Repositories Code-Repo

Rev

Rev 160 | Go to most recent revision | Blame | Last modification | View Log | RSS feed

#include <xc.h>
#include <delays.h>
#include <stdio.h>
#include <string.h>
#include "defines.h"
#include "base_INTERRUPTS.h"
#include "base_TIMERS.h"
#include "base_UART.h"
#include "base_I2C.h"
#include "base_SPI.h"
#include "base_ADC.h"
#include "sensor_nfc_PN532.h"
#include "sensor_lux_TSL2561.h"
#include "sensor_temp_BMP085.h"
#include "sensor_gyro_L3G.h"
#include "sensor_accel_LSM303.h"
#include "sensor_rtc_DS3231.h"
#include "display_led_HT16K33.h"
#include "display_oled_ssd1306.h"
#include "display_oled_ssd1331.h"
#include "display_oled_NHD-0216KZW-AB5.h"
#include "comm_xbee.h"

// <editor-fold defaultstate="collapsed" desc="Configuration Bits">
/* --------------------------- Configuration Bits --------------------------- */
/* CONFIG1L @ 0x1FFF8 */
#pragma config CFGPLLEN = ON        // Enable PLL on startup
#pragma config PLLDIV = 3           // Set PPL prescaler to 3 (to get 4MHz)
#pragma config WDTEN = OFF          // Turn off watchdog timer
#pragma config STVREN = OFF         // Stack overflow/underflow reset disabled
#pragma config XINST = OFF          // Turn off extended instruction set

/* CONFIG1H @ 0x1FFF9 */
#pragma config CP0 = OFF            // Program memory is not code-protected

/* CONFIG2L @ 0x1FFFA */
#pragma config CLKOEC = OFF         // CLKO output disabled on RA6 pin
#pragma config SOSCSEL = LOW        // Low Power T1OSC/SOSC circuit selected
#pragma config IESO = ON            // Internal external oscillator switch over disabled
#pragma config OSC = HSPLL          // Use external oscillator (101)
#pragma config FCMEN = OFF          // Fail-safe clock monitor disabled

/* CONFIG2H @ 0x1FFFB */
#pragma config WDTPS = 1            // Watchdog postscaler of 1:1

/* CONFIG3L @ 0x1FFFC */
#pragma config RTCOSC = T1OSCREF    // RTCC uses T1OSC/T1CKI
#pragma config DSBOREN = ON         // Deep sleep BOR enabled
#pragma config DSWDTPS = M2         // Deep sleep watchdog postscaler of 1:2 (36m)
#pragma config DSWDTEN = OFF        // Deep sleep watchdog timer disabled
#pragma config DSWDTOSC = INTOSCREF  // DSWDT clock select uses INTRC

/* CONFIG3H @ 0x1FFFD */
#pragma config PLLSEL = PLL96       // Use 96MHz PLL 4MHz -> 96MHz / 2 = 48MHz
#pragma config ADCSEL = BIT12       // 12-bit ADC
#pragma config MSSP7B_EN = MSK7     // 7-bit address masking mode
#pragma config IOL1WAY = OFF        // IOLOCK bit can be set and cleared as needed

/* CONFIG4L @ 0x1FFFE */
#pragma config WPCFG = ON           // Configuration words page protected

/* CONFIG4H @ 0x1FFFF */
#pragma config WPEND = PAGE_WPFP    // Pages WPFP<6:0> through Configuration Words erase/write protected
#pragma config WPDIS = OFF          // WPFP<6:0>/WPEND region ignored
/* -------------------------------------------------------------------------- */
// </editor-fold>

#if defined(_TEST_UART)
// <editor-fold defaultstate="collapsed" desc="_TEST_UART">
int main() {
    char buffer[100];

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    char output[] = "\r\nBegin Program\r\n";
    DBG_PRINT_MAIN(output, strlen(output));

    while (1) {
        char length = UART1_Read_Buffer((char *) buffer);
        if (length != 0) {
            UART1_WriteS(buffer, length);
        }

        Delay10KTCYx(255);
        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_I2C_MASTER)
// <editor-fold defaultstate="collapsed" desc="_TEST_I2C_MASTER">
void main(void) {
    char length = 0;
    char result = 0;
    char buffer[100];
    char output[64];

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data); // Initialize the I2C handler code

    I2C_Configure_Master(I2C_100KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    while (1) {
        buffer[0] = 0x8;

        I2C_Master_Send(0x24, 1, buffer);
        do {
            result = I2C_Get_Status();
        } while (!result);
        sprintf(output, "S: %X ", result);
        DBG_PRINT_MAIN(output, strlen(output));

        I2C_Master_Recv(0x24, 2);
        do {
            result = I2C_Get_Status();
        } while (!result);
        sprintf(output, "S: %X ", result);
        DBG_PRINT_MAIN(output, strlen(output));
        length = I2C_Read_Buffer(buffer);
        sprintf(output, "L: %d D: ", length);
        DBG_PRINT_MAIN(output, strlen(output));
        for (char i = 0; i < length; i++) {
            sprintf(output, "%c ", buffer[i]);
            DBG_PRINT_MAIN(output, strlen(output));
        }
        sprintf(output, "\r\n");
        DBG_PRINT_MAIN(output, strlen(output));

        I2C_Master_Restart(0x30, 0xBB, 2);
        result = I2C_Get_Status();
        while (!result) {
            result = I2C_Get_Status();
        }
        sprintf(output, "S: %X ", result);
        DBG_PRINT_MAIN(output, strlen(output));
        length = I2C_Read_Buffer(buffer);
        sprintf(output, "L: %d D: ", length);
        DBG_PRINT_MAIN(output, strlen(output));
        for (char i = 0; i < length; i++) {
            sprintf(output, "%c ", buffer[i]);
            DBG_PRINT_MAIN(output, strlen(output));
        }
        sprintf(output, "\r\n");
        DBG_PRINT_MAIN(output, strlen(output));

        Delay10KTCYx(255);
        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_I2C_SLAVE)
// <editor-fold defaultstate="collapsed" desc="_TEST_I2C_SLAVE">
void main(void) {
    char length = 0;
    char result = 0;
    char buffer[100];
    char output[64];

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data); // Initialize the I2C handler code

    I2C_Configure_Slave(0x24);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    while (1) {

        result = I2C_Get_Status();
        while (!result) {
            result = I2C_Get_Status();
        }
        sprintf(output, "S: %X ", result);
        DBG_PRINT_MAIN(output, strlen(output));
        length = I2C_Read_Buffer(buffer);
        sprintf(output, "L: %d D: ", length);
        DBG_PRINT_MAIN(output, strlen(output));
        for (char i = 0; i < length; i++) {
            sprintf(output, "%X ", buffer[i]);
            DBG_PRINT_MAIN(output, strlen(output));
        }
        sprintf(output, "\r\n");
        DBG_PRINT_MAIN(output, strlen(output));

        Delay10KTCYx(255);
        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_SPI)
// <editor-fold defaultstate="collapsed" desc="_TEST_SPI">
void main(void) {
    char length = 0;
    char result = 0;
    char buffer[100];
    char output[64];
    char test[8] = "ASDF123";

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    SPI_DATA spi_data;
    SPI2_Init(&spi_data, SPI2_FOSC_8); // Initialize the SPI module

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    while (1) {

        SPI2_Write(test, 7);
        while (result != 7) {
            length = SPI2_Read_Buffer(buffer);
            if (length) {
                result += length;
            }
        }
        result = 0;

        for (char i = 0; i < result; i++) {
            sprintf(output, "%X ", buffer[i]);
            DBG_PRINT_MAIN(output, strlen(output));
        }
        sprintf(output, "\r\n");
        DBG_PRINT_MAIN(output, strlen(output));

        Delay10KTCYx(255);
        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_ADC)
// <editor-fold defaultstate="collapsed" desc="_TEST_ADC">
void main(void) {
    unsigned int x, y, z;
    char buffer[60];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    SPI_DATA spi_data;
    SPI2_Init(&spi_data, SPI2_FOSC_8); // Initialize the SPI module
    SSD1306_DATA ssd1306_data;
    SSD1306_Init(&ssd1306_data); // Initialize the SSD1331 OLED display (uses SPI2)
    ADC_DATA adc_data;
    ADC_Init(&adc_data, ADC_TAD_20, ADC_FOSC_64_);

    SSD1306_Begin(SSD1306_SWITCHCAPVCC);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(buffer, "\r\nBegin Program\r\n");
    SSD1306_Write_String(buffer, strlen(buffer));

    memset(buffer, 0, 60);
    SSD1306_Clear_Display();
    SSD1306_Display();

    while (1) {
        // ADC read from AN0-AN2 and prints to display
        ADC_Start(ADC_CHANNEL_AN2);
//        SSD1306_Fill_Rect(0, 0, SSD1306_LCDWIDTH, 8, SSD1331_BLACK);
        SSD1306_Set_Cursor(0, 0);
        while (!ADC_Get_Result(&x));
        sprintf(buffer, "X: %u", x);
        SSD1306_Write_String(buffer, strlen(buffer));
        SSD1306_Display();

        ADC_Start(ADC_CHANNEL_AN1);
//        SSD1306_Fill_Rect(0, 8, SSD1306_LCDWIDTH, 8, SSD1331_BLACK);
        SSD1306_Set_Cursor(0, 8);
        while (!ADC_Get_Result(&y));
        sprintf(buffer, "Y: %u", y);
        SSD1306_Write_String(buffer, strlen(buffer));
        SSD1306_Display();

        ADC_Start(ADC_CHANNEL_AN0);
//        SSD1306_Fill_Rect(0, 16, SSD1306_LCDWIDTH, 8, SSD1331_BLACK);
        SSD1306_Set_Cursor(0, 16);
        while (!ADC_Get_Result(&z));
        sprintf(buffer, "Z: %u", z);
        SSD1306_Write_String(buffer, strlen(buffer));
        SSD1306_Display();
    }
}
// </editor-fold>
#elif defined(_TEST_TIMER1_RTC)
// <editor-fold defaultstate="collapsed" desc="_TEST_TIMER1_RTC">
void main(void) {

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    Timer1_Init();

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    LED_BLUE_TRIS = 0;
    LED_RED_TRIS = 0;

    Timer1_Enable();

    while (1) {

    }
}
// </editor-fold>
#elif defined(_TEST_NFC)
// <editor-fold defaultstate="collapsed" desc="_TEST_NFC">
void main(void) {
    char length = 0;
    char output[64];

    // NFC stuff
    NFC_FIRMWARE_VERSION version;
    NFC_TargetDataMiFare cardData[2];
    NFC_TargetDataMiFare cardData_prev[2];

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data); // Initialize the I2C handler code
    NFC_DATA nfc_data;
    NFC_Init(&nfc_data); // Initialize the NFC chip (uses I2C)

    I2C_Configure_Master(I2C_400KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    version = NFC_Get_Firmware_Version();
    while (!version.IC) {
        sprintf(output, "Waiting for NFC board..\r\n");
        DBG_PRINT_MAIN(output, strlen(output));
        Delay10KTCYx(3);
        version = NFC_Get_Firmware_Version();
    }
    sprintf(output, "Found chip PN5%X\r\n", version.IC);
    DBG_PRINT_MAIN(output, strlen(output));
    sprintf(output, "Firmware ver. %d.%d\r\n", version.Ver, version.Rev);
    DBG_PRINT_MAIN(output, strlen(output));
    NFC_SAMConfig();

    memset(cardData, 0, 24);

    while (1) {

        //        // This query will hang until the NFC chip replies (card detected)
        //        length = NFC_readPassiveTargetID(cardData);
        //        if (length) {
        //            DBG_PRINT_MAIN("Cards Found: %u\r\n", length);
        //            DBG_PRINT_MAIN("UID Length: %d bytes\r\n", cardData[0].NFCID_LEN);
        //            DBG_PRINT_MAIN("UID: ");
        //            for (i = 0; i < cardData[0].NFCID_LEN; i++) {
        //                DBG_PRINT_MAIN("%02X ", cardData[0].NFCID[i]);
        //            }
        //            DBG_PRINT_MAIN("\r\n");
        //            if (length == 2) {
        //                DBG_PRINT_MAIN("UID Length: %d bytes\r\n", cardData[1].NFCID_LEN);
        //                DBG_PRINT_MAIN("UID: ");
        //                for (i = 0; i < cardData[1].NFCID_LEN; i++) {
        //                    DBG_PRINT_MAIN("%02X ", cardData[1].NFCID[i]);
        //                }
        //                DBG_PRINT_MAIN("\r\n");
        //            }
        //        }

        //        // This query will hang until the NFC chip replies (card detected)
        //        length = NFC_readPassiveTargetID(cardData);
        //        if (length) {
        //            DBG_PRINT_MAIN("Cards Found: %u\r\n", length);
        //            DBG_PRINT_MAIN("UID Length: %d bytes\r\n", cardData[0].NFCID_LEN);
        //            DBG_PRINT_MAIN("UID: ");
        //            for (i = 0; i < cardData[0].NFCID_LEN; i++) {
        //                DBG_PRINT_MAIN("%02X ", cardData[0].NFCID[i]);
        //            }
        //            DBG_PRINT_MAIN("\r\n");
        //            if (length == 2) {
        //                DBG_PRINT_MAIN("UID Length: %d bytes\r\n", cardData[1].NFCID_LEN);
        //                DBG_PRINT_MAIN("UID: ");
        //                for (i = 0; i < cardData[1].NFCID_LEN; i++) {
        //                    DBG_PRINT_MAIN("%02X ", cardData[1].NFCID[i]);
        //                }
        //                DBG_PRINT_MAIN("\r\n");
        //            }
        //        }

        // This query will not wait for a detection before responding
        length = NFC_Poll_Targets(1, 1, cardData);
        if (!length) {
            memset(cardData_prev, 0, 24);
        } else if (length == 1) {
            if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0) {
                // Do nothing
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[1].NFCID, cardData[0].NFCID_LEN) == 0) {
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            } else {
                sprintf(output, "UID: ");
                DBG_PRINT_MAIN(output, strlen(output));
                for (char i = 0; i < cardData[0].NFCID_LEN; i++) {
                    sprintf(output, "%02X ", cardData[0].NFCID[i]);
                    DBG_PRINT_MAIN(output, strlen(output));
                }
                sprintf(output, "\r\n");
                DBG_PRINT_MAIN(output, strlen(output));
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            }
            memset(&cardData_prev[1], 0, 12);
        } else if (length == 2) {
            if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0 &&
                    memcmp(&cardData[1].NFCID, &cardData_prev[1].NFCID, cardData[1].NFCID_LEN) == 0) {
                // Do nothing
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[1].NFCID, cardData[0].NFCID_LEN) == 0 &&
                    memcmp(&cardData[1].NFCID, &cardData_prev[0].NFCID, cardData[1].NFCID_LEN) == 0) {
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
                memcpy((char *) &cardData_prev[1], (const char *) &cardData[1], 12);
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0) {
                // First card matched
                sprintf(output, "UID2: ");
                DBG_PRINT_MAIN(output, strlen(output));
                for (char i = 0; i < cardData[1].NFCID_LEN; i++) {
                    sprintf(output, "%02X ", cardData[1].NFCID[i]);
                    DBG_PRINT_MAIN(output, strlen(output));
                }
                sprintf(output, "\r\n");
                DBG_PRINT_MAIN(output, strlen(output));
                memcpy(&cardData_prev[1], (const char *) &cardData[1], 12);
            } else if (memcmp(&cardData[1].NFCID, &cardData_prev[1].NFCID, cardData[1].NFCID_LEN) == 0) {
                // Second card matched
                sprintf(output, "UID1: ");
                DBG_PRINT_MAIN(output, strlen(output));
                for (char i = 0; i < cardData[0].NFCID_LEN; i++) {
                    sprintf(output, "%02X ", cardData[0].NFCID[i]);
                    DBG_PRINT_MAIN(output, strlen(output));
                }
                sprintf(output, "\r\n");
                DBG_PRINT_MAIN(output, strlen(output));
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            } else {
                // No match
                sprintf(output, "UID1: ");
                DBG_PRINT_MAIN(output, strlen(output));
                for (char i = 0; i < cardData[0].NFCID_LEN; i++) {
                    sprintf(output, "%02X ", cardData[0].NFCID[i]);
                    DBG_PRINT_MAIN(output, strlen(output));
                }
                sprintf(output, "\r\n");
                DBG_PRINT_MAIN(output, strlen(output));
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
                sprintf(output, "UID2: ");
                DBG_PRINT_MAIN(output, strlen(output));
                for (char i = 0; i < cardData[1].NFCID_LEN; i++) {
                    sprintf(output, "%02X ", cardData[1].NFCID[i]);
                    DBG_PRINT_MAIN(output, strlen(output));
                }
                sprintf(output, "\r\n");
                DBG_PRINT_MAIN(output, strlen(output));
                memcpy((char *) &cardData_prev[1], (const char *) &cardData[1], 12);
            }
        }
    }
}
// </editor-fold>
#elif defined(_TEST_LUX)
// <editor-fold defaultstate="collapsed" desc="_TEST_LUX">
void main(void) {
    char output[64];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data);
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    TSL2561_DATA lux_data;
    LUX_Init(&lux_data, TSL2561_ADDR_FLOAT);

    I2C_Configure_Master(I2C_100KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    LUX_Begin();

    // You can change the gain on the fly, to adapt to brighter/dimmer light situations
//    LUX_Set_Gain(TSL2561_GAIN_0X);   // set no gain (for bright situtations)
    LUX_Set_Gain(TSL2561_GAIN_16X);  // set 16x gain (for dim situations)

    // Changing the integration time gives you a longer time over which to sense light
    // longer timelines are slower, but are good in very low light situtations!
//    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_13MS);  // shortest integration time (bright light)
    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_101MS);  // medium integration time (medium light)
//    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_402MS);  // longest integration time (dim light)

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    while (1) {
        unsigned long lum = LUX_Get_Full_Luminosity();
        unsigned int ir = lum >> 16;
        unsigned int full = lum & 0xFFFF;
        sprintf(output, "IR: %d\r\n", ir);
        DBG_PRINT_MAIN(output, strlen(output));
        sprintf(output, "Visible: %d\r\n", full - ir);
        DBG_PRINT_MAIN(output, strlen(output));
        sprintf(output, "Full: %d\r\n", full);
        DBG_PRINT_MAIN(output, strlen(output));
        sprintf(output, "Lux: %ld\r\n\r\n", LUX_Calculate_Lux(full, ir));
        DBG_PRINT_MAIN(output, strlen(output));

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_BMP)
// <editor-fold defaultstate="collapsed" desc="_TEST_BMP">
void main(void) {
    char output[64];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data);
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    BMP085_DATA bmp_data;
    BMP_Init(&bmp_data);

    I2C_Configure_Master(I2C_400KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    BMP_Begin(BMP085_ULTRAHIGHRES);

    BMP_Read_Temperature();
    BMP_Read_Pressure();
    BMP_Read_Altitude(101592);

    while (1) {
        sprintf(output, "Temp: %f *C\r\n", BMP_Read_Temperature());
        DBG_PRINT_MAIN(output, strlen(output));
        sprintf(output, "Pressure: %ld Pa\r\n", BMP_Read_Pressure());
        DBG_PRINT_MAIN(output, strlen(output));
        sprintf(output, "Altitude: %f meters\r\n", BMP_Read_Altitude(101592));
        DBG_PRINT_MAIN(output, strlen(output));

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_GYRO)
// <editor-fold defaultstate="collapsed" desc="_TEST_GYRO">
void main(void) {
    char output[64];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data);
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    L3G_DATA gyro_data;
    L3G_Init(&gyro_data, L3GD20_DEVICE, L3G_SA0_HIGH);

    I2C_Configure_Master(I2C_100KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    L3G_Begin();
    int x,y,z;
    while (1) {
        L3G_Read(&x, &y, &z);
        sprintf(output, "X: %d  Y: %d  Z: %d\r\n", x, y, z);
        DBG_PRINT_MAIN(output, strlen(output));

        Delay10KTCYx(100);
    }
}
// </editor-fold>
#elif defined(_TEST_ACCEL)
// <editor-fold defaultstate="collapsed" desc="_TEST_ACCEL">
void main(void) {
    char output[64];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data);
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    LSM303_DATA acc_data;
    LSM303_Init(&acc_data, LSM303DLHC_DEVICE, ACC_ADDRESS_SA0_A_LOW);

    I2C_Configure_Master(I2C_100KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    LSM303_Begin();
    int a_x, a_y, a_z, m_x, m_y, m_z;
    while (1) {
        LSM303_Read_Acc(&a_x, &a_y, &a_z);
        LSM303_Read_Mag(&m_x, &m_y, &m_z);
        sprintf(output, "A - X: %d  Y: %d  Z: %d\r\n", a_x, a_y, a_z);
        DBG_PRINT_MAIN(output, strlen(output));
        sprintf(output, "M - X: %d  Y: %d  Z: %d\r\n", m_x, m_y, m_z);
        DBG_PRINT_MAIN(output, strlen(output));

        Delay10KTCYx(100);
    }
}
// </editor-fold>
#elif defined(_TEST_RTC)
// <editor-fold defaultstate="collapsed" desc="_TEST_RTC">
void main(void) {
    char output[64];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data);
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);

    I2C_Configure_Master(I2C_100KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    DS3231_Begin();
    // Sec, Min, Hour, DOW, Day, Month, Year, Mil Time, AM/PM
    DS3231_Set_Time(00, 59, 7, 5, 18, 1, 13, 0, 0);

    char sec, min, hour, day, date, month, year, h_mil, h_am_pm;
    while (1) {
        DS3231_Get_Time(&sec, &min, &hour, &day, &date, &month, &year, &h_mil, &h_am_pm);
        sprintf(output, "%02d:%02d:%02d %s %s - %d/%d/%d (%d)\r\n", hour, min, sec, (h_am_pm) ? "PM" : "AM",
                (h_mil) ? "24H" : "12H", month, date, year, day);
        DBG_PRINT_MAIN(output, strlen(output));
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_LED_BACKPACK)
// <editor-fold defaultstate="collapsed" desc="_TEST_LED_BACKPACK">
void main(void) {
    unsigned int counter = 0;
    char output[64];

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data); // Initialize the I2C handler code
    LED_DATA led_data;
    LED_Init(&led_data); // Initialize the LED backpack (uses I2C);

    I2C_Configure_Master(I2C_400KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    LED_Start();
    LED_Write_Digit_Num(0, 1, 1);
    LED_Write_Digit_Num(1, 2, 0);
    LED_Write_Digit_Num(2, 3, 0);
    LED_Write_Digit_Num(3, 4, 0);
    LED_Write_Display();
    for (char i = 0; i < 15; i++) {
        LED_Set_Brightness(15 - i);
        Delay10KTCYx(100);
    }
    for (char i = 0; i < 15; i++) {
        LED_Set_Brightness(i);
        Delay10KTCYx(100);
    }
    LED_Blink_Rate(HT16K33_BLINK_OFF);

    while (1) {
        LED_Write_Num(counter);
        counter++;
        if (counter > 9999)
            counter = 0;

        //        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_SSD1306_OLED)
// <editor-fold defaultstate="collapsed" desc="_TEST_SDS1306_OLED">
void main(void) {
    char output[64];
    
    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    SPI_DATA spi_data;
    SPI2_Init(&spi_data, SPI2_FOSC_4); // Initialize the SPI module
    SSD1306_DATA ssd1306_data;
    SSD1306_Init(&ssd1306_data); // Initialize the OLED code

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts
    
    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    SSD1306_Begin(SSD1306_SWITCHCAPVCC);

    SSD1306_Display(); // Show splashscreen

    while (1) {
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_DrawLine();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_DrawRect();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_FillRect();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_DrawCircle();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Fill_Circle(SSD1306_LCDWIDTH / 2, SSD1306_LCDHEIGHT / 2, 10, SSD1306_WHITE);
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_DrawRoundRect();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_FillRoundRect();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_DrawTriangle();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_FillTriangle();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Test_DrawChar();
        SSD1306_Display();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1306_Clear_Display();
        SSD1306_Set_Text_Size(1);
        SSD1306_Set_Text_Color(SSD1306_WHITE);
        SSD1306_Set_Cursor(0, 0);
        sprintf(output, "Hello World!\n");
        SSD1306_Write_String(output, strlen(output));
        //        SSD1306_Set_Text_Color_BG(BLACK, WHITE);
        unsigned int i = 65535;
        sprintf(output, "%u %d\n", i, i);
        SSD1306_Write_String(output, strlen(output));
        //        SSD1306_Set_Text_Size(2);
        //        SSD1306_Set_Text_Color(WHITE);
        unsigned long l = 0xDEADBEEF;
        sprintf(output, "0x%lX", (long) l);
        SSD1306_Write_String(output, strlen(output));
        SSD1306_Display();

        //        SSD1306_Clear_Display();
        //        SSD1306_Set_Rotation(0);
        //        SSD1306_Set_Text_Size(1);
        //        SSD1306_Set_Text_Color(SSD1306_WHITE);
        //        SSD1306_Set_Cursor(0, 0);
        //        SSD1306_Write_String("%u", i);
        //        i++;
        //        SSD1306_Display();

    }
}
// </editor-fold>
#elif defined(_TEST_SSD1331_OLED)
// <editor-fold defaultstate="collapsed" desc="_TEST_SSD1331_OLED">
void main(void) {
    char output[128];
    
    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    SPI_DATA spi_data;
    SPI2_Init(&spi_data, SPI2_FOSC_64); // Initialize the SPI module
    SSD1331_DATA ssd1331_data;
    SSD1331_Init(&ssd1331_data); // Initialize the OLED code

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    SSD1331_Begin();

    while (1) {

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1331_Set_Rotation(0);
        SSD1331_Test_Pattern();

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1331_Clear_Display();
        SSD1331_Set_Rotation(0);
        SSD1331_Set_Cursor(0, 0);
        sprintf(output, "Lorem ipsum dolor sit amet, consectetur adipiscing elit. Curabit adipiscing ante sed nibh tincidunt feugiat.");
        SSD1331_Write_String(output, strlen(output));

//        Delay10KTCYx(255);
//        Delay10KTCYx(255);
//        SSD1331_Clear_Display();
//        SSD1331_Set_Rotation(3);
//        SSD1331_Set_Cursor(0, 0);
//        SSD1331_Write_String("Lorem ipsum dolor sit amet, consectetur adipiscing elit. Curabitur adipiscing ante sed nibh tincidunt feugiat. Maecenas enim massa");

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1331_Set_Rotation(0);
        SSD1331_Test_DrawLines(SSD1331_YELLOW);

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        SSD1331_Set_Rotation(3);
        SSD1331_Test_DrawLines(SSD1331_BLUE);

                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(0);
                SSD1331_Test_DrawRect(SSD1331_GREEN);
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(1);
                SSD1331_Test_DrawRect(SSD1331_RED);
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(2);
                SSD1331_Test_DrawRect(SSD1331_BLUE);
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(3);
                SSD1331_Test_DrawRect(SSD1331_YELLOW);
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(0);
                SSD1331_Test_FillRect(SSD1331_YELLOW, SSD1331_MAGENTA);
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(3);
                SSD1331_Test_FillRect(SSD1331_BLUE, SSD1331_GREEN);

                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(0);
                SSD1331_Clear_Display();
                SSD1331_Test_FillCircle(10, SSD1331_BLUE);
                SSD1331_Test_DrawCircle(10, SSD1331_WHITE);
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(3);
                SSD1331_Clear_Display();
                SSD1331_Test_FillCircle(10, SSD1331_MAGENTA);
                SSD1331_Test_DrawCircle(10, SSD1331_YELLOW);
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(0);
                SSD1331_Test_DrawTria();
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(3);
                SSD1331_Test_DrawTria();

                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(0);
                SSD1331_Test_DrawRoundRect();
        
                Delay10KTCYx(255);
                Delay10KTCYx(255);
                SSD1331_Set_Rotation(3);
                SSD1331_Test_DrawRoundRect();

        //        SSD1331_Clear_Display();
        //        SSD1331_Set_Rotation(3);
        //        SSD1331_Set_Cursor(0,0);
        //        SSD1331_Set_Text_Color_BG(SSD1331_WHITE, SSD1331_BLACK);
        //        SSD1331_Write_String("%u", i);
        //        i++;
    }
}
// </editor-fold>
#elif defined(_TEST_OLED_CHAR)
// <editor-fold defaultstate="collapsed" desc="_TEST_OLED_CHAR">
void main(void) {
    char output[64];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

//    UART1_Init();
    OLED_CHAR_DATA oled_data;
    NHD_Init(&oled_data);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    NHD_Begin(16, 2);

    sprintf(output, "Lorem ipsum dolor sit amet, consectetur adipisicing elit, sed do");
    NHD_Write_String(output, strlen(output));
    NHD_Set_Cursor(0,1);
    sprintf(output, "eiusmod tempor incididunt ut labore et dolore magna aliqua. Ut e");
    NHD_Write_String(output, strlen(output));

    while (1) {
        Delay10KTCYx(150);
        NHD_Scroll_Display_Left();
    }
}
// </editor-fold>
#elif defined(_TEST_NFC_TO_SSD1306_OLED)
// <editor-fold defaultstate="collapsed" desc="_TEST_NFC_TO_SSD1306_OLED">
void main(void) {
    char output[64];

    // NFC stuff
    NFC_FIRMWARE_VERSION version;
    NFC_TargetDataMiFare cardData[2];
    NFC_TargetDataMiFare cardData_prev[2];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data);
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    NFC_DATA nfc_data;
    NFC_Init(&nfc_data);
    SPI_DATA spi_data;
    SPI2_Init(&spi_data, SPI2_FOSC_4);
    SSD1306_DATA ssd1306_data;
    SSD1306_Init(&ssd1306_data);

    I2C_Configure_Master(I2C_400KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    SSD1306_Begin(SSD1306_SWITCHCAPVCC);
    memset(cardData, 0, 24);
    memset(cardData_prev, 0, 24);
    SSD1306_Clear_Display();
    SSD1306_Set_Rotation(0);
    SSD1306_Set_Cursor(0, 0);

    version = NFC_Get_Firmware_Version();
    while (!version.IC) {
        sprintf(output, "Waiting for NFC board..\n");
        SSD1306_Write_String(output, strlen(output));
        SSD1306_Display();
        Delay10KTCYx(255);
        version = NFC_Get_Firmware_Version();
    }
    sprintf(output, "PN5%X Ver. %d.%d\n", version.IC, version.Ver, version.Rev);
    SSD1306_Write_String(output, strlen(output));
    SSD1306_Display();
    NFC_SAMConfig();

    while (1) {

        // This query will not wait for a detection before responding
        char length = NFC_Poll_Targets(1, 1, cardData);
        if (!length) {
            memset(cardData_prev, 0, 24);
        } else if (length == 1) {
            if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0) {
                // Do nothing
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[1].NFCID, cardData[0].NFCID_LEN) == 0) {
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            } else {
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[0].NFCID[0], cardData[0].NFCID[1], cardData[0].NFCID[2], cardData[0].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            }
            memset(&cardData_prev[1], 0, 12);
        } else if (length == 2) {
            if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0 &&
                    memcmp(&cardData[1].NFCID, &cardData_prev[1].NFCID, cardData[1].NFCID_LEN) == 0) {
                // Do nothing
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[1].NFCID, cardData[0].NFCID_LEN) == 0 &&
                    memcmp(&cardData[1].NFCID, &cardData_prev[0].NFCID, cardData[1].NFCID_LEN) == 0) {
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
                memcpy((char *) &cardData_prev[1], (const char *) &cardData[1], 12);
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0) {
                // First card matched
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[1].NFCID[0], cardData[1].NFCID[1], cardData[1].NFCID[2], cardData[1].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy(&cardData_prev[1], (const char *) &cardData[1], 12);
            } else if (memcmp(&cardData[1].NFCID, &cardData_prev[1].NFCID, cardData[1].NFCID_LEN) == 0) {
                // Second card matched
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[0].NFCID[0], cardData[0].NFCID[1], cardData[0].NFCID[2], cardData[0].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            } else {
                // No match
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[0].NFCID[0], cardData[0].NFCID[1], cardData[0].NFCID[2], cardData[0].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[1].NFCID[0], cardData[1].NFCID[1], cardData[1].NFCID[2], cardData[1].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy((char *) &cardData_prev[1], (const char *) &cardData[1], 12);
            }
        }
    }
}
// </editor-fold>
#elif defined(_TEST_LUX_TO_CHAR_OLED)
// <editor-fold defaultstate="collapsed" desc="_TEST_LUX_TO_CHAR_OLED">
void main(void) {
    char output[64];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    OLED_CHAR_DATA oled_data;
    NHD_Init(&oled_data);
    TSL2561_DATA lux_data;
    LUX_Init(&lux_data, TSL2561_ADDR_FLOAT);

    I2C_Configure_Master(I2C_400KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    NHD_Begin(16, 2);

    // You can change the gain on the fly, to adapt to brighter/dimmer light situations
    LUX_Set_Gain(TSL2561_GAIN_0X); // set no gain (for bright situtations)
//    LUX_Set_Gain(TSL2561_GAIN_16X);  // set 16x gain (for dim situations)

    // Changing the integration time gives you a longer time over which to sense light
    // longer timelines are slower, but are good in very low light situtations!
//    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_13MS); // shortest integration time (bright light)
    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_101MS);  // medium integration time (medium light)
//    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_402MS);  // longest integration time (dim light)

    while (1) {
        unsigned long lum = LUX_Get_Full_Luminosity();
        unsigned int ir = lum >> 16;
        unsigned int full = lum & 0xFFFF;
        NHD_Set_Cursor(0, 0);
        sprintf(output, "I: %d ", ir);
        NHD_Write_String(output, strlen(output));
        sprintf(output, "V: %d        ", full - ir);
        NHD_Write_String(output, strlen(output));
        NHD_Set_Cursor(0, 1);
        sprintf(output, "Lux: %ld        ", LUX_Calculate_Lux(full, ir));
        NHD_Write_String(output, strlen(output));

//        Delay10KTCYx(100);
    }
}
// </editor-fold>
#elif defined(_TEST_RTC_TO_LED_BACKPACK_CHAR_OLED)
// <editor-fold defaultstate="collapsed" desc="_TEST_RTC_TO_LED_BACKPACK_CHAR_OLED">
void main(void) {
    char output[64];

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    LED_DATA led_data;
    LED_Init(&led_data);
    OLED_CHAR_DATA oled_data;
    NHD_Init(&oled_data);

    I2C_Configure_Master(I2C_400KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    LED_Start();
    LED_Draw_Colon(1);
    NHD_Begin(16, 2);
    DS3231_Begin();

    char sec, min, hour, day, date, month, year, h_mil, h_am_pm;
    int time;
    while (1) {
        DS3231_Get_Time(&sec, &min, &hour, &day, &date, &month, &year, &h_mil, &h_am_pm);
        time = hour * 100 + min;
        LED_Write_Num(time);

        NHD_Set_Cursor(0, 0);
        sprintf(output, "%02d:%02d:%02d %s", hour, min, sec, h_am_pm ? "PM" : "AM");
        NHD_Write_String(output, strlen(output));
        
        NHD_Set_Cursor(12, 0);
        switch (day) {
            case 1:
                sprintf(output, "*MON");
                break;
            case 2:
                sprintf(output, "*TUE");
                break;
            case 3:
                sprintf(output, "*WED");
                break;
            case 4:
                sprintf(output, "*THU");
                break;
            case 5:
                sprintf(output, "*FRI");
                break;
            case 6:
                sprintf(output, "*SAT");
                break;
            case 7:
                sprintf(output, "*SUN");
                break;
        }
        NHD_Write_String(output, strlen(output));

        NHD_Set_Cursor(0, 1);
        switch (month) {
            case 1:
                sprintf(output, "January");
                break;
            case 2:
                sprintf(output, "February");
                break;
            case 3:
                sprintf(output, "March");
                break;
            case 4:
                sprintf(output, "April");
                break;
            case 5:
                sprintf(output, "May");
                break;
            case 6:
                sprintf(output, "June");
                break;
            case 7:
                sprintf(output, "July");
                break;
            case 8:
                sprintf(output, "August");
                break;
            case 9:
                sprintf(output, "September");
                break;
            case 10:
                sprintf(output, "October");
                break;
            case 11:
                sprintf(output, "November");
                break;
            case 12:
                sprintf(output, "December");
                break;
        }
        NHD_Write_String(output, strlen(output));

        sprintf(output, " %d 20%d", date, year);
        NHD_Write_String(output, strlen(output));

        Delay10KTCYx(100);
    }
}
// </editor-fold>
#elif defined(_TEST_AHRS)
// <editor-fold defaultstate="collapsed" desc="_TEST_AHRS">
void main(void) {
    char output[64];

    // Set all ports as digial I/O except for AN0-AN2 (pins 2-4)
    ANCON0 = 0xF8;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data);
    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    L3G_DATA gyro_data;
    L3G_Init(&gyro_data, L3GD20_DEVICE, L3G_SA0_HIGH);
    LSM303_DATA acc_data;
    LSM303_Init(&acc_data, LSM303DLHC_DEVICE, ACC_ADDRESS_SA0_A_LOW);

    I2C_Configure_Master(I2C_100KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    sprintf(output, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(output, strlen(output));

    L3G_Begin();
    LSM303_Begin();
    int a_x, a_y, a_z, m_x, m_y, m_z, g_x, g_y, g_z;
    while (1) {
        L3G_Read(&g_x, &g_y, &g_z);
        LSM303_Read_Acc(&a_x, &a_y, &a_z);
        LSM303_Read_Mag(&m_x, &m_y, &m_z);
        sprintf(output, "GAM:%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
                g_x,g_y,g_z,a_x,a_y,a_z,m_x,m_y,m_z);
        DBG_PRINT_MAIN(output, strlen(output));

        Delay10KTCYx(255);
    }
}
// </editor-fold>
#elif defined(_TEST_XBEE)
// <editor-fold defaultstate="collapsed" desc="_TEST_XBEE">
void main(void) {
    char buffer[100];

    XBEE_RX_AT_COMMAND_RESPONSE_FRAME *rx_at_cmd_response_frame;
    XBEE_RX_DATA_PACKET_FRAME *rx_data_frame;
    XBEE_RX_DATA_TX_STATUS_FRAME *rx_tx_status_frame;
    XBEE_RX_REMOTE_AT_COMMAND_FRAME *rx_remote_at_cmd_frame;
    XBEE_RX_NODE_IDENTIFICATION_INDICATOR_FRAME *rx_node_ident_frame;
    XBEE_RX_MODEM_STATUS_FRAME *rx_modem_status_frame;

    /* --------------------- Oscillator Configuration --------------------- */
    //    OSCTUNEbits.PLLEN = 1;          // Enable 4x PLL
    OSCCONbits.IRCF = 0b111; // Set INTOSC postscaler to 8MHz
    OSCCONbits.SCS = 0b00; // Use 96MHz PLL as primary clock source
    /* -------------------------------------------------------------------- */

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    UART_DATA uart_data;
    UART1_Init(&uart_data); // Initialize the UART handler code
    XBEE_DATA xbee_data;
    XBee_Init(&xbee_data);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts


    sprintf(buffer, "\r\nBegin Program\r\n");
    DBG_PRINT_MAIN(buffer, strlen(buffer));

    while (1) {

//#define _ROUTER
#define _COORDINATOR

#ifdef _ROUTER
        XBEE_TX_DATA_PACKET_FRAME *tx_data_frame;
        tx_data_frame = (void *) buffer;
        tx_data_frame->frame_type = XBEE_TX_DATA_PACKET;
        tx_data_frame->frame_id = 1;
        tx_data_frame->destination_64.UPPER_32.long_value = 0x00000000;
        tx_data_frame->destination_64.LOWER_32.long_value = 0x00000000;
        tx_data_frame->destination_16.INT_16.int_value = 0xFEFF;
        tx_data_frame->broadcast_radius = 0;
        tx_data_frame->options = 0;
        tx_data_frame->data[0] = 0x54;
        tx_data_frame->data[1] = 0x78;
        tx_data_frame->data[2] = 0x32;
        tx_data_frame->data[3] = 0x43;
        tx_data_frame->data[4] = 0x6F;
        tx_data_frame->data[5] = 0x6F;
        tx_data_frame->data[6] = 0x72;
        tx_data_frame->data[7] = 0x11;
        XBee_Process_Transmit_Frame(buffer, XBEE_TX_DATA_PACKET_FRAME_SIZE + 8);

        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
        Delay10KTCYx(255);
#endif

#ifdef _COORDINATOR
        int length = XBee_Get_Received_Frame(buffer);
        if (length != 0) {
            switch (*(char *) buffer) {
                case XBEE_RX_AT_COMMAND_RESPONSE:
                    sprintf(buffer, "XBEE: parsing recieved AT command response frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    rx_at_cmd_response_frame = (void *) buffer;
//                    DBG_PRINT_MAIN("Frame ID: %u\r\n", rx_at_cmd_response_frame->frame_id);
//                    DBG_PRINT_MAIN("AT Command: %c%c  Status: %02X\r\n", rx_at_cmd_response_frame->command[0], \\
//                            rx_at_cmd_response_frame->command[1], rx_at_cmd_response_frame->command_status);
                    if (length > XBEE_RX_AT_COMMAND_RESPONSE_FRAME_SIZE) {
//                        DBG_PRINT_MAIN("Command Data: ");
                        for (int i = 0; i < length - XBEE_RX_AT_COMMAND_RESPONSE_FRAME_SIZE; i++) {
//                            DBG_PRINT_MAIN("%02X ", rx_at_cmd_response_frame->data[i]);
                        }
//                        DBG_PRINT_MAIN("\r\n");
                    }
                    break;
                case XBEE_RX_DATA_PACKET:
                    sprintf(buffer, "XBEE: parsing recieved data recieved frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    rx_data_frame = (void *) buffer;
                    XBee_Convert_Endian_64(&(rx_data_frame->source_64));
                    XBee_Convert_Endian_16(&(rx_data_frame->source_16));
//                    DBG_PRINT_MAIN("Source 64: %08lX %08lX  Source 16: %04X  Options: %02X\r\n", \\
//                            rx_data_frame->source_64.UPPER_32.long_value, \\
//                            rx_data_frame->source_64.LOWER_32.long_value, \\
//                            rx_data_frame->source_16.INT_16.int_value, \\
//                            rx_data_frame->recieve_options);
//                    DBG_PRINT_MAIN("Data: ");
                    for (int i = 0; i < length - XBEE_RX_DATA_PACKET_FRAME_SIZE; i++) {
//                        DBG_PRINT_MAIN("%02X ", rx_data_frame->data[i]);
                    }
//                    DBG_PRINT_MAIN("\r\n");
                    break;
                case XBEE_RX_DATA_TX_STATUS:
                    sprintf(buffer, "XBEE: parsing recieved TX status frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    rx_tx_status_frame = (void *) buffer;
                    XBee_Convert_Endian_16(&(rx_tx_status_frame->destination_16));
//                    DBG_PRINT_MAIN("Frame ID: %u  Destination 16: %04X\r\n", \\
//                            rx_tx_status_frame->frame_id, rx_tx_status_frame->destination_16.INT_16.int_value);
//                    DBG_PRINT_MAIN("Transmit Retry Count: %02X  Delivery Status: %02X  Discovery Status: %02X\r\n", \\
//                            rx_tx_status_frame->transmit_retry_count, rx_tx_status_frame->delivery_status, \\
//                            rx_tx_status_frame->discovery_status);
                    break;
                case XBEE_RX_IO_DATA_SAMPLE:
                    sprintf(buffer, "XBEE: parsing recieved IO data sample frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    break;
                case XBEE_RX_EXPLICIT_COMMAND:
                    sprintf(buffer, "XBEE: parsing recieved explicit command frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    break;
                case XBEE_RX_REMOTE_AT_COMMAND_RESPONSE:
                    sprintf(buffer, "XBEE: parsing recieved remote AT command frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    rx_remote_at_cmd_frame = (void *) buffer;
                    break;
                case XBEE_RX_ROUTE_RECORD:
                    sprintf(buffer, "XBEE: parsing recieved route record frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    break;
                case XBEE_RX_NODE_IDENTIFICATION:
                    sprintf(buffer, "XBEE: parsing recieved node identification frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    rx_node_ident_frame = (void *) buffer;
                    XBee_Convert_Endian_64(&(rx_node_ident_frame->source_64));
                    XBee_Convert_Endian_16(&(rx_node_ident_frame->source_16));
                    XBee_Convert_Endian_64(&(rx_node_ident_frame->remote_64));
                    XBee_Convert_Endian_16(&(rx_node_ident_frame->remote_16));
                    XBee_Convert_Endian_16(&(rx_node_ident_frame->parent_16));
//                    DBG_PRINT_MAIN("Source 64: %08lX %08lX  Source 16: %04X  Options: %02X\r\n", \\
//                            rx_node_ident_frame->source_64.UPPER_32.long_value, \\
//                            rx_node_ident_frame->source_64.LOWER_32.long_value, \\
//                            rx_node_ident_frame->source_16.INT_16.int_value, \\
//                            rx_node_ident_frame->recieve_options);
//                    DBG_PRINT_MAIN("Remote 64: %08lX %08lX  Remote 16: %04X  Parent 16: %04X\r\n", \\
//                            rx_node_ident_frame->remote_64.UPPER_32.long_value, \\
//                            rx_node_ident_frame->remote_64.LOWER_32.long_value, \\
//                            rx_node_ident_frame->remote_16.INT_16.int_value, \\
//                            rx_node_ident_frame->parent_16.INT_16.int_value);
//                    DBG_PRINT_MAIN("Device Type: %02X  Source Event: %02X\r\n", \\
//                            rx_node_ident_frame->device_type, rx_node_ident_frame->source_event);
                    break;
                case XBEE_RX_FRAME_MODEM_STATUS:
                    sprintf(buffer, "XBEE: parsing recieved modem status frame\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    rx_modem_status_frame = (void *) buffer;
//                    DBG_PRINT_MAIN("Status: %02X\r\n", rx_modem_status_frame->status);
                    break;
                default:
                    sprintf(buffer, "??\r\n");
                    DBG_PRINT_MAIN(buffer, strlen(buffer));
                    break;
            }
        }
#endif

    }
}
// </editor-fold>
#else
int main() {
    char output[64];

    // Set all ports as digial I/O
    ANCON0 = 0xFF;
    ANCON1 = 0x1F;

    // NFC stuff
    NFC_FIRMWARE_VERSION version;
    NFC_TargetDataMiFare cardData[2];
    NFC_TargetDataMiFare cardData_prev[2];

    I2C_DATA i2c_data;
    I2C_Init(&i2c_data);
    LED_DATA led_data;
    LED_Init(&led_data);
    OLED_CHAR_DATA oled_data;
    NHD_Init(&oled_data);
    TSL2561_DATA lux_data;
    LUX_Init(&lux_data, TSL2561_ADDR_FLOAT);
    NFC_DATA nfc_data;
    NFC_Init(&nfc_data);
    SPI_DATA spi_data;
    SPI2_Init(&spi_data, SPI2_FOSC_4);
    SSD1306_DATA ssd1306_data;
    SSD1306_Init(&ssd1306_data);

    I2C_Configure_Master(I2C_400KHZ);

    Interrupt_Init(); // Initialize the interrupt priorities
    Interrupt_Enable(); // Enable high-priority interrupts and low-priority interrupts

    LED_Start();
    LED_Draw_Colon(1);
    NHD_Begin(16, 2);
    DS3231_Begin();

    // You can change the gain on the fly, to adapt to brighter/dimmer light situations
//    LUX_Set_Gain(TSL2561_GAIN_0X); // set no gain (for bright situtations)
    LUX_Set_Gain(TSL2561_GAIN_16X);  // set 16x gain (for dim situations)

    // Changing the integration time gives you a longer time over which to sense light
    // longer timelines are slower, but are good in very low light situtations!
    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_13MS); // shortest integration time (bright light)
//    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_101MS);  // medium integration time (medium light)
//    LUX_Set_Timing(TSL2561_INTEGRATIONTIME_402MS);  // longest integration time (dim light)

    SSD1306_Begin(SSD1306_SWITCHCAPVCC);
    memset(cardData, 0, 24);
    memset(cardData_prev, 0, 24);
    SSD1306_Clear_Display();
    SSD1306_Set_Rotation(0);
    SSD1306_Set_Cursor(0, 0);
    
    char sec, min, hour, day, date, month, year, h_mil, h_am_pm;
    int time;

    version = NFC_Get_Firmware_Version();
    while (!version.IC) {
        sprintf(output, "Waiting for NFC board..\n");
        SSD1306_Write_String(output, strlen(output));
        SSD1306_Display();
        Delay10KTCYx(255);
        version = NFC_Get_Firmware_Version();
    }
    sprintf(output, "PN5%X Ver. %d.%d\n", version.IC, version.Ver, version.Rev);
    SSD1306_Write_String(output, strlen(output));
    SSD1306_Display();
    NFC_SAMConfig();

    while (1) {
        // Time to LED backpack
        DS3231_Get_Time(&sec, &min, &hour, &day, &date, &month, &year, &h_mil, &h_am_pm);
        time = hour * 100 + min;
        LED_Write_Num(time);

        // Lux to Character OLED
        unsigned long lum = LUX_Get_Full_Luminosity();
        unsigned int ir = lum >> 16;
        unsigned int full = lum & 0xFFFF;
        NHD_Set_Cursor(0, 0);
        sprintf(output, "I: %d ", ir);
        NHD_Write_String(output, strlen(output));
        sprintf(output, "V: %d        ", full - ir);
        NHD_Write_String(output, strlen(output));
        NHD_Set_Cursor(0, 1);
        sprintf(output, "Lux: %ld        ", LUX_Calculate_Lux(full, ir));
        NHD_Write_String(output, strlen(output));

        // NFC Query
        char length = NFC_Poll_Targets(1, 1, cardData);
        if (!length) {
            memset(cardData_prev, 0, 24);
        } else if (length == 1) {
            if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0) {
                // Do nothing
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[1].NFCID, cardData[0].NFCID_LEN) == 0) {
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            } else {
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[0].NFCID[0], cardData[0].NFCID[1], cardData[0].NFCID[2], cardData[0].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            }
            memset(&cardData_prev[1], 0, 12);
        } else if (length == 2) {
            if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0 &&
                    memcmp(&cardData[1].NFCID, &cardData_prev[1].NFCID, cardData[1].NFCID_LEN) == 0) {
                // Do nothing
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[1].NFCID, cardData[0].NFCID_LEN) == 0 &&
                    memcmp(&cardData[1].NFCID, &cardData_prev[0].NFCID, cardData[1].NFCID_LEN) == 0) {
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
                memcpy((char *) &cardData_prev[1], (const char *) &cardData[1], 12);
            } else if (memcmp(&cardData[0].NFCID, &cardData_prev[0].NFCID, cardData[0].NFCID_LEN) == 0) {
                // First card matched
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[1].NFCID[0], cardData[1].NFCID[1], cardData[1].NFCID[2], cardData[1].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy(&cardData_prev[1], (const char *) &cardData[1], 12);
            } else if (memcmp(&cardData[1].NFCID, &cardData_prev[1].NFCID, cardData[1].NFCID_LEN) == 0) {
                // Second card matched
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[0].NFCID[0], cardData[0].NFCID[1], cardData[0].NFCID[2], cardData[0].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
            } else {
                // No match
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[0].NFCID[0], cardData[0].NFCID[1], cardData[0].NFCID[2], cardData[0].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy((char *) &cardData_prev[0], (const char *) &cardData[0], 12);
                sprintf(output, "UID: %02X %02X %02X %02X\n", cardData[1].NFCID[0], cardData[1].NFCID[1], cardData[1].NFCID[2], cardData[1].NFCID[3]);
                SSD1306_Write_String(output, strlen(output));
                SSD1306_Display();
                memcpy((char *) &cardData_prev[1], (const char *) &cardData[1], 12);
            }
        }


        Delay10KTCYx(100);
    }
}
#endif