#include "system/task_rand.h"
#include "system/hardware_task.h"
#include "system/data_channel.h"
#include "system/float_word.h"

#include "stdio.h"

#define POLYNOM ((1 << 31)|(1 << 21)|(1 << 1)|(1 << 0))

int32_t shift_lsfr(int32_t *lsfr) {
	int feedback;

	feedback = *lsfr & 1;

	*lsfr >>= 1;

	if(feedback == 1)
		*lsfr ^= POLYNOM;
	return *lsfr;

}

int32_t pot(int32_t base, int exp) {
	int32_t res = 1;
	while(exp-- >= 0)
		res *= base;

	return res;
}

uint8_t clamp_value(uint8_t value) {
		
	if(value & 128) {
		value &= 0b10000001;
		//value &= ((1 << 7) | (1 << 0));
	} else {
		//value |= ~((1 << 1) | (1 << 0));
		value |= 0b01111100;
	}
	return value;

}

int task_rand_run( void * task ) {
	
	rand_config* config = ( rand_config* ) task;
	float_word seed = { .value = config->seed };

	uint32_t lfsr = seed.word;

	uint32_t data_channel_base = config->base.sink;
	data_channel_clear(data_channel_base);

	for(uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
		float_word res, value;

		//res.value = lfsr;
		//


		uint32_t E = (value.word >> 23) & 0xff;
		E = clamp_value(E);
		
		res.word = value.word;
		res.word &= ~(0xFF << 23);
		res.word |= (E << 23);

		value.word = shift_lsfr(&lfsr) ;

		data_channel_write( data_channel_base, res.word);
	}

    return 0;
}