#include <stdio.h>
#include "modeswitch.h"
#include "main.h"


MODESWITCH_mode_t mode;

MODESWITCH_mode_t MODESWITCH_ReadMode(void)
{
  

  int m0 = (HAL_GPIO_ReadPin(GPIO_INPUT_MODE_B0_GPIO_Port, GPIO_INPUT_MODE_B0_Pin) << 0);
  int m1 = (HAL_GPIO_ReadPin(GPIO_INPUT_MODE_B1_GPIO_Port, GPIO_INPUT_MODE_B1_Pin) << 1);
  int m2 = (HAL_GPIO_ReadPin(GPIO_INPUT_MODE_B2_GPIO_Port, GPIO_INPUT_MODE_B2_Pin) << 2);
  int m3 = (HAL_GPIO_ReadPin(GPIO_INPUT_MODE_B3_GPIO_Port, GPIO_INPUT_MODE_B3_Pin) << 3);

  

  mode = m0 | m1 | m2 | m3;
  mode = 0x0f &~mode;
  printf("mode=%i\n",mode);

  return mode;
}


MODESWITCH_mode_t MODESWITCH_GetMode(void)
{
  return mode;
}