/* 
 * TODO Do conditional includes based on which target we are building for.
 *   Actually, target-specific includes should go in the port folder
 *   This should be specified in the master config file
 * TODO This file should be moved to the posix port folder
 *
 */

/* FreeRTOS includes. */
/* #include "FreeRTOS_POSIX.h" */

/* System headers. */
#include <stdbool.h>
#include <string.h>
#include <stdio.h>

/* Library includes */
#include <pb_encode.h>
#include <pb_decode.h>

/* (Ideally) platform-agnostic project includes. */
#include "master_posix.h"
#include "main.h"
#include "devices.h"
#include "config.h"
#include "dataflow.h"
#include "handshake.pb.h"
#include "data.pb.h"
#include "stream.h"
#include "stream_stdio.h"
#include "port.h"
#include "handshake.h"

/* FreeRTOS+POSIX. should go in the port folder */
/* #include "FreeRTOS_POSIX/pthread.h" */
/* #include "FreeRTOS_POSIX/mqueue.h" */
/* #include "FreeRTOS_POSIX/time.h" */
/* #include "FreeRTOS_POSIX/fcntl.h" */
/* #include "FreeRTOS_POSIX/errno.h" */

/* Constants. */
#define LINE_BREAK    "\r\n"
#define device_MDR s2m_MDR_response
#define GET_IDX_FROM_ADDR(i2c_addr) (i2c_addr>>1)-1
#define GET_ADDR_FROM_IDX(idx)      (idx+1)<<1
#define GET_BIT_FROM_IDX(a, b) a[b>>5]&(1<<(b%32))
#define SET_BIT_FROM_IDX(a, b) a[b>>5]|=(1<<(b%32))
#define COUNTOF(__BUFFER__)   (sizeof(__BUFFER__) / sizeof(*(__BUFFER__)))

#define STREAM_INIT(PERIPH, vptr, sptr) \
    sptr.read=&read_##PERIPH;		\
    sptr.write=&write_##PERIPH;		\
    sptr.props=vptr;

#define BUS_DEVICE_LIMIT 2

hs_func_t hs_jumptable[NUM_STATES] = {
    HS_STATE_TABLE(EXPAND_AS_JUMPTABLE)
};

device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
uint32_t allocated[4]={0};
uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE};
uint8_t data_idx;

uint8_t *data_rbuf[ROUTING_BUFSIZE];        /*< Buffer to store data to be routed */
uint8_t *cmd_routing_buf[ROUTING_BUFSIZE];  /*< Buffer to store commands to be routed */

uint8_t    data_src_idx_rbuf[ROUTING_BUFSIZE];   /*< Index information for data source */
uint8_t    cmd_src_idx_rbuf[ROUTING_BUFSIZE];    /*< Index information for command source */
uint8_t    cmd_dst_idx_rbuf[ROUTING_BUFSIZE];    /*< Index information for command dest */

uint32_t    data_len_buf[ROUTING_BUFSIZE];

uint32_t   data_routing_ptr = 0;     /*< Pointer to tail of both data and data index buffers */
uint32_t   cmd_routing_ptr = 0;      /*< Pointer to tail of cmd and cmd index buffers */

p_stream_t stream;

static void *handshake_func(void * pvArgs);
static void *dataflow_func(void *pvArgs);
static void *routing_func(void *pvArgs);

dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t routing_buf_idx);
bool routing(void);
bool cmd_routing(void);
uint8_t get_CTS(uint8_t i2c_addr);
bool todo_hs_or_not_todo_hs(uint8_t i2c_addr);
state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status);
bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);
bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void **args);
bool master_encode_MDR_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);


/*-----------------------------------------------------------*/

/**
 * @brief Main function; 
 *
 * See the top of this file for detailed description.
 */
void vStartPOSIXMaster(void *pvParams)
{
    pthread_t handshake_thread, dataflow_thread, routing_thread;
    p_stream_t device_streams[2];
    STREAM_INIT(STDIO, NULL, device_streams[0]);
    STREAM_INIT(STDIO, NULL, device_streams[1]);

    /* This is the global stream that is being used by handhsake and dataflow */
    STREAM_INIT(STDIO, NULL, stream);
    
    pthread_create(&handshake_thread, NULL, handshake_func, NULL);
    pthread_create(&dataflow_thread, NULL, dataflow_func, NULL);
    pthread_create(&routing_thread, NULL, routing_func, NULL);


    /* This function will be defined for the port
     * Each port will have a configuration file that includes details about the bus, 
     *     either in a custom format or in a devicetree format (preferred)
     * 
     */
    
    /* Add device-specific stream/thread declerations here, if needed */
    /* ... */
    
    /* This task was created with the native xTaskCreate() API function, so
       must not run off the end of its implementing thread. */
    /* vTaskDelete(NULL); */
}

static void *handshake_func(void * pvArgs)
{    
    printf("Handshake thread started %s", LINE_BREAK);
    for (;;) {
	for (int dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT-1; dev_idx++) {
	    if (todo_hs_or_not_todo_hs(GET_ADDR_FROM_IDX(dev_idx))) {
		int hs_state = HS_STATE_0;
		void **args;
		args = malloc(sizeof(uint8_t*)*2);
		
		uint32_t dev_idx = 0; /*< Do something with this, not relevant anymore */
		
		while (hs_state != HS_STATE_FAIL && hs_state != HS_STATE_SUCCESS)
		    hs_state = hs_jumptable[hs_state](stream, args);
		
		if (hs_state == HS_STATE_SUCCESS) {
		    s2m_MDR_response MDR_res_message = s2m_MDR_response_init_default;
		    MDR_res_message.subscriptions.funcs.decode = decode_subscriptions_callback;
		    MDR_res_message.subscriptions.arg = (void*)dev_idx;
		    pb_istream_t MDR_res_stream = pb_istream_from_buffer(args[1],
									 *(uint16_t*)args[0]);
		    if (!pb_decode(&MDR_res_stream, s2m_MDR_response_fields, &MDR_res_message)) {
			hs_state = HS_STATE_FAIL;
		    }
		    else {
			device_info[dev_idx]              = malloc(sizeof(device_info_t));
			device_info[dev_idx]->i2c_addr    = 0x0;
			device_info[dev_idx]->device_id   = dev_idx;
			device_info[dev_idx]->MDR         = MDR_res_message;		    
			hs_state = HS_REGISTERED;
		    }
		}
	       
		dev_sts[dev_idx] = get_state_from_hs_status(GET_ADDR_FROM_IDX(dev_idx), hs_state);
	    }
	}
    }
    return NULL;
}

static void *dataflow_func(void *pvArgs)
{
    printf("Dataflow thread started %s", LINE_BREAK);
    for (;;) {
	for (int device_idx = 0; device_idx < BUS_DEVICE_LIMIT-1; device_idx++) {
	    if (dev_sts[device_idx] == REGISTERED) {
		device_dataflow(GET_ADDR_FROM_IDX(device_idx), SLAVE_TX, 0);
	    }
	}
    }
    return NULL;
}

static void *routing_func(void *pvArgs)
{
    printf("Routing thread started %s", LINE_BREAK);
    for (;;) {
	routing();
	cmd_routing();
    }
    return NULL;
}

dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, volatile uint8_t rbuf_idx)
{
    uint8_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);    
    dataflow_status_t df_status = DF_IDLE;
    uint8_t CTS_buf[2] = {0x2, 0xFF};
    
    uint8_t DOC_buf[4];
    uint8_t cmd_dest;
    
    uint32_t data_len = 0;

    void **vptr = malloc(sizeof(uint8_t*));
    vptr[0] = malloc(sizeof(uint8_t*));
    vptr[0] = &i2c_addr;
    
    /* TODO Add default values to the CTS message in proto */
#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
    uint8_t debug_buf[128]={0};
#endif
    
    while (df_status != DF_SUCCESS && df_status != DF_FAIL) {
	switch (df_status) {
	case (DF_IDLE):
	{
	    uint8_t SOR_buf[2] = {SOR_code, 0x0};
	    if (stream.write(SOR_buf, 2, vptr, &stream) != 0) {
		df_status = DF_FAIL;
		break;
	    }
	    else {
		if (SOR_code == SLAVE_TX) {
		    df_status = DF_RX_DOC;
		}
		else if (SOR_code == SLAVE_RX_DATAPOINT) {
		    df_status = DF_TX_DATA;
		}
		else if (SOR_code == SLAVE_RX_COMMAND) {
		    df_status = DF_TX_CMD;
		}
	    }
	    break;
	}
	
	case (DF_RX_DOC):
	{
	    if (stream.read(DOC_buf, 4, vptr, &stream) != 0) {
		df_status = DF_FAIL;
		break;
	    }
	    else {
		if (DOC_buf[0] == 0x0) {
		    /* Do nothing DOC; should become redundant once dataflow is initiated using classes */
		    df_status = DF_SUCCESS;
		}
		else if (DOC_buf[1] == DATA) {
		    df_status = DF_CTS;
		    data_len = DOC_buf[3];
		}
		else if (DOC_buf[1] == CMD_UNICAST) {
		    df_status = DF_CTS;
		    cmd_dest  = DOC_buf[0];
		    data_len = DOC_buf[3];
		}
		else if (DOC_buf[1] == CMD_MULTICAST) {
		    /* TODO */
		}
		else if (DOC_buf[1] == CMD_BROADCAST) {
		    /* TODO */
		}
		else {
		    df_status = DF_FAIL;
		}
	    }
	    break;
	}
	
	case (DF_CTS):
	{
	    if (stream.write(CTS_buf, 2, vptr, &stream) != 0) {
		df_status = DF_FAIL;
	    }
	    else {
		if (DOC_buf[1] == DATA) {
		    df_status = DF_RX_DATA;
		}
		else {
		    if (DOC_buf[1] == CMD_UNICAST) {
			df_status = DF_RX_CMD;
		    }
		}
	    }
	    break;
	}
	
	case (DF_RX_DATA):
	{
	    data_rbuf[data_routing_ptr] = malloc(sizeof(uint8_t)*data_len);
	    if (stream.read(data_rbuf[data_routing_ptr], data_len, vptr, &stream) != 0) {
		df_status = DF_FAIL;
		free(data_rbuf[data_routing_ptr]);
		break;
	    }
	    else {
		data_src_idx_rbuf[data_routing_ptr] = dev_idx;
		data_len_buf[data_routing_ptr] = (uint8_t)data_len;
		data_routing_ptr++;
		df_status = DF_SUCCESS;
	    }
	    break;
	}
	
	case (DF_TX_DATA):
	{
	    /* TODO error checking  */
	    /* Will need to package datapoint and MDR to know their lengths
	       Once cached, will not need to do this */

	    /* Do this after handshake to cache ==================================== */
	    uint8_t MDR_buf[128];
	    uint8_t src_device_idx = data_src_idx_rbuf[rbuf_idx];
	    s2m_MDR_response data_src_MDR = device_info[src_device_idx]->MDR;
	    pb_ostream_t MDR_ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
	    data_src_MDR.subscriptions.funcs.encode=master_encode_MDR_callback;
	    pb_encode(&MDR_ostream, s2m_MDR_response_fields, &data_src_MDR);	    
	    uint8_t MDR_len = MDR_ostream.bytes_written;
	    /* ==================================================================== */
	    
	    uint8_t data_len = data_len_buf[rbuf_idx];
	    uint8_t data_MDR_len_buf[4] = {0, MDR_len, 0, data_len};

	    uint8_t status = get_CTS(i2c_addr);

	    if (status != 0 ||
		stream.write(data_MDR_len_buf, 4, vptr, &stream) != 0);
	    
	    status = get_CTS(i2c_addr);	    
	    if (status == 0 &&
		stream.write(MDR_buf, MDR_len, vptr, &stream) == 0 &&
		stream.write(data_rbuf[rbuf_idx], data_len, vptr, &stream) == 0) {
		df_status = DF_SUCCESS;
	    }
	    else {
		df_status = DF_FAIL;
	    }
	    break;
	}
	
	case (DF_RX_CMD):
	{
	    uint8_t *cmd_buf;	    
	    cmd_buf = (uint8_t*)malloc(data_len);
	    
	    if (stream.read(cmd_buf, data_len, vptr, &stream) != 0) {
		free(cmd_buf);
		df_status = DF_FAIL;		
	    }
	    else {
		cmd_routing_buf[cmd_routing_ptr] = malloc(sizeof(uint8_t)*data_len);
		memcpy(cmd_routing_buf[cmd_routing_ptr], cmd_buf, data_len);
		free(cmd_buf);
		
		cmd_src_idx_rbuf[cmd_routing_ptr] = dev_idx;
		cmd_dst_idx_rbuf[cmd_routing_ptr] = cmd_dest;
		cmd_routing_ptr++;
		
		df_status = DF_SUCCESS;
	    }
	    break;
	}
	
	case (DF_TX_CMD):
	{
	    uint8_t data_len = sizeof(cmd_routing_buf[rbuf_idx]);
	    uint8_t len_buf[] = {0x0, data_len, 0x0, 0x0};
	    
	    uint8_t status = get_CTS(i2c_addr);
	    if (status == 0 &&
		stream.write(len_buf, 4, vptr, &stream) == 0) {
		df_status = DF_FAIL;
	    }
	    
	    if (df_status != DF_FAIL) {
		uint8_t status = get_CTS(i2c_addr);
		if (status == 0 &&
		    stream.write(cmd_routing_buf[rbuf_idx], data_len, vptr, &stream) == 0) {
		    df_status = DF_SUCCESS;
		}
		else {
		    df_status = DF_FAIL;
		}
	    }
	    break;
	}
	case DF_SUCCESS:
	case DF_FAIL:
	    break;
	}
    }
    
    return df_status;
}

bool routing(void)
{
    /* This table holds information on where to send each datapoint in the routing buffer  */
    uint32_t routing_table[ROUTING_BUFSIZE][4] = {{0, 0}};
    
    /* Build table with routing information */
    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < data_routing_ptr; rbuf_data_idx++) {
	uint8_t src_module_idx = data_src_idx_rbuf[rbuf_data_idx];
	for (uint8_t dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT; dev_idx++) {
	    if (!(GET_BIT_FROM_IDX(allocated, dev_idx) && 1)) { // No module at this index
		continue;
	    }
	    bool alloc = false;
	    for (uint8_t dev_sub_idx = 0;
		 dev_sub_idx < subs_info[dev_idx]->mod_idx && !alloc; dev_sub_idx++) {
		if (subs_info[dev_idx]->module_ids[dev_sub_idx] ==
		    device_info[src_module_idx]->MDR.module_id) {
		    SET_BIT_FROM_IDX(routing_table[rbuf_data_idx], dev_idx);
		    alloc = true;
		}
	    }
	    /* TODO entity ID, I2C addr and class routing, should go in the if condition above */
	}
    }

    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < data_routing_ptr; rbuf_data_idx++) {
	for (uint8_t device_idx = 0; device_idx < BUS_DEVICE_LIMIT; device_idx++) {
	    if (GET_BIT_FROM_IDX(allocated, device_idx) &&
		GET_BIT_FROM_IDX(routing_table[rbuf_data_idx], device_idx)) {
		device_dataflow(GET_ADDR_FROM_IDX(device_idx), SLAVE_RX_DATAPOINT, rbuf_data_idx);
	    }
	}
	free(data_rbuf[rbuf_data_idx]);
    }

    /* Reset the routing pointer, since all data in buffer should have been routed */    
    data_routing_ptr = 0;
    return true;
}

bool cmd_routing(void)
{
    /* uint32_t routing_table[4]; */
    for (uint8_t rbuf_cmd_idx = 0; rbuf_cmd_idx < cmd_routing_ptr; rbuf_cmd_idx++) {
	uint8_t dst_module_idx = cmd_dst_idx_rbuf[rbuf_cmd_idx];
	for (int dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT; dev_idx++) {
	    if (GET_BIT_FROM_IDX(allocated, dev_idx) &&
		device_info[dev_idx]->MDR.module_id == dst_module_idx) {
	    	device_dataflow(GET_ADDR_FROM_IDX(dev_idx), SLAVE_RX_COMMAND, rbuf_cmd_idx);
	    }
	}
	free(cmd_routing_buf[rbuf_cmd_idx]);
    }
    cmd_routing_ptr = 0;
    return true;
}

bool todo_hs_or_not_todo_hs(uint8_t i2c_addr)
{
    uint8_t device_idx = GET_IDX_FROM_ADDR(i2c_addr);
    state_t device_curr_state = dev_sts[device_idx];
    bool do_hs = false;
    switch(device_curr_state) {
    case NO_HS:
    case CONNECTED:
    case FAILED:
    case OFFLINE:
	do_hs = true;
	break;
    case REGISTERED:
    case NO_DATA:
	do_hs = false;
	break;	
    }
    return do_hs;
}

state_t get_state_from_hs_status(uint16_t device_addr, hs_status_t hs_status)
{
    state_t device_state = OFFLINE;
    switch(hs_status) {
    case IDLE:
    case HS_FAILED:
	device_state = OFFLINE;
	break;
    case HS_MDR_ACK:
    case HS_MDR_CTS:
    case HS_MDR_MDR:
	device_state = FAILED;
	break;
    case HS_REGISTERED:
	device_state = REGISTERED;
	break;    
    }
    return device_state;
}


bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
{
    _subscriptions subs;
    int *subs_idx = (int*)args;

    /* Check is storage is allocated; if not, allocate it */
    if ((GET_BIT_FROM_IDX(allocated, *subs_idx)) == 0) {
	subs_info[*subs_idx] = (subscription_info_t*)malloc(sizeof(subscription_info_t));
	SET_BIT_FROM_IDX(allocated, *subs_idx);
	subs_info[*subs_idx]->mod_idx = subs_info[*subs_idx]->entity_idx =
	    subs_info[*subs_idx]->class_idx = subs_info[*subs_idx]->i2c_idx = 0;
    }
    
    if(!pb_decode(istream, _subscriptions_fields, &subs))
	return false;

    /* Parse all fields if they're included */
    if (subs.has_module_id)
	subs_info[*subs_idx]->module_ids[subs_info[*subs_idx]->mod_idx++] =
	    subs.module_id;
    if (subs.has_entity_id)
	subs_info[*subs_idx]->entity_ids[subs_info[*subs_idx]->entity_idx++] =
	    subs.entity_id;
    if (subs.has_module_class)
	subs_info[*subs_idx]->module_class[subs_info[*subs_idx]->class_idx++] =
	    subs.module_class;
    if (subs.has_i2c_address)
	subs_info[*subs_idx]->i2c_address[subs_info[*subs_idx]->i2c_idx++] =
	    subs.i2c_address;
    return true;
}

bool master_encode_MDR_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
{
    if (!pb_encode_tag_for_field(ostream, field)) {
	return false;
    }
    return true;
}

uint8_t get_CTS(uint8_t i2c_addr)
{
    uint8_t CTS_buf[2];
    
    void **vptr = malloc(sizeof(uint8_t*));
    vptr[0] = malloc(sizeof(uint8_t*));
    vptr[0] = &i2c_addr;   
    
    uint8_t status = stream.read(CTS_buf, 2, vptr, &stream);
    if (status == 0 && CTS_buf[1] == 0x1) {
	return 0;
    }
    else {
	return 1;
    }
}