#include "effects/snake.h"
#include "functions.h"

SnakeEffect::SnakeEffect() {
	window = &Window::window_with_clock;
	_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 _map;
}

void SnakeEffect::_place_apple() {
	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) DBG("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) DBG("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;
}*/

/* This uses a predefined meandering path through the complete field.
   The snake always tries to reach the field matching following criteria:
     (1) Being free.
     (2) Having a number smaller than the field the apple is on. (Watch out for "overflows".)
*/
int8_t SnakeEffect::_manual_decision() {
	uint8_t head_index = _coords_to_field_id(_pos);
	uint8_t apple_index = _coords_to_field_id(_apple);
	uint8_t tail_index = _coords_to_field_id(_tail);
	if (SNAKE_DEBUG) DBG("SnakeEffect * Decision. head: %d, apple: %d, tail: %d", head_index, apple_index, tail_index);
	
	uint16_t best_distance = 0xFFFF;
	int8_t decision = 0;
	
	for (int i=-1; i<=1; i++) { // Test all thre possible directions (left, ahead, right)
		Coords new_pos = _new_pos(_dir + i);
		uint8_t new_index = _coords_to_field_id(new_pos);
		
		int16_t distance;
		if (apple_index >= new_index) {
			distance = apple_index - new_index;
		} else {
			distance = (window->width * window->height) - apple_index + new_index;
		}
		if (SNAKE_DEBUG) DBG("SnakeEffect *   Decision: %d would have distance %d", i, distance);
		if (distance < best_distance && _is_free(_dir + i)) {
			best_distance = distance;
			decision = i;
		}
	}
	
	if (SNAKE_DEBUG) DBG("SnakeEffect *   Decision taken: %d with distance %d", decision, best_distance);
		
	/*	apple_index > new_index && head_index > apple_index
		apple_index > new_index && head_index < apple_index && new_index > head_index

	uint16_t head_index = (_head_rounds<<8) | _coords_to_field_id(_pos);
	uint16_t apple_index = (_head_rounds<<8) | _coords_to_field_id(_apple);
	uint16_t tail_index = (_tail_rounds<<8) | _coords_to_field_id(_tail);
	
	if (apple_index < head_index) apple_index += 0x100;
	
	uint8_t best_dist = 0xFF;
	int8_t decision = 0;
	for (int i=-1; i<=1; i++) {
		Coords new_pos = _new_pos(_dir + i);
		uint16_t theoretical_index = (_head_rounds<<8) | _coords_to_field_id(new_pos);
		if (theoretical_index < head_index) theoretical_index += 0x100;
		int16_t dist = apple_index - theoretical_index;
		if (dist < 0) dist += window->height * window->width;
		if (dist < best_dist && _is_free(_dir + i) && theoretical_index<tail_index && theoretical_index<tail_index) {
			decision = i;
			best_dist = dist;
		}
	}*/
	_dir = (_dir + decision) % 4;
	return decision;
}

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::_coords_to_field_id(Coords c) {
	if (c.y==0) return window->width - c.x;
	if (c.x % 2 == 0) {
		// even columns
		return window->width + c.x*(window->height - 1) + c.y - 1;
	} else {
		// odd columns
		return window->width + (c.x+1)*(window->height-1) - c.y;
	}
}

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 < 50) {
		return;
	}
	_round++;
	_last_move_at = now;
	_manual_decision();
	
	if (_dying==0 && !_is_free(_dir)) {
		_dying = 150;
		return;
	}
	
	_pos = _new_pos(_dir);
	
	uint8_t index_head = _coords_to_field_id(_pos);
	uint8_t index_tail = _coords_to_field_id(_tail);
	
	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]++;
			if (_map[i]==_length-1) {
				_tail = _i2xy(i);
			}
		}
		else _map[i]=0;
	}
	_map[_xy2i(_pos)] = 1;
	if (_pos.x==_apple.x && _pos.y==_apple.y) {
		_place_apple();
	}
	
	if (index_head > _coords_to_field_id(_pos)) _head_rounds++;
	if (index_tail > _coords_to_field_id(_tail)) _tail_rounds++;
	if (_head_rounds > 0 && _head_rounds > 0) {
		uint8_t min = (_head_rounds < _tail_rounds) ? _tail_rounds : _head_rounds;
		_head_rounds -= min;
		_tail_rounds -= min;
	}
}

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; }