#define Q12_SHIFT 12
#define COMMON_ANODE 0

// Segment pins: A, B, C, D, E, F, G, DP
GPIO_TypeDef* seg_ports[8] = {GPIOA, GPIOA, GPIOA, GPIOB, GPIOA, GPIOA, GPIOA, GPIOA};
uint16_t      seg_pins[8]  = {GPIO_PIN_0, GPIO_PIN_1, GPIO_PIN_12, GPIO_PIN_1,
                              GPIO_PIN_4, GPIO_PIN_5, GPIO_PIN_9, GPIO_PIN_11};

// Digit COM pins
GPIO_TypeDef* com_ports[4] = {GPIOB, GPIOB, GPIOB, GPIOB};
uint16_t      com_pins[4]  = {GPIO_PIN_4, GPIO_PIN_5, GPIO_PIN_6, GPIO_PIN_3};

// 7-seg digit map (abcdefg, dp = bit7)
const uint8_t SEG_MAP[10] = {
    0b0111111, // 0
    0b0000110, // 1
    0b1011011, // 2
    0b1001111, // 3
    0b1100110, // 4
    0b1101101, // 5
    0b1111101, // 6
    0b0000111, // 7
    0b1111111, // 8
    0b1101111  // 9
};

void SystemClock_Config(void);
static void MX_GPIO_Init(void);

int32_t toQ12(float val) {
    return (int32_t)(val * (1 << Q12_SHIFT));
}

void write_digit(uint8_t pos, uint8_t num, uint8_t dp) {
    uint8_t seg = SEG_MAP[num];
    if (dp) seg |= 0x80; // add decimal point

    for (uint8_t i=0; i<8; i++) {
        GPIO_PinState state;
        if (!COMMON_ANODE)
            state = (seg & (1<<i)) ? GPIO_PIN_RESET : GPIO_PIN_SET;
        else
            state = (seg & (1<<i)) ? GPIO_PIN_SET : GPIO_PIN_RESET;

        HAL_GPIO_WritePin(seg_ports[i], seg_pins[i], state);
    }

    HAL_GPIO_WritePin(com_ports[pos], com_pins[pos],
                      COMMON_ANODE ? GPIO_PIN_RESET : GPIO_PIN_SET);
    HAL_Delay(3); // short refresh
    HAL_GPIO_WritePin(com_ports[pos], com_pins[pos],
                      COMMON_ANODE ? GPIO_PIN_SET : GPIO_PIN_RESET);
}

// Format number into 4 digits + decide decimal point
void format_display(int32_t q12_val, uint8_t digits[4], int8_t *dp_pos) {
    float value = (float)q12_val / (1 << Q12_SHIFT);
    uint16_t scaled;

    if (value < 10.0f) {           // show 3 decimals (x.xxx)
        scaled = (uint16_t)(value * 1000);
        *dp_pos = 0;  // dp after digit0
    } else if (value < 100.0f) {   // show 2 decimals (xx.xx)
        scaled = (uint16_t)(value * 100);
        *dp_pos = 1;  // dp after digit1
    } else if (value < 1000.0f) {  // show 1 decimal (xxx.x)
        scaled = (uint16_t)(value * 10);
        *dp_pos = 2;  // dp after digit2
    } else {                       // integer only (xxxx)
        scaled = (uint16_t)(value);
        *dp_pos = -1; // no dp
    }

    digits[0] = (scaled / 1000) % 10;
    digits[1] = (scaled / 100) % 10;
    digits[2] = (scaled / 10) % 10;
    digits[3] = scaled % 10;
}

int main(void) {
    HAL_Init();
    SystemClock_Config();
    MX_GPIO_Init();

    int32_t counter_q12 = 0;
    float step_value = 0.05f;   // default increment step

    while (1) {
        uint8_t digits[4];
        int8_t dp_pos;

        format_display(counter_q12, digits, &dp_pos);

        for (uint8_t i=0; i<4; i++) {
            uint8_t dp = (i == dp_pos);
            write_digit(i, digits[i], dp);
        }

        // increment counter by step_value
        counter_q12 += toQ12(step_value);
        if (counter_q12 > toQ12(9999.0f)) {
            counter_q12 = 0;
        }

        HAL_Delay(50); // slow down count
    }
}

/* ===== GPIO Init ===== */
static void MX_GPIO_Init(void) {
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();

    GPIO_InitTypeDef GPIO_InitStruct = {0};
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

    for (uint8_t i=0; i<8; i++) {
        GPIO_InitStruct.Pin = seg_pins[i];
        HAL_GPIO_Init(seg_ports[i], &GPIO_InitStruct);
    }
    for (uint8_t i=0; i<4; i++) {
        GPIO_InitStruct.Pin = com_pins[i];
        HAL_GPIO_Init(com_ports[i], &GPIO_InitStruct);
        HAL_GPIO_WritePin(com_ports[i], com_pins[i], GPIO_PIN_RESET);
    }
}