#include "ufb/fb_7seg.h"
#include "ufb/progmem.h"

enum SevenSegBars {
  T = 1, RT = 2, RB = 4, B = 8, LB = 16, LT = 32, M = 64
};

static const uint8_t PROGMEM seven[] = {
        [0] = T | RT | RB | B | LB | LT,
        [1] = RT | RB,
        [2] = T | RT | M | LB | B,
        [3] = T | RT | M | RB | B,
        [4] = RT | RB | M | LT,
        [5] = T | LT | M | RB | B,
        [6] = T | LT | LB | B | RB | M,
        [7] = T | RT | RB,
        [8] = T | LT | RT | LB | RB | B | M,
        [9] = T | LT | RT | RB | B | M,
};

fbpos_t fb_7seg_dig(fbpos_t x, fbpos_t y, fbpos_t w, fbpos_t h, fbpos_t th, uint8_t digit, fbcolor_t color)
{
    const uint8_t mask = digit > 9 ? 0 : pgm_read_byte(&seven[digit]);
    const fbpos_t wi = w - th * 2;
    const fbpos_t hi = (h - th * 3) / 2;

    bool bcolor = !color; // changed for XOR

    fb_rect(x + th,
            y,
            wi,
            th, bcolor ^ (bool) (mask & T));

    fb_rect(x + th,
            y + th + hi,
            wi,
            th, bcolor ^ (bool) (mask & M));

    fb_rect(x + th,
            y + th * 2 + hi * 2,
            wi,
            th, bcolor ^ (bool) (mask & B));

    fb_rect(x,
            y + th,
            th,
            hi, bcolor ^ (bool) (mask & LT));

    fb_rect(x,
            y + hi + th * 2,
            th,
            hi, bcolor ^ (bool) (mask & LB));

    fb_rect(x + th + wi,
            y + th,
            th,
            hi, bcolor ^ (bool) (mask & RT));

    fb_rect(x + th + wi,
            y + th * 2 + hi,
            th,
            hi, bcolor ^ (bool) (mask & RB));

    return w;
}

fbpos_t fb_7seg_period(fbpos_t x, fbpos_t y, fbpos_t w, fbpos_t h, fbpos_t th, fbcolor_t color)
{
    const fbpos_t hi = (h - th * 3) / 2;
    fb_rect(x, y + hi * 2 + th * 2, th, th, color);
    return th;
}

void fb_7seg_number(fbpos_t x, fbpos_t y, fbpos_t w, fbpos_t h, fbpos_t th, fbpos_t spacing, fbcolor_t color, uint16_t num, uint8_t places, uint8_t decimals)
{
    uint8_t digits[5] = {};
    uint8_t pos = 4;
    while (num > 0) {
        uint8_t res = num % 10;
        num /= 10;
        digits[pos] = res;
        pos--;
    }

    bool drawing = false;
    for (uint8_t i = 5 - places; i < 5; i++) {
        uint8_t d = digits[i];

        if (i == 5 - decimals) {
            fb_7seg_period(x, y, w, h, th, color);
            x += th + spacing;
        }

        if (!drawing && d == 0) {
            if (i == 5 - decimals - 1) {
                fb_7seg_dig(x, y, w, h, th, d, color);
                x += w + spacing;
                drawing = true;
            } else {
                fb_7seg_dig(x, y, w, h, th, 8, !color); // clear
                x += w + spacing;
            }
        } else {
            fb_7seg_dig(x, y, w, h, th, d, color);
            x += w + spacing;
            drawing = true;
        }
    }
}