#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;
    uint32_t SOR_code = *(uint32_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 {
	if (SOR_code == SLAVE_TX) {
	    df_state = DF_RX_DOC;
	}
	else if (SOR_code == SLAVE_RX_DATAPOINT) {
	    df_state = DF_TX_DATA;
	}
	else if (SOR_code == SLAVE_RX_COMMAND) {
	    df_state = DF_TX_CMD;
	}
	else {
	    df_state = DF_STATE_FAIL;
	}
    }
    return df_state;
}

int DF_func_rx_doc(p_stream_t stream, void **args)
{
    uint8_t DOC_buf[4];
    int df_state = DF_RX_DOC;
    if (stream.read(DOC_buf, 4, NULL, &stream) != 0) {
	df_state = DF_STATE_FAIL;
    }
    else {
	if (DOC_buf[0] == 0x0) {
	    /* Do nothing DOC */
	    df_state = DF_SUCCESS;
	}
	else if (DOC_buf[1] == DATA) {
	    df_state = DF_RX_CTS;
	    args[1] = malloc(sizeof(uint8_t*));
	    args[1] = &DOC_buf; /* Put pointer to DOC buf in this arg */
	    
	}
	else if (DOC_buf[1] == CMD_UNICAST) {
	    df_state = DF_RX_CTS;
	    args[1] = malloc(sizeof(uint8_t*));
	    args[1] = &DOC_buf; /* Put pointer to DOC buf in this arg */
	}
	else {
	    df_state = DF_STATE_FAIL;
	}
    }
    return df_state;
}

int DF_func_rx_cts(p_stream_t stream, void **args)
{
    int df_state = DF_RX_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 {
	if (DOC_buf[1] == DATA) { 
	    df_state = DF_RX_DATA;
	}
	else {
	    if (DOC_buf[1] == CMD_UNICAST) {
		df_state = DF_RX_CMD;
	    }
	}
    }
    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_SUCCESS;
    }
    return df_state;
}

int DF_func_tx_data(p_stream_t stream, void **args)
{
    /* TODO Do this after MDR is cached after handshake, let's not encode it again here */
}

int DF_func_tx_cmd(p_stream_t stream, void **args)
{
    
}