#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;
  display->bcd_digits = 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;
}

/**
 * @brief Convert a digit from a regular number to seven segments
 * @param[in] number The number to convert
 * @param[in] digit The digit from least significant, starting with 0
 * @return Description
 */
uint16_t seven_segment_convert(uint16_t number, uint16_t digit, uint8_t dot);

/**
 * @brief Converts a number to serial strem to send via the SIPO register.
 * @param[in] number The bcd number to convert to the values for shift register
 * @param[in] digit The digit from the least significant, starting with 0
 * @return Description
 */
uint16_t convert_num_digit(uint16_t bcd_number, uint8_t digits_count, uint8_t digit, uint8_t dot);

uint16_t seven_segment_convert(uint16_t bcd_number, uint16_t digit, uint8_t dot) {
  return ~(numbers_seven_segments[(bcd_number >> (digit << 2)) & 0xF] | dot << 7);
}

uint16_t convert_num_digit(uint16_t number, uint8_t digits_count, uint8_t digit, uint8_t dot) {
  uint16_t seven_segments = seven_segment_convert(number, 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 bcd = 0;
  for (uint8_t i = 0; i < display->digits_count; i++) {
    bcd |= (number % 10) << (i << 2);
    number /= 10;
  }
  display->bcd_digits = bcd;
}

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_digit_set(display_t *display, uint8_t digit, uint8_t number) {
  reg_write_bits_pos(&display->bcd_digits, number, digit << 2, 0xF);
}

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->bcd_digits, 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;
}