#include "effect_snake.h"
#include "functions.h"

SnakeEffect::SnakeEffect() {
	window = new Window(0, 0, LED_WIDTH, LED_HEIGHT-6);
	_pixels = window->width * window->height;
	_map = new uint8_t[_pixels];
	_init();
}

void SnakeEffect::_init() {
	_dying = 0;
	_round = 0;
	_last_apple_at = millis();
	_last_move_at = millis();
	_dir = SNAKE_DIR_NORTH;
	_length = 4;
	_pos = {(uint8_t)(window->width/2), (uint8_t)(window->height/2)};
	for (int i=0; i<_pixels; i++) _map[i]=0;
	_map[_xy2i(_pos)]=1;
	_map[_xy2i(_pos)+window->width*1]=2;
	_map[_xy2i(_pos)+window->width*2]=3;
	_map[_xy2i(_pos)+window->width*3]=4;
	_place_apple();
}

SnakeEffect::~SnakeEffect() {
	delete window;
	delete _map;
}

void SnakeEffect::_place_apple() {
	if (SNAKE_DEBUG) {
		_apple = {3, 3};
		return;
	}
	if (_length < _pixels) {
		uint8_t start = random8(_pixels);
		for (int i=0; i<_pixels; i++) {
			if (_map[start + i]==0) {
				_apple = _i2xy(start + i);
				return;
			}
		}
	}
}

void SnakeEffect::_decide() {
	uint8_t f_l = _free_spaces(_dir - 1);
	uint8_t f_s = _free_spaces(_dir);
	uint8_t f_r = _free_spaces(_dir + 1);
	uint8_t a_l = _to_apple(_dir - 1);
	uint8_t a_s = _to_apple(_dir);
	uint8_t a_r = _to_apple(_dir + 1);
	
	float* inputs = new float[6];
	inputs[0] = f_l;
	inputs[1] = f_s;
	inputs[2] = f_r;
	inputs[3] = a_l;
	inputs[4] = a_s;
	inputs[5] = a_r;
	if (SNAKE_DEBUG) LOGln("SnakeEffect * Position: %d, %d - Inputs: %3.1f %3.1f %3.1f %3.1f %3.1f %3.1f", _pos.x, _pos.y, inputs[0], inputs[1], inputs[2], inputs[3], inputs[4], inputs[5]);
	float* outputs = NULL;
	uint8_t i=0;
	for (uint8_t layer=1; layer<_net_layers; layer++) {
		outputs = new float[_net_layout[layer]];
		for (uint8_t j=0; j<_net_layout[layer]; j++) {
			outputs[j] = 0.0;
		}
		for (uint8_t idx_out=0; idx_out<_net_layout[layer]; idx_out++) {
			for (uint8_t idx_in=0; idx_in<_net_layout[layer-1]; idx_in++) {
				float weight;
				memcpy(&weight, &(_weights[i]), sizeof(weight));
				outputs[idx_out] += weight * inputs[idx_in];
				//outputs[idx_out] += (*(float*)&(_weights[i])) * inputs[idx_in];
				i++;
			}
		}
		delete inputs;
		inputs = outputs;
	}
	
	int8_t decision = 0;
	float last;
	for (uint8_t i=0; i<_net_layout[_net_layers - 1]; i++) {
		if (i==0 || outputs[i]>last) {
			last = outputs[i];
			decision = i;
		}
	}
	decision = decision - 1;
	delete outputs;
	
	if (SNAKE_DEBUG) LOGln("SnakeEffect * Decision: %d", decision);

	_dir += decision;
	if (_dir < 0) _dir += 4;
	if (_dir > 3) _dir -= 4;
}

/**
 * This is old (and hence disabled) code, showing
 * a simple, hand-crafted "AI" for plaing snake.
 **
int8_t SnakeEffect::_manual_decision() {	
	bool free_l  = _is_free(_dir - 1);
	bool free_s  = _is_free(_dir);
	bool free_r  = _is_free(_dir + 1);
	bool apple_l = _to_apple(_dir - 1);
	bool apple_s = _to_apple(_dir);
	bool apple_r = _to_apple(_dir + 1);
	
	if (!free_s) {
		if (apple_l && free_l) return -1;
		if (apple_r && free_r) return 1;
		if (free_l) return -1;
		return 1;
	}
	
	if (apple_s) return 0;
	if (apple_l && free_l) return -1;
	if (apple_r && free_r) return 1;
	return 0;
}*/

bool SnakeEffect::_is_free(uint8_t dir) {
	return _free_spaces(dir)!=0;
}

uint8_t SnakeEffect::_free_spaces(uint8_t dir) {
	int8_t x=0;
	int8_t y=0;
	uint8_t d = dir % 4;
	switch(d) {
		case SNAKE_DIR_NORTH: y=-1; break;
		case SNAKE_DIR_EAST:  x=1; break;
		case SNAKE_DIR_SOUTH: y=1; break;
		case SNAKE_DIR_WEST:  x=-1; break;
	}
	Coords p(_pos);
	uint8_t i=0;
	while (true) {
		p.x += x;
		p.y += y;
		if (p.x<0 || p.x>=window->width || p.y<0 || p.y>=window->height || _map[_xy2i(p)]!=0) {
			return i;
		}
		i++;
	}
}

uint8_t SnakeEffect::_to_apple(uint8_t dir) {
	uint8_t d = dir % 4;
	int8_t d_x = _apple.x - _pos.x;
	int8_t d_y = _apple.y - _pos.y;
	
	switch(d) {
		case SNAKE_DIR_NORTH: return d_y < 0 ? -d_y : 0;
		case SNAKE_DIR_EAST:  return d_x > 0 ?  d_x : 0;
		case SNAKE_DIR_SOUTH: return d_y > 0 ?  d_y : 0;
		case SNAKE_DIR_WEST:  return d_x < 0 ? -d_x : 0;
	}
	return 0;
}

Coords SnakeEffect::_new_pos(uint8_t dir) {
	uint8_t d = dir % 4;
	Coords p(_pos);
	switch(d) {
		case SNAKE_DIR_NORTH: p.y--; break;
		case SNAKE_DIR_EAST:  p.x++; break;
		case SNAKE_DIR_SOUTH: p.y++; break;
		case SNAKE_DIR_WEST:  p.x--; break;
	}
	return p;
}

uint16_t SnakeEffect::_xy2i(Coords c) {
	return _xy2i(c.x, c.y);
}

uint16_t SnakeEffect::_xy2i(uint8_t x, uint8_t y) {
	return y*window->width + x;
}

Coords SnakeEffect::_i2xy(uint16_t i) {
	return {(uint16_t)(i%window->width), (uint16_t)(i/window->width)};
}

void SnakeEffect::_move() {
	
	
	if (_dying > 0) {
		_dying--;
		
		if (_dying==0) {
			_init();
		}
		
		return;
	}
	
	unsigned long now = millis();
	if (_last_move_at < now && now - _last_move_at < 0) {
		return;
	}
	_round++;
	_last_move_at = now;
	_decide();
	
	if (_dying==0 && !_is_free(_dir)) {
		_dying = 150;
		return;
	}
	
	_pos = _new_pos(_dir);
	if (SNAKE_DEBUG) LOGln("SnakeEffect * new_pos: %d, %d", _pos.x, _pos.y);
	if (SNAKE_DEBUG) LOGln("SnakeEffect * apple:   %d, %d", _apple.x, _apple.y);
	if (_pos.x==_apple.x && _pos.y==_apple.y) {
		_last_apple_at = millis();
		_length++;
	}
	for (int i=0; i<_pixels; i++) {
		if (_map[i]>0 && _map[i]<_length-1) _map[i]++;
		else _map[i]=0;
	}
	_map[_xy2i(_pos)] = 1;
	if (_pos.x==_apple.x && _pos.y==_apple.y) {
		_place_apple();
	}
}

void SnakeEffect::_draw() {
	if (_dying) {
		window->fadeToBlackBy(4);
		return;
	}
	window->clear();
	CRGB red(0xBB0000);
	for (int i=0; i<_pixels; i++) {
		if (_map[i]>0) window->setPixelByIndex(i, &red);
	}
	CRGB white(0xFFFFFF);
	window->setPixel(_pos.x, _pos.y, &white);
	CRGB green(0xFFFF00);
	window->setPixel(_apple.x, _apple.y, &green);
}

void SnakeEffect::loop(uint16_t ms) {
	//window->fadeToBlackBy(2);
	//CRGB color(CHSV(hue, 200, 255));
	//window->setPixel(this->coords.x, this->coords.y, &color);
	//hue++;
	if (_dying==0 && (millis() < _last_apple_at || millis() - _last_apple_at > 30000)) {
		_dying = 150;
	}
	_move();
	_draw();
}

boolean SnakeEffect::can_be_shown_with_clock() { return true; }