// STM32 Nucleo-L031K6 HAL Blink + printf() example
// Simulation: https://wokwi.com/projects/367244067477216257

#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stm32l0xx_hal.h>

// ST Nucleo Green user LED (PB3)
#define LED_PORT                GPIOB
#define LED_3_PIN               GPIO_PIN_3
#define LED_GREEN_PIN           GPIO_PIN_6
#define LED_RED_PIN             GPIO_PIN_5
#define LED_BLUE_PIN            GPIO_PIN_4
#define LED_PORT_CLK_ENABLE     __HAL_RCC_GPIOB_CLK_ENABLE
#define VCP_TX_Pin              GPIO_PIN_2
#define VCP_RX_Pin              GPIO_PIN_15

#define SUCCESS 0
#define FAIL 1
#define REPEAT 2

#define POOL_SIZE 5

typedef int(*pf)(void);

typedef struct Process {
  int id;
  pf function;
  int period;
  int start;
  int priority;
} Process;

struct Process POOL[POOL_SIZE];
int head, tail, timerCounter;

int kernelInit(void);
int kernelAddProc(Process* newProc);
void kernelLoop(void);
void kernelClock(void);

int processA(void);
int processB(void);
int processC(void);

void sortPool(void);

UART_HandleTypeDef huart2;

void SystemClock_Config(void);
static void MX_USART2_UART_Init(void);

int main(void) {
  HAL_Init();
  SystemClock_Config();

  initGPIO();
  MX_USART2_UART_Init();

  kernelInit();

  Process proc1 = { 1, processA, 5, 5, 1 };
  Process proc2 = { 2, processB, 15, 15, 2 };
  Process proc3 = { 3, processC, 10, 10, 3 };

  kernelAddProc(&proc1);
  kernelAddProc(&proc2);
  kernelAddProc(&proc3);

  kernelLoop();

  return 0;
}

int kernelInit(void) {
  head = 0;
  tail = 0;
  timerCounter = 0;

  return SUCCESS;
}

int kernelAddProc(Process *newProc) {
  if((tail + 1) % POOL_SIZE == head) return FAIL;

  POOL[tail].id = newProc->id;
  POOL[tail].function = newProc->function;
  POOL[tail].period = newProc->period;
  POOL[tail].start = 0;

  if (newProc->start <= 0) {
    POOL[tail].start = newProc->start;
  }

  POOL[tail].start += newProc->period;
  tail = (tail + 1) % POOL_SIZE;

  return SUCCESS;
}

void kernelLoop(void) {
  uint32_t previousTime;

  while(1) {
    uint32_t currentTime = HAL_GetTick();
    uint32_t elapsedTime = currentTime - previousTime;

    if (elapsedTime >= 1000) {
      previousTime = currentTime;

      printf("\nTime: %d seconds", timerCounter);

      if (head != tail) {
        sortPool();

        if (POOL[head].start <= 0) {
          int ans = POOL[head].function();

          if (ans == REPEAT) {
            kernelAddProc(&POOL[head]);
          }

          head = (head + 1) % POOL_SIZE;
        }
      }

      kernelClock();
      previousTime = HAL_GetTick();
      timerCounter++;
      printf("\n");
    }
  }
}

void kernelClock() {
  int i = head;

  while (i != tail) {
    POOL[i].start--;
    i = (i+1) % POOL_SIZE;
  }
}

int processA(void) {
  printf("\n\n------------------------");
  printf("\n| Process A is running |");
  printf("\n------------------------");

  HAL_GPIO_WritePin(LED_PORT, LED_BLUE_PIN, GPIO_PIN_SET);
  HAL_Delay(900);
  HAL_GPIO_WritePin(LED_PORT, LED_BLUE_PIN, GPIO_PIN_RESET);

  return REPEAT;
}

int processB(void) {
  printf("\n\n------------------------");
  printf("\n| Process B is running |");
  printf("\n------------------------");

  HAL_GPIO_WritePin(LED_PORT, LED_RED_PIN, GPIO_PIN_SET);
  HAL_Delay(1500);
  HAL_GPIO_WritePin(LED_PORT, LED_RED_PIN, GPIO_PIN_RESET);

  return REPEAT;
}

int processC(void) {
  printf("\n\n------------------------");
  printf("\n| Process C is running |");
  printf("\n------------------------");

  HAL_GPIO_WritePin(LED_PORT, LED_GREEN_PIN, GPIO_PIN_SET);
  HAL_Delay(1300);
  HAL_GPIO_WritePin(LED_PORT, LED_GREEN_PIN, GPIO_PIN_RESET);

  return REPEAT;
}

void sortPool() {
  int next = head;
  int adj = (next + 1) % POOL_SIZE;

  while (adj != tail) {
    if (
      POOL[adj].start < POOL[next].start ||
      (POOL[next].start == POOL[adj].start && POOL[adj].priority > POOL[next].priority) 
    ) {
      next = adj;
    }
    adj = (adj + 1) % POOL_SIZE;
  }

  Process temp;
  temp = POOL[next];
  POOL[next] = POOL[head];
  POOL[head] = temp;
}

// ---------------------------------------------

void osSystickHandler(void) {
  // 1 Hz blinking:
  if ((HAL_GetTick() % 500) == 0)
  {
    HAL_GPIO_TogglePin(LED_PORT, LED_3_PIN);
  }
}

void initGPIO() {
  GPIO_InitTypeDef GPIO_Config;

  GPIO_Config.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_Config.Pull = GPIO_NOPULL;
  GPIO_Config.Speed = GPIO_SPEED_FREQ_HIGH;

  GPIO_Config.Pin = LED_RED_PIN;
  LED_PORT_CLK_ENABLE();
  HAL_GPIO_Init(LED_PORT, &GPIO_Config);

  GPIO_Config.Pin = LED_GREEN_PIN;
  LED_PORT_CLK_ENABLE();
  HAL_GPIO_Init(LED_PORT, &GPIO_Config);

  GPIO_Config.Pin = LED_BLUE_PIN;
  LED_PORT_CLK_ENABLE();
  HAL_GPIO_Init(LED_PORT, &GPIO_Config);

  __HAL_RCC_GPIOB_CLK_ENABLE();
}

void SystemClock_Config(void) {
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
    in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLLMUL_4;
  RCC_OscInitStruct.PLL.PLLDIV = RCC_PLLDIV_2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
                                | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
  {
    Error_Handler();
  }
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART2;
  PeriphClkInit.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
}


/**
    @brief USART2 Initialization Function
    @param None
    @retval None
*/
static void MX_USART2_UART_Init(void) {
  __HAL_RCC_GPIOA_CLK_ENABLE();
  /**USART2 GPIO Configuration
    PA2     ------> USART2_TX
    PA15     ------> USART2_RX
  */
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  GPIO_InitStruct.Pin = VCP_TX_Pin | VCP_RX_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
  GPIO_InitStruct.Alternate = GPIO_AF4_USART2;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  huart2.Instance = USART2;
  huart2.Init.BaudRate = 115200;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    Error_Handler();
  }

  __HAL_RCC_USART2_CLK_ENABLE();
}

void Error_Handler(void) {
  /* User can add his own implementation to report the HAL error return state */
}


// The following makes printf() write to USART2:

#define STDOUT_FILENO   1
#define STDERR_FILENO   2

int _write(int file, uint8_t *ptr, int len) {
  switch (file)
  {
    case STDOUT_FILENO:
      HAL_UART_Transmit(&huart2, ptr, len, HAL_MAX_DELAY);
      break;

    case STDERR_FILENO:
      HAL_UART_Transmit(&huart2, ptr, len, HAL_MAX_DELAY);
      break;

    default:
      return -1;
  }

  return len;
}