#include <stdint.h>
#include <stdlib.h>
#include "dataflow.h"

/**
 * Args table for all states
 *
 * |-------+-------------------|
 * | Index | Variable          |
 * |-------+-------------------|
 * |     0 | SOR_buf           |
 * |     1 | DOC_buf           |
 * |     2 | data_rbuf         |
 * |     3 | data_routing_ptr  |
 * |     4 | data_src_idx_rbuf |
 * |     5 | data_len_buf      |
 * |     6 | cmd_routing_buf   |
 * |     7 | cmd_routing_ptr   |
 * |     8 | cmd_src_idx_rbuf  |
 * |     9 | cmd_dst_idx_rbuf  |
 * |-------+-------------------|
 *
 */


int DF_func_idle(p_stream_t stream, void **args)
{
    int df_state = DF_IDLE;
    uint8_t SOR_code = *(uint8_t*)args[0];
    uint8_t SOR_buf[2] = {SOR_code, 0x0};
    
    if (stream.write(SOR_buf, 2, NULL, &stream) != 0) {
	df_state = DF_STATE_FAIL;
    }
    else {
	switch (SOR_code) {
	case SLAVE_TX:
	    df_state = DF_RX_DOC;
	    break;
	case SLAVE_RX_DATAPOINT:
	    df_state = DF_TX_DATA;
	    break;
	case SLAVE_RX_COMMAND:
	    df_state = DF_TX_CMD;
	    break;
	default:
	    df_state = DF_STATE_FAIL;
	    break;
	}
    }
    return df_state;
}

int DF_func_rx_doc(p_stream_t stream, void **args)
{
    int df_state = DF_RX_DOC;
    args[1] = malloc(sizeof(uint8_t)*4);
    
    if (stream.read(args[1], 4, NULL, &stream) != 0) {
	free(args[1]);
	df_state = DF_STATE_FAIL;
    }
    else {
	switch (((uint8_t*)args[1])[1]) {
	case 0x0:
	    df_state = DF_SUCCESS;
	    break;
	case DATA:
	    df_state = DF_TX_CTS;
	    break;
	case CMD_UNICAST:
	    df_state = DF_TX_CTS;
	    break;
	default:
	    df_state = DF_STATE_FAIL;
	    break;
	}
    }
    return df_state;
}

int DF_func_tx_cts(p_stream_t stream, void **args)
{
    int df_state = DF_TX_CTS;
    uint8_t CTS_buf[2] = {0x2, 0xFF};
    uint8_t *DOC_buf = args[1];
    if (stream.write(CTS_buf, 2, NULL, &stream) != 0) {
	df_state = DF_FAIL;
    }
    else {
	switch (DOC_buf[1]) {
	case DATA:
	    df_state = DF_RX_DATA;
	    break;
	case CMD_UNICAST:
	    df_state = DF_RX_CMD;
	    break;
	}
    }
    return df_state;
}

int DF_func_rx_data(p_stream_t stream, void **args)
{
    int df_state = DF_RX_DATA;
    uint8_t **data_rbuf = (uint8_t**)args[2];
    uint8_t *data_routing_ptr = (uint8_t*)args[3];
    uint8_t *DOC_buf = (uint8_t*)args[1];
    uint8_t data_len = DOC_buf[3];
    
    data_rbuf[*data_routing_ptr] = malloc(sizeof(uint8_t)*data_len);
    
    if (stream.read(data_rbuf[*data_routing_ptr], data_len, NULL, &stream) != 0) {
	df_state = DF_STATE_FAIL;
	free(data_rbuf[*data_routing_ptr]);
    }
    else {
	uint8_t* data_src_idx_rbuf = (uint8_t*)args[4];
	uint8_t* data_len_buf = (uint8_t*)args[5];
	data_src_idx_rbuf[*data_routing_ptr] = 0;
	/* data_src_idx_rbuf[data_routing_ptr] = dev_idx; */
	data_len_buf[*data_routing_ptr] = data_len;
	data_routing_ptr++;
	df_state = DF_STATE_SUCCESS;
    }
    return df_state;
}

int DF_func_rx_cmd(p_stream_t stream, void **args)
{
    int df_state = DF_RX_CMD;
    uint8_t *DOC_buf = args[1];
    uint8_t data_len = DOC_buf[3], cmd_dest = DOC_buf[0];

    uint8_t **cmd_routing_buf = (uint8_t**)args[6];
    uint8_t *cmd_routing_ptr = (uint8_t*)args[7];

    uint8_t* cmd_src_idx_rbuf = (uint8_t*)args[8];
    uint8_t* cmd_dst_idx_rbuf = (uint8_t*)args[9];
    
    cmd_routing_buf[*cmd_routing_ptr] = (uint8_t*)malloc(sizeof(uint8_t)*data_len);
    
    if (stream.read(cmd_routing_buf[*cmd_routing_ptr], data_len, NULL, &stream) != 0) {
	free(cmd_routing_buf[*cmd_routing_ptr]);
	df_state = DF_STATE_FAIL;
    }
    else {	
	cmd_src_idx_rbuf[*cmd_routing_ptr] = 0;
	cmd_dst_idx_rbuf[*cmd_routing_ptr] = cmd_dest;
	cmd_routing_ptr++;		
	df_state = DF_STATE_SUCCESS;
    }
    return df_state;
}

int DF_func_tx_data(p_stream_t stream, void **args)
{
    int df_state = DF_RX_DATA;
    uint8_t **MDR_rbuf = (uint8_t**)args[10];
    uint8_t *MDR_len_buf = (uint8_t*)args[12];    
    uint8_t *data_len_buf = (uint8_t*)args[5];
    uint8_t rbuf_idx = *(uint8_t*)args[11];
    uint8_t **data_rbuf = (uint8_t**)args[2];
    uint8_t CTS_buf[2];
    uint8_t data_MDR_len_buf[4] = {0, MDR_len_buf[rbuf_idx], 0, data_len_buf[rbuf_idx]};
    
    if (stream.read(CTS_buf, 2, NULL, &stream) != 0)
	df_state = DF_STATE_FAIL;
    else if (CTS_buf[1] != 0x1)
	df_state = DF_STATE_FAIL;
    if (df_state != DF_STATE_FAIL) {
	if (stream.write(data_MDR_len_buf, 4, NULL, &stream) != 0)
	    df_state = DF_STATE_FAIL;
	else {
	    if (stream.read(CTS_buf, 2, NULL, &stream) != 0)
		df_state = DF_STATE_FAIL;
	    else if (CTS_buf[1] != 0x1)
		df_state = DF_STATE_FAIL;
	}
    }
    if (df_state != DF_STATE_FAIL) {
	if (stream.write(MDR_rbuf[rbuf_idx], MDR_len_buf[rbuf_idx], NULL, &stream) == 0 &&
	    stream.write(data_rbuf[rbuf_idx], data_len_buf[rbuf_idx], NULL, &stream) == 0)
	    df_state = DF_STATE_SUCCESS;
	else
	    df_state = DF_STATE_FAIL;
    }
    return df_state;
}

int DF_func_tx_cmd(p_stream_t stream, void **args)
{
    /* TODO This function will not work unless there is a cmd_len buffer */
    uint8_t rbuf_idx = *(uint8_t*)args[11];   
    uint8_t **cmd_routing_buf = (uint8_t**)args[6];
    
    return DF_STATE_FAIL;
}