#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

typedef volatile uint32_t gpio_reg32_t;

typedef uint8_t gpio_pin_t;
typedef bool gpio_level_t;

#define GPIO_LEVEL_LOW  ((gpio_level_t)false)
#define GPIO_LEVEL_HIGH ((gpio_level_t)true)

/* GPIO direction */
typedef enum {
    GPIO_DIRECTION_INPUT = 0U,
    GPIO_DIRECTION_OUTPUT = 1U
} gpio_direction_t;

#define GPIO_BASE 0x3FF44000UL

/* GPIO Registers */
#define GPIO_OUT_REG        (*(gpio_reg32_t *)(GPIO_BASE + 0x04UL))
#define GPIO_OUT_W1TS_REG   (*(gpio_reg32_t *)(GPIO_BASE + 0x08UL))
#define GPIO_OUT_W1TC_REG   (*(gpio_reg32_t *)(GPIO_BASE + 0x0CUL))

#define GPIO_ENABLE_REG     (*(gpio_reg32_t *)(GPIO_BASE + 0x20UL))
#define GPIO_ENABLE_W1TS_REG \
    (*(gpio_reg32_t *)((uintptr_t)GPIO_BASE + 0x24UL))
#define GPIO_ENABLE_W1TC_REG (*(gpio_reg32_t *)(GPIO_BASE + 0x28UL))

#define GPIO_IN_REG         (*(gpio_reg32_t *)(GPIO_BASE + 0x3CUL))

#define RED_LED_PIN 2
#define GREEN_LED_PIN 5
#define YELLOW_LED_PIN 4

#define SWITCH_PIN 12

void gpio_driver_set_output(gpio_pin_t pin)
{
    GPIO_ENABLE_W1TS_REG = (1UL << pin);
}

void gpio_driver_set_input(gpio_pin_t pin)
{
    GPIO_ENABLE_W1TC_REG = (1UL << pin);
}

void gpio_driver_write(gpio_pin_t pin, gpio_level_t level)
{
    if (level == GPIO_LEVEL_HIGH) {
        GPIO_OUT_W1TS_REG = (1UL << pin);
    } else {
        GPIO_OUT_W1TC_REG = (1UL << pin);
    }
}

void gpio_driver_toggle(gpio_pin_t pin)
{
    GPIO_OUT_REG ^= (1UL << pin);
}

gpio_level_t gpio_driver_read(gpio_pin_t pin)
{
    return (gpio_level_t)((GPIO_IN_REG & (1UL << pin)) != 0UL);
}

void warning_mode_cycle(void){
  for(uint8_t i = 0;i <= 2; i++){
      gpio_driver_write(YELLOW_LED_PIN, 1U);
      vTaskDelay(700 / portTICK_PERIOD_MS);
      gpio_driver_write(YELLOW_LED_PIN, 0U);
      vTaskDelay(700 / portTICK_PERIOD_MS);
  }
  gpio_driver_write(RED_LED_PIN, 1U);
  gpio_level_t switch_level = gpio_driver_read(SWITCH_PIN);

}

void app_main() {

  gpio_driver_set_input(SWITCH_PIN);

  gpio_driver_set_output(GREEN_LED_PIN);
  gpio_driver_set_output(RED_LED_PIN);
  gpio_driver_set_output(YELLOW_LED_PIN);


  while (true) {
    // Read the switch level
    gpio_level_t switch_level = gpio_driver_read(SWITCH_PIN);
    if(switch_level == 0U){
      gpio_driver_write(YELLOW_LED_PIN, 0U);
      gpio_driver_write(RED_LED_PIN, 0U);
      gpio_driver_write(GREEN_LED_PIN, 1U);
    }else{
      gpio_driver_write(GREEN_LED_PIN, 0U);
      warning_mode_cycle();
    }
    
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}