#include "xparameters.h"
#include "xil_printf.h"
#include "xgpio.h"

#define CHANNEL_LEDS 1
#define CHANNEL_SWITCHES 2

// Print LSB 'bits' from 'number' in binary format
void print_bin(u32 number, u8 bits)
{
  u32 mask = 1 << (bits - 1);
  for(; mask; mask >>= 1)
    xil_printf("%d", (number & mask) != 0);
}


int main()
{
  XGpio_Config *gpio_0_cfg;
  XGpio gpio_0;
  u32 gpio_0_data;

  // Initialise GPIO
  gpio_0_cfg = XGpio_LookupConfig(XPAR_AXI_GPIO_0_BASEADDR);
  XGpio_CfgInitialize(&gpio_0, gpio_0_cfg, gpio_0_cfg->BaseAddress);

  // Print header
  print("LED & Switch Example\n\n\r");
  print("Select required operation :-\n\r");
  print("r - Read from switch register\n\r");
  print("w - Write to LED register\n\r");
  print("q - Quit application\n\r");

  // Wait for user input & perform required operation
  char8 key;
  do
  {
    key = inbyte();

    switch (key)
    {
      case 'r':
        gpio_0_data = XGpio_DiscreteRead(&gpio_0, CHANNEL_SWITCHES);
        xil_printf("Switch Register @ 0x%x = 0b", gpio_0_cfg->BaseAddress);
        print_bin(gpio_0_data, 8);
        print("\n\r");
        break;

      case 'w':
        gpio_0_data = 0b00000001;
        for (u8 i = 0; i <= 15; i++)
        {
          XGpio_DiscreteWrite(&gpio_0, CHANNEL_LEDS, gpio_0_data);
          xil_printf("LED Register @ 0x%x = 0b", gpio_0_cfg->BaseAddress);
          print_bin(gpio_0_data, 8);
          print("\n\r");
          usleep(100000);
          if (i < 7)
            gpio_0_data <<= 1;
          else
            gpio_0_data >>= 1;
        }
        break;
    }
  } while (key != 'q');

  // Print footer
  print("All done!\n\n\r");

  // Exit application
  return 0;
}