/**
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <stdio.h>
#include <stdlib.h>

#include "pico/stdlib.h"
#include "LiteX/SPIBone.h"
#include "LiteX/SDRAM.h"
#include "helpers.h"
#include "rust_hal_types.h"
#include "fastPRNG.h"
#include "cli.h"

#define READBUF_LEN 64
char readbuf[READBUF_LEN + 1] = { '\0' };
uint32_t readbuf32[READBUF_LEN + 1] = { '\0' };

void cli_println(char * str);
void single_readb_test(char * buf, uint32_t addr, size_t read_len, size_t buf_len);
void multiple_readb_test(char * buf, uint32_t * tmp_buf, uint32_t addr, size_t read_len);
cli_status_t test_sdram(int argc, char **argv);
cli_status_t set_rdiv(int argc, char **argv);
cli_status_t set_wdiv(int argc, char **argv);
cli_status_t set_chks(int argc, char **argv);
cli_status_t set_slen(int argc, char **argv);

periph_status_states PERIPHERALS_STATUS = PERIPH_STATUS_STATES_UNKNOWN;

cmd_t cmds[5] = {
    {
        .cmd = "sdram",
        .func = test_sdram,
    },
    {
        .cmd = "rdiv",
        .func = set_rdiv,
    },
    {
        .cmd = "wdiv",
        .func = set_wdiv,
    },
    {
        .cmd = "chks",
        .func = set_chks,
    },
    {
        .cmd = "slen",
        .func = set_slen,
    },
};

int main() {  
    stdio_init_all();
    // stellar_spi * handle;
    ext_clock * clk;

    int clk_ret = si_init(&clk);

    // Must come after Rust code, because Rust code will reinit SIO.
    gpio_init(GPIO_FPGA_SPI_CS);
	gpio_put(GPIO_FPGA_SPI_CS, 1);
	gpio_set_dir(GPIO_FPGA_SPI_CS, GPIO_OUT);

    sleep_ms(1000);
    if(clk_ret == EXT_CLOCK_ERROR_SUCCESS)
    {
        printf("Hardware init okay.\n");
    }

    clk_ret = si_start(clk, 33333333);
    printf("si_start ret: %d\n", clk_ret);    

    // readbuf[READBUF_LEN] = '\0';
    
    // // multiple_readb_test(readbuf, readbuf32, 0x00000800, READBUF_LEN);
    // single_readb_test(readbuf, 0x00000800, 1024, READBUF_LEN);
    // printf("Read data: %s\n", readbuf);

    printf("Initializing SDRAM\r\n");
    InitializeSDRAM();
    // test_sdram();

    cli_status_t rslt = CLI_OK;

    cli_t cli = {
        .println = cli_println,
        .cmd_tbl = cmds,
        .cmd_cnt = sizeof(cmds) / sizeof(cmd_t)
    };

    if((rslt = cli_init(&cli)) != CLI_OK)
    {
        printf("CLI: Failed to initialise");
    }

    while(true)
    {
        int chr = getchar_timeout_us(1000);
        if(chr !=  PICO_ERROR_TIMEOUT)
        {
            putchar(chr);
            cli_put(&cli, (char) chr);
            cli_process(&cli);
        }
    }

    return 0;
}

void cli_println(char * str)
{
    printf("%s", str);
}

void single_readb_test(char * buf, uint32_t addr, size_t read_len, size_t buf_len)
{
    for(int i = 0; i < read_len; i++)
    {
        uint32_t data;

        spi_bone_read32_blocking(addr + i*4, &data, 1);
        if(i < buf_len)
        {
            buf[i] = (char) data;
        }
    }
}

void multiple_readb_test(char * buf, uint32_t * tmp_buf, uint32_t addr, size_t read_len)
{
    spi_bone_read32_blocking(addr, tmp_buf, read_len);

    for(int i = 0; i < read_len / 4; i++)
    {
        buf[i] = (char) tmp_buf[i];
    }
}

float SPIBONE_READ_CLKDIV = 5.0f;
float SPIBONE_WRITE_CLKDIV = 1.6f;
float SPIBONE_CLKDIV = SPIBONE_WRITE_CLKDIV;
int SPIBONE_CHUNK_SIZE = 1;
int SPIBONE_SDRAM_LEN = 0x10000;

uint32_t rxbuf[128] = {0};
uint32_t txbuf[128] = {0};

cli_status_t set_rdiv(int argc, char **argv)
{
    if(argc < 2)
    {
        return CLI_E_INVALID_ARGS;
    }

    SPIBONE_READ_CLKDIV = atof(argv[1]);

    return CLI_OK;
}

cli_status_t set_wdiv(int argc, char **argv)
{
    if(argc < 2)
    {
        return CLI_E_INVALID_ARGS;
    }

    SPIBONE_WRITE_CLKDIV = atof(argv[1]);

    return CLI_OK;
}

cli_status_t set_chks(int argc, char **argv)
{
    if(argc < 2)
    {
        return CLI_E_INVALID_ARGS;
    }

    SPIBONE_CHUNK_SIZE = atoi(argv[1]);

    return CLI_OK;
}

cli_status_t set_slen(int argc, char **argv)
{
    if(argc < 2)
    {
        return CLI_E_INVALID_ARGS;
    }

    SPIBONE_SDRAM_LEN = atoi(argv[1]);

    return CLI_OK;
}

cli_status_t test_sdram(int argc, char **argv)
{
    float old_clkdiv = SPIBONE_CLKDIV;
    static fastPRNG::fastXS32 fastR(0x12345678);
    // while (true)
    // {
    // SPIBONE_CLKDIV = old_clkdiv - 0.01f;

    printf("Setting write clock div to %9.6f %d\r\n", SPIBONE_WRITE_CLKDIV, (uint32_t)(SPIBONE_WRITE_CLKDIV * 1000));
    printf("Setting read clock div to %9.6f %d\r\n", SPIBONE_READ_CLKDIV, (uint32_t)(SPIBONE_READ_CLKDIV * 1000));
    printf("Will test %d words\r\n", SPIBONE_SDRAM_LEN / 4);

    SPIBONE_CLKDIV = SPIBONE_WRITE_CLKDIV;
    printf("Testing SDRAM (Short Simple - %d Word Chunk Write)\r\n", SPIBONE_CHUNK_SIZE);
    for (uint32_t i = 0; i < SPIBONE_SDRAM_LEN; i += 4 * SPIBONE_CHUNK_SIZE)
    {
        for (uint32_t j = 0; j < SPIBONE_CHUNK_SIZE; j++)
        {
            txbuf[j] = i + j*4;
        }

        spi_bone_write32_blocking(0x40000000 + i, txbuf, SPIBONE_CHUNK_SIZE);
    }

    SPIBONE_CLKDIV = SPIBONE_READ_CLKDIV;
    printf("Testing SDRAM (Short Simple - %d Word Chunk Read)\r\n", SPIBONE_CHUNK_SIZE);
    for (uint32_t i = 0; i < SPIBONE_SDRAM_LEN; i += 4 * SPIBONE_CHUNK_SIZE)
    { // 800000
        spi_bone_read32_blocking(0x40000000 + i, rxbuf, SPIBONE_CHUNK_SIZE);

        for (uint32_t j = 0; j < SPIBONE_CHUNK_SIZE; j++)
        {
            uint32_t offs = i + j * 4;
            if (rxbuf[j] != offs)
            {
                printf("Memory read failed at 0x%08X (%08X)\r\n", 0x40000000 + offs, rxbuf[j]);
                return CLI_OK;
            }
        }
    }

    SPIBONE_CLKDIV = SPIBONE_WRITE_CLKDIV;
    printf("Testing SDRAM (Short Random - %d Word Chunk Write)\r\n", SPIBONE_CHUNK_SIZE);
    fastR.seed(0x12345678);
    for (uint32_t i = 0; i < SPIBONE_SDRAM_LEN; i += 4 * SPIBONE_CHUNK_SIZE)
    {
        for (uint32_t j = 0; j < SPIBONE_CHUNK_SIZE; j++)
        {
            txbuf[j] = fastR.xoshiro128p();
        }

        spi_bone_write32_blocking(0x40000000 + i, txbuf, SPIBONE_CHUNK_SIZE);
    }

    SPIBONE_CLKDIV = SPIBONE_READ_CLKDIV;
    printf("Testing SDRAM (Short Random - %d Word Chunk Read)\r\n", SPIBONE_CHUNK_SIZE);
    fastR.seed(0x12345678);
    for (uint32_t i = 0; i < SPIBONE_SDRAM_LEN; i += 4 * SPIBONE_CHUNK_SIZE)
    {
        spi_bone_read32_blocking(0x40000000 + i, rxbuf, SPIBONE_CHUNK_SIZE);

        for (uint32_t j = 0; j < SPIBONE_CHUNK_SIZE; j++)
        {
            uint32_t offs = i + j * 4;
            uint32_t random = fastR.xoshiro128p();
            if (rxbuf[j] != random)
            {
                printf("Memory read failed at 0x%08X (%08X %08X)\r\n", 0x40000000 + offs, random, rxbuf[j]);
                return CLI_OK;
            }
        }
    }

    return CLI_OK;
    // }
}