#include <stdio.h>
#include <wiringPi.h>
#include <mcp23017.h>
#include <time.h>

const char disp_numbers[] = {89, 90, 91, 92, 93};
const char run = 95;
const char lt = 88;

const char base = 81;
void init(void)
{
  wiringPiSetup();
  mcp23017Setup(base, 0x21) ;
  
  for(int i = 0; i < 16; i++)
  {
    pinMode(base + i, OUTPUT);
    digitalWrite(base + i, 1);
  }
  digitalWrite(run, 0);
}
void clear_displays(void)
{
    for(int i = 0; i < 5; i++)
    {
        digitalWrite(disp_numbers[i], 1);
    }
}
void select_display(int number)
{
    digitalWrite(disp_numbers[number-1], 0);
}

void toggle_display(int number)
{
    select_display(number);
    clear_displays();
}
void print_pins()
{
    for(int i = 0; i < 16; i++)
    {
      int state = digitalRead(base + i);
      printf("PIN %d is %d\n", i, state);
    }
}
void select_digit(char number)
{
    // MSB to LSB
    digitalWrite(84, (number % 2));
    number = number / 2;
    digitalWrite(85, (number % 2));
    number = number / 2;
    digitalWrite(86, (number % 2));
    number = number / 2;
    digitalWrite(87, (number % 2));
}
void select_segment(int number)
{
    digitalWrite(81, (number % 2));
    number = number / 2;
    digitalWrite(82, (number % 2));
    number = number / 2;
    digitalWrite(83, (number % 2));
}

int main(void)
{
  
    clock_t t;
    init();
    print_pins();
    int numbers[] = {0, 1, 2 ,3 ,4 ,5, 6, 7};
    for(int i = 0; i < 50000; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            //t = clock();
            select_segment(j);
            //printf("select_segment %d\n", j);
            select_digit(numbers[j]);
            //printf("select_digit %d\n", numbers[j]);
            toggle_display(3);
            //t = clock() - t;
            //printf("took %f ms \n" , (((float) t)/CLOCKS_PER_SEC)*1000);
        }
        //print_pins();
    }
  
}