/*
 * fw_app.c
 *
 * Copyright (C) 2019 Sylvain Munaut
 * All rights reserved.
 *
 * LGPL v3+, see LICENSE.lgpl3
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 3 of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include "console.h"
#include "led.h"
#include "mini-printf.h"
#include "spi.h"
#include "utils.h"
#include "registers.h"

#define FLASH_MAILBOX_STORAGE 0x0F0000

/*
double dupa;
float kupa;
*/

static uint32_t copy_array[16];

static void print_mailbox_contents()
{
    int i = 0;
    printf("Mailbox contents:\n");
    for(i = 0; i < 16; ++i)
        printf("Mailbox[%d]: %d\n", i, mailbox_regs->data[i]);
}

static void store_mailbox_regs_flash()
{
    // Copy current mailbox array to a temporary array for bulk memory write
    // our Wishbone implementation does not support bulk writes
    for (int i = 0; i < 16; ++i)
        copy_array[i] = mailbox_regs->data[i];

    flash_write_enable();
    flash_sector_erase(FLASH_MAILBOX_STORAGE);
    // Wait for flash to finish (poll status register)
    while (flash_read_sr() & 0x01) /* WIP bit */;
    flash_write_enable();

    flash_page_program((void*)copy_array, FLASH_MAILBOX_STORAGE, sizeof(copy_array));
    while (flash_read_sr() & 0x01) /* WIP bit */;
}

static void read_mailbox_regs_flash()
{
    flash_read((void*)copy_array, FLASH_MAILBOX_STORAGE, sizeof(copy_array));

    // Copy data to actual mailbox
    for (int i = 0; i < 16; ++i)
        mailbox_regs->data[i] = copy_array[i];
}

static void dummy_write_to_flash()
{
    printf("Dummy writing to flash\n");
    int i = 0;
    for (i = 0; i < 16; ++i)
    {
        mailbox_regs->data[i] = 0x7890;
    }

    print_mailbox_contents();
    store_mailbox_regs_flash();
}

static void
serial_no_init()
{
	uint8_t buf[8];

	flash_manuf_id(buf);
	printf("Flash Manufacturer : %s\n", hexstr(buf, 3, true));

	flash_unique_id(buf);
	printf("Flash Unique ID    : %s\n", hexstr(buf, 8, true));

    //dummy_write_to_flash();

    /* Testing floats TODO: test if calculation is correct
    dupa = 3.1415;
    kupa = 2.74f;

    dupa *= kupa;

    printf("Pi * e: %.3f\n", dupa);
    */
}

//static char _printf_buf[128];

static void write_period1()
{
    printf("Writing period1 0x1\n");
    mailbox_regs->regs.period1 = 0x1;
    printf("Done writing\n");

    print_mailbox_contents();
    store_mailbox_regs_flash();
}

static void write_period1_2()
{
    printf("Writing period1 0x2\n");
    mailbox_regs->regs.period1 = 0x2;
    printf("Done writing\n");

    print_mailbox_contents();
    store_mailbox_regs_flash();
}

static void write_period1_4()
{
    printf("Writing period1 0x4\n");
    mailbox_regs->regs.period1 = 0x4;
    printf("Done writing\n");
    printf("Reading now \n");
    printf("value: %d\n", mailbox_regs->regs.period1);
    print_mailbox_contents();
    store_mailbox_regs_flash();
}

static void clear_period1()
{
    printf("Clearing period1 val: %d\n", mailbox_regs->regs.period1);
    mailbox_regs->regs.period1 = 0x0;
    printf("Done clearing\n");
}

static void write_delay1()
{
    printf("Writing delay1 0x1234\n");
    mailbox_regs->regs.delay1 = 0x1234;
    printf("Done writing\n");
    store_mailbox_regs_flash();
}

static void clear_delay1()
{
    printf("Clearing delay1 val: %d\n", mailbox_regs->regs.delay1);
    mailbox_regs->regs.delay1 = 0x0;
    printf("Done clearing\n");
}

static void print_flash_contents()
{
    read_mailbox_regs_flash();
    print_mailbox_contents();
}

static void
boot_dfu(void)
{
	/* Boot firmware */
	volatile uint32_t *boot = (void*)0x80000000;
	*boot = (1 << 2) | (1 << 0);
}

void
usb_dfu_rt_cb_reboot(void)
{
        boot_dfu();
}

void main()
{
	int cmd = 0;

	/* Init console IO */
	console_init();
	puts("Booting App image..\n");

	/* LED */
	led_init();
	led_color(48, 96, 5);
	led_blink(true, 200, 1000);
	led_breathe(true, 100, 200);
	led_state(true);

	/* SPI */
	spi_init();

	/* Enable USB directly */
	serial_no_init();

	/* Main loop */
	while (1)
	{
		/* Prompt ? */
		if (cmd >= 0)
			printf("Command> ");

		/* Poll for command */
		cmd = getchar_nowait();

		if (cmd >= 0) {
			if (cmd > 32 && cmd < 127) {
				putchar(cmd);
				putchar('\r');
				putchar('\n');
			}

			switch (cmd)
			{
			case 'b':
				boot_dfu();
				break;
            case 'w':
                write_period1();
                break;
            case 'k':
                clear_period1();
                break;
            case 'a':
                write_period1_2();
                break;
            case 's':
                write_period1_4();
                break;
            case 'o':
                write_delay1();
                break;
            case 'm':
                clear_delay1();
                break;
            case 'f':
                print_flash_contents();
                break;
			default:
				break;
			}
		}
	}
}