#include <stdio.h>
#include <io.h>
#include <unistd.h>
#include "system.h"
#include "altera_avalon_mailbox_simple.h"

#define SLOW 500000
#define FAST 250000

static int chg_val = SLOW;
static altera_avalon_mailbox_dev *mbox_rx, *mbox_tx;


static int tx_mailbox(int *message)
{
    int ret;
    int timeout = 0x10000;      /* timeout counter*/

    ret = altera_avalon_mailbox_send(mbox_tx, (void*)message, timeout, POLL);
    if (ret != 0) {
        printf("ERROR: Mailbox send failed!!, ret=%d, msg=%d\n", ret, message[0]);
        return -1;
    }

    return 0;
}


static void rx_callback(void *rx_message)
{
    int speed = *((int*)rx_message);
    int tx_message[2] = {-1, 0};     /* [0]: command (change count), [1]: extended pointer (unused) */

    /* Check value */
    if ((speed < 100) || (speed > 10000)) {
        printf("ERROR: Speed range from 100 [ms] to 10000 [ms], specific value=%d[ms]\n", speed);

        /* Notify to HPS */
        tx_mailbox(tx_message);
        return;
    }

    /* Set value */
    chg_val = speed * 1000;
}


int main(void)
{
	int period = SLOW;
    int ret;
    int message[2] = {0, 0};    /* [0]: command (change count), [1]: extended pointer (unused) */

    printf("Hello from Nios V, start!!\n");

    /* Open mailbox (RX) */
    mbox_rx = altera_avalon_mailbox_open(MAILBOX_SIMPLE_HPS2NIOS_NAME,
                                         NULL,
                                         rx_callback);
    if (mbox_rx == NULL) {
        printf("ERROR: mbox_rx is NULL!!\n");
        return -1;
    }
    /* Open mailbox (TX) */
    mbox_tx = altera_avalon_mailbox_open(MAILBOX_SIMPLE_NIOS2HPS_NAME,
                                         NULL,
                                         NULL);
    if (mbox_tx == NULL) {
        printf("ERROR: mbox_tx is NULL!!\n");
        return -1;
    }

    while(1)
    {
        if (chg_val != period) {
            printf("Change speed, %d -> %d\n", period, chg_val);
            period = chg_val;

            /* Notify to HPS */
            message[0]++;
            ret = tx_mailbox(message);
            if (ret != 0) {
                return -1;
            }
        }

        IOWR(SUBSYS_PERIPH_LED_PIO_BASE, 0, 0x55);
        usleep(period);
        IOWR(SUBSYS_PERIPH_LED_PIO_BASE, 0, 0x2A);
        usleep(period);
        IOWR(SUBSYS_PERIPH_LED_PIO_BASE, 0, 0x00);
        usleep(period);
        IOWR(SUBSYS_PERIPH_LED_PIO_BASE, 0, 0x7F);
        usleep(period);
    }

    /* Close mailbox */
    altera_avalon_mailbox_close(mbox_rx);
    altera_avalon_mailbox_close(mbox_tx);

    return 0;
}