#include "stm32f1xx_hal.h"
#include <bits/time.h>
#include <pthread.h>
#include <stdio.h>
#include <unistd.h>

uint8_t gpio_status[4][64];

static uint8_t get_1st_pin_from_bitmask(uint16_t bitmask){
  for(int i = 0; i < 16; i++){
    if(bitmask & (1 << i)) return i;
  }
  return 0;
}

void HAL_GPIO_TogglePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin){
  uint8_t pin = get_1st_pin_from_bitmask(GPIO_Pin);
  gpio_status[GPIOx->index][pin] = !gpio_status[GPIOx->index][pin];
  printf("toggling pin\n");
}
GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin){
  uint8_t pin = get_1st_pin_from_bitmask(GPIO_Pin);
  printf("reading pin %d\n", gpio_status[GPIOx->index][pin]);
  return gpio_status[GPIOx->index][pin];
}
void HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState){
  uint8_t pin = get_1st_pin_from_bitmask(GPIO_Pin);
  gpio_status[GPIOx->index][pin] = PinState;
  printf("writing pin\n");
}

static void* tim_handler(void* arg){
  TIM_HandleTypeDef* htim = arg;

  struct timespec start, current;
  clock_gettime(CLOCK_MONOTONIC, &start);
  
  uint64_t delay_ps = (uint64_t) 15625 * (htim->presc+1) * (htim->period+1);

  int exec_counter = 0;
  
  while(htim->started){
    clock_gettime(CLOCK_MONOTONIC, &current);
    uint64_t time = (uint64_t) 1000000000 * (current.tv_sec - start.tv_sec) + (current.tv_nsec - start.tv_nsec);

    unsigned int nb_exec = time * 1000 / delay_ps;
    int backlog = nb_exec - exec_counter;
    if(backlog > 10)
      printf("timer simulation lagging behind (%d ticks)\n", backlog);
    while(backlog > 0 && htim->started){
      HAL_TIM_PeriodElapsedCallback(htim);
      exec_counter++;
      backlog = nb_exec - exec_counter;
    }
  }
  return NULL;
}

HAL_StatusTypeDef HAL_TIM_Base_Start_IT(TIM_HandleTypeDef *htim){
  printf("starting timer\n");
  htim->started = 1;
  pthread_t tim_thread;
  pthread_create(&tim_thread, NULL, tim_handler, htim);
  return HAL_STATUS_OK;
}
HAL_StatusTypeDef HAL_TIM_Base_Stop_IT(TIM_HandleTypeDef *htim){
  printf("stopping timer\n");
  htim->started = 0;
  return HAL_STATUS_OK;
}