#include <stdio.h>
#include <string.h>

#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"
#include "hardware/flash.h"
#include "hardware/sync.h"

// Test-Sektor: weit weg von allem anderen. Pico 2W hat 4 MB Flash.
// 0xE0000 = 896 KB Offset. Wuerde im OTA-Layout knapp hinter der App liegen,
// hier aber unbenutzt.
#define TEST_FLASH_OFFSET 0xE0000u
#define TEST_FLASH_ADDR   (XIP_BASE + TEST_FLASH_OFFSET)

static void blink_forever(uint32_t on_ms, uint32_t off_ms) {
    while (true) {
        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, true);
        sleep_ms(on_ms);
        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, false);
        sleep_ms(off_ms);
    }
}

static void hexdump16(const uint8_t *p) {
    for (int i = 0; i < 16; i++) {
        printf("%02x ", p[i]);
    }
    printf("\n");
}

int main(void) {
    stdio_init_all();
    sleep_ms(3000);  // Host-Terminal Zeit zum Verbinden

    if (cyw43_arch_init()) {
        while (true) {
            tight_loop_contents();
        }
    }

    printf("\n=== flashwrite sanity test ===\n");
    printf("Test-Sektor: XIP-Adresse 0x%08x (Flash-Offset 0x%05x)\n",
           (unsigned)TEST_FLASH_ADDR, (unsigned)TEST_FLASH_OFFSET);
    printf("FLASH_SECTOR_SIZE=%u  FLASH_PAGE_SIZE=%u\n",
           (unsigned)FLASH_SECTOR_SIZE, (unsigned)FLASH_PAGE_SIZE);

    const uint8_t *flash = (const uint8_t *)TEST_FLASH_ADDR;

    printf("Vor erase:    ");
    hexdump16(flash);

    printf("Erase 4 KB...\n");
    uint32_t irq = save_and_disable_interrupts();
    flash_range_erase(TEST_FLASH_OFFSET, FLASH_SECTOR_SIZE);
    restore_interrupts(irq);

    printf("Nach erase:   ");
    hexdump16(flash);

    for (uint32_t i = 0; i < FLASH_SECTOR_SIZE; i++) {
        if (flash[i] != 0xFF) {
            printf("FEHLER: Erase unvollstaendig bei +%lu: 0x%02x\n", i, flash[i]);
            blink_forever(50, 50);
        }
    }
    printf("Erase verifiziert (alle 4096 Byte = 0xFF).\n");

    uint8_t pattern[FLASH_PAGE_SIZE];
    for (uint32_t i = 0; i < FLASH_PAGE_SIZE; i++) {
        pattern[i] = (uint8_t)(i ^ 0x5A);
    }

    printf("Program 256 Byte...\n");
    irq = save_and_disable_interrupts();
    flash_range_program(TEST_FLASH_OFFSET, pattern, FLASH_PAGE_SIZE);
    restore_interrupts(irq);

    printf("Nach program: ");
    hexdump16(flash);

    if (memcmp(flash, pattern, FLASH_PAGE_SIZE) == 0) {
        printf("ERFOLG: Pattern korrekt im Flash.\n");
        printf("LED: schnelles Blinky 200/200.\n");
        blink_forever(200, 200);
    } else {
        printf("FEHLER: Pattern-Mismatch.\n");
        for (uint32_t i = 0; i < FLASH_PAGE_SIZE; i++) {
            if (flash[i] != pattern[i]) {
                printf("  Diff bei +%lu: got 0x%02x, expected 0x%02x\n",
                       i, flash[i], pattern[i]);
                break;
            }
        }
        blink_forever(50, 50);
    }
}