#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/watchdog.h"
#include "hardware/uart.h"


// UART defines
#define UART_ID uart0
#define BAUD_RATE 115200

// Pins can be changed, see the GPIO function select table in the datasheet for information on GPIO assignments
#define UART_TX_PIN 0
#define UART_RX_PIN 1

// Output Pin
#define HEARTBEAT_PIN 24


// INPUT PINS
#define INPUT_PIN_START 8
#define NUM_INPUT_PINS 16

int main()
{
    stdio_init_all();

    // Watchdog example code
    if (watchdog_caused_reboot()) {
        printf("Rebooted by Watchdog!\r\n");
        // Whatever action you may take if a watchdog caused a reboot
    }
    
    // Enable the watchdog, requiring the watchdog to be updated every 100ms or the chip will reboot
    // second arg is pause on debug which means the watchdog will pause when stepping through code
    watchdog_enable(1000, 1);
    
    // You need to call this function at least more often than the 100ms in the enable call to prevent a reboot
    watchdog_update();

    // Set up our UART
    uart_init(UART_ID, BAUD_RATE);
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
    
    // Setup Inputs
    for (int i = 0; i < NUM_INPUT_PINS; i++) {
        gpio_init(INPUT_PIN_START + i);
        gpio_set_dir(INPUT_PIN_START + i, GPIO_IN);
        // No pullups or pulldowns on the input pins
        gpio_set_pulls(INPUT_PIN_START + i, false, false);
    }

    // Setup Heartbeat Output
    gpio_init(HEARTBEAT_PIN);
    gpio_set_dir(HEARTBEAT_PIN, GPIO_OUT);

    
    // Send a welcome message over UART
    printf("MBARI TcuController %s %s\r\n", __DATE__, __TIME__);
    

    // Main Loop
    int counter = 0;
    while (true) {
        // read the state of the input pins and print them out
        printf("$TCUINPUTS,");  
        for (int i = 0; i < NUM_INPUT_PINS; i++) {
            printf("%d,", gpio_get(INPUT_PIN_START + i));
        }
        printf("*\r\n");
        watchdog_update();

        // Toggle the heartbeat pin
        if (counter++ > 5) {
            counter = 0;
            gpio_put(HEARTBEAT_PIN, !gpio_get(HEARTBEAT_PIN));
        }
        sleep_ms(100);

    }
}