#include "display.h"
#include "registers.h"

#define CYCLES 17

const char numbers_seven_segments[11] = {
  0x3F, /* 0 */
  0x06, /* 1 */
  0x5B, /* 2 */
  0x4F, /* 3 */
  0x66, /* 4 */
  0x6D, /* 5 */
  0x7D, /* 6 */
  0x07, /* 7 */
  0x7F, /* 8 */
  0x6F, /* 9 */
  0x40, /* - */
};

uint16_t pow16(uint16_t num, uint8_t pow) {
  for (uint16_t i = 0; i < pow; i++) {
    num *= num;
  }
  return num;
}


void display_init(display_t *display, uint16_t digits, pin_t pin_data,
                  pin_t pin_sftclk, pin_t pin_strobe) {
  display->digits_count = digits;

  for (uint8_t i = 0; i < display->digits_count; i++) {
    display->digits[i] = '\0';
  }

  display->digits_en = ~0;
  display->digit_dots = 0;

  display->pin_data = pin_data;
  display->pin_sftclk = pin_sftclk;
  display->pin_strobe = pin_strobe;

  display->max_value = pow16(10, digits);
  display->current_cycle = 0;
  display->current_shifter = 0;
  display->current_digit = 0;
}

uint16_t seven_segment_convert(char character, uint16_t digit, uint8_t dot);
uint16_t convert_num_digit(char character, uint8_t digits_count, uint8_t digit, uint8_t dot);

uint16_t seven_segment_convert(char character, uint16_t digit, uint8_t dot) {
  uint8_t data;

  if (character >= '0' && character <= '9') {
    data = numbers_seven_segments[character - '0'];
  } else {
    switch (character) {
    case 'E':
      data = 0b01111001;
      break;
    case 'r':
      data = 0b01010000;
      break;
    default:
      data = 0;
      break;
    }
  }

  return ~(data | dot << 7);
}

uint16_t convert_num_digit(char character, uint8_t digits_count, uint8_t digit, uint8_t dot) {
  uint16_t seven_segments = seven_segment_convert(character, digits_count - 1 - digit, dot);
  uint16_t digits = 1 << digit;

  return (seven_segments << 8) | digits;
}

void display_dots(display_t* display, uint8_t dots) {
  display->digit_dots = dots;
}

void display_convert_number(display_t *display, uint32_t number) {
  uint32_t digit = 0;
  for (uint8_t i = 0; i < display->digits_count; i++) {
    digit = (number % 10) << (i << 2);
    display->digits[i] = digit;
    number /= 10;
  }
}

void display_enable_digit(display_t *display, uint8_t digit, uint8_t enable) {
  uint16_t digits = 1 << digit;
  if (enable != 0) {
    display->digits_en |= digits;
  } else {
    display->digits_en &= ~digits;
  }
}

void display_digits_set(display_t *display, char* digits) {
  for (uint8_t digit = 0; digit < display->digits_count; digit++) {
    display->digits[digit] = digits[digit];
  }
}

void display_digit_set(display_t *display, uint8_t digit, uint8_t number) {
  display->digits[digit] = number;
}

display_update_state_t display_update(display_t *display) {
  display_update_state_t state = DISPLAY_UPDATE_SHIFT;

  if (display->current_cycle == 0) {
    // Set up.
    display->current_digit = (display->current_digit + 1) % display->digits_count;

    if (display->digits_en & (1 << display->current_digit)) {
      display->current_shifter = convert_num_digit(
          display->digits[display->current_digit], display->digits_count, display->current_digit,
          (display->digit_dots >> display->current_digit) & 1);
    } else {
      // Still send the data even for disabled digits,
      // to prevent display having different brightness
      // depending on how many digits are currently on.
      display->current_shifter = 0;
    }

    state = DISPLAY_UPDATE_NEW_DIGIT;
  }

  if (display->current_cycle < CYCLES - 1) {
    pin_write(&display->pin_data,
              reg_read_bits_pos(&display->current_shifter,
                                15 - display->current_cycle,
                                1));

    pin_set(&display->pin_sftclk);
    pin_reset(&display->pin_sftclk);
  } else {
    // Send strobe
    pin_set(&display->pin_strobe);
    pin_reset(&display->pin_strobe);

    state = DISPLAY_UPDATE_DISPLAY_DIGIT;
  }

  display->current_cycle = (display->current_cycle + 1) % CYCLES;

  return state;
}