#include "sds.h"

#include "interface.h"
#include "communication_interface.h"
#include "mem.h"

extern struct MTask *current_task;

struct BCShareSpec *sds_get(uint8_t id)
{
	if (id >= current_task->shares.size) {
		send_message((struct MTMessageFro)
			{ .cons=MTFException_c
			, .data={.MTFException=
				{ .cons=MTESdsUnknown_c
				, .data={.MTESdsUnknown=
					{.f0=current_task->taskid, .f1=id}
				}}
			}});
		return NULL;
	}
	return current_task->shares.elements+id;
}

void sds_update(uint8_t taskid, uint8_t sdsid, String255 value)
{
	msg_debug(SC("Receiving for task %u sds %u\n"), taskid, sdsid);

	current_task = mem_task_head();
	while (current_task != NULL) {
		if (current_task->taskid == taskid) {
			msg_debug(SC("Found task for the sds update\n"));
			struct BCShareSpec *b = sds_get(sdsid);
			if (b == NULL) {
				msg_log(SC("no addr for sds???\n"));
				break;
			}

			for (uint8_t i = 0; i<value.size; i++)
				b->bcs_value.elements[i] = value.elements[i];
			return;
		}
		current_task = mem_task_next(current_task);
	}

	msg_log(SC("Help, received an unknown sds\n"));
	send_message((struct MTMessageFro)
		{ .cons=MTFException_c
		, .data={.MTFException=
			{ .cons=MTESdsUnknown_c
			, .data={.MTESdsUnknown=
				{.f0=taskid
				,.f1=sdsid
				}
			}}
		}});
}

void sds_set_mtask(struct BCShareSpec *sds, uint16_t *stack)
{
	uint8_t *b = sds->bcs_value.elements;
	bool same = true;
	//Write to the memory
	for (uint16_t i = 0; i<sds->bcs_value.size/2; i++) {
		same = same && (b[i*2]*256u + b[i*2+1] == stack[i]);
		b[i*2] = stack[i]/256;
		b[i*2+1] = stack[i]%256;
	}

	//Send if it is different and an upstream sds
	msg_debug(SC("addr: %lu, itasks: %u, same: %u\n"),
		sds, sds->bcs_itasks, same);
	if (sds->bcs_itasks && !same) {
		msg_log(SC("Send sds update\n"));
		send_message((struct MTMessageFro)
			{ .cons=MTFSdsUpdate_c
			, .data={ .MTFSdsUpdate=
				{ .f0=current_task->taskid
				, .f1=sds->bcs_ident
				, .f2=
					{ .size=sds->bcs_value.size/2
					, .elements=stack
					}
				}
			}});
	}
}

uint16_t *sds_get_mtask(struct BCShareSpec *sds, uint16_t *stack)
{
	uint8_t *b = sds->bcs_value.elements;
	for (uint8_t i = 0; i<sds->bcs_value.size/2; i++) {
		*stack++ = b[i*2]*256 + b[i*2+1]%256;
	}
	return stack;
}
