#include <Arduino.h>

#include "controls.h"
#include "error.h"
#include "IC74HC595.h"
#include "7Segment.h"
#include "matrix.h"

#define PIN_BUZZER                 21

#define PWM_FREQUENCY         1000
#define PWM_BITWIDTH          12
#define PWM_BUZZER_CHANNEL    0

#define ENTITY_ALIVE          1 << 0
#define ENTITY_FOOD           1 << 1
#define ENTITY_SNAKE          1 << 2

#define ENTITY_HIGH_MASK       248     // 11111000
#define ENTITY_LOW_MASK        7       // 00000111

#define ENTITY_SHIFT_LIFETIME  3

int8_t player_x;
int8_t player_y;
int8_t delta_x;
int8_t delta_y;

// Entities represent a lit pixel on the LED matrix. Each entity is an 8 bit mask that consists of:
//
// 8             0
// 0 0 0 0 0 0 0 0
// ^       ^ ^ ^ ^
// |       | | | `-- Alive or dead. When 1, display this entity.
// |       | | `---- Food (1)
// |       | `------ Snake (1)
// |       |
// `-------+-------- Lifetime. How many game cycles should this entity remain for? 0-248.
//                   The lifetime gets counted down each cycle!

#define GAMESTATE_SCREENCAL      1
#define GAMESTATE_PLAY           2
#define GAMESTATE_LOSE           3
#define GAMESTATE_WIN            4

uint8_t entities[64] = {0};
int found_food = 0;
int8_t tail_length = 2;
uint32_t gamespeed;
uint32_t lastupdate;
uint32_t difficulty = 0;
uint8_t score = 0;
uint8_t open_indexes[64] = {0};

Button btn_left = {PIN_BUTTON_LEFT, 0, LOW, BUTTON_STABLETIME, 0};
Button btn_up = {PIN_BUTTON_UP, 0, LOW, BUTTON_STABLETIME, 0};
Button btn_down = {PIN_BUTTON_DOWN, 0, LOW, BUTTON_STABLETIME, 0};
Button btn_right = {PIN_BUTTON_RIGHT, 0, LOW, BUTTON_STABLETIME, 0};

uint8_t gamestate = 0;

void initSerial()
{
	Serial.begin(115200);
	Serial.printf("\nSERIAL READY\n");
}

void initBuzzer() {
  ledcAttachChannel(PIN_BUZZER, 1000, 10, PWM_BUZZER_CHANNEL);
  ledcWriteTone(PIN_BUZZER, 2000);
	delay(1000);
	ledcWriteTone(PIN_BUZZER, 0);
}

void move_player()
{
	player_x += delta_x;
	player_y += delta_y;
	if ( player_x > 7) {
		player_x = 0;
	} else if ( player_x < 0 ) {
		player_x = 7;
	}
	if ( player_y > 7 ) {
		player_y = 0;
	} else if ( player_y < 0 ) {
		player_y = 7;
	}
}

void collide_player()
{
	uint8_t entity;
	entity = entities[(player_x * 8) + player_y];
	// Is there an entity where we're going?
	if ( entity > 0 ) {
		// We die if we hit our own body
		if ( (entity & ENTITY_SNAKE) == ENTITY_SNAKE ) {
			ledcWriteTone(PIN_BUZZER, 2000);
			delay(1000);
			ledcWriteTone(PIN_BUZZER, 0);
			init_game();
		} else if ( (entity & ENTITY_FOOD) == ENTITY_FOOD ) {
			found_food = 1;
			score += 1;
		}
	}
}

void draw_entities()
{
	uint8_t entity;
	resetMatrix();
	if ( gamestate == GAMESTATE_SCREENCAL ) {
		for ( int i = 0 ; i < 64 ; i++ ) {			
			if ( pokeMatrixPixel((i / 8), (i % 8)) != ERRNO_SUCCESS ) {
				Serial.printf("Failed to poke pixel : %d\n", i, errno);
				return;
			}
		}
	} else {
		// Poke matrix pixels for every living entity
		for ( int i = 0 ; i < 64; i++ ) {
			entity = entities[i];
			if ( ( entity & ENTITY_ALIVE ) == ENTITY_ALIVE ) {
				if ( pokeMatrixPixel((i / 8), (i % 8)) != ERRNO_SUCCESS ) {
					Serial.printf("Failed to poke pixel for entity %d : %d\n", i, errno);
					return;
				}
			}
		}
	}
	displayMatrix();
}

void update_matrix_entities()
{
	uint8_t entity;
	int8_t elife = 0;
	// Poke matrix pixels for every living entity
	for ( int i = 0 ; i < 64; i++ ) {
		entity = entities[i];
		if ( (entity & ENTITY_ALIVE) == ENTITY_ALIVE && i != ((player_x * 8) + player_y) ) {
			// Snake parts age (except the head)
			if ( (entity & ENTITY_SNAKE) == ENTITY_SNAKE ) {
				elife = (entity & ENTITY_HIGH_MASK) >> ENTITY_SHIFT_LIFETIME;
				if ( found_food == 0 ) {
					// Count down their lifetime
					if ( elife == 0 ) {
						entity = 0;
					} else {
						elife -= 1;
						entity = (elife << ENTITY_SHIFT_LIFETIME) | (entity & ENTITY_LOW_MASK);
					}
					entities[i] = entity;
				} /*else {
					elife += 1;
					entity = (elife << ENTITY_SHIFT_LIFETIME) | (entity & ENTITY_LOW_MASK);
					entities[i] = entity;
					}*/
			}
		}
	}
}

void place_random_entity(int8_t eflags, int8_t set_player)
{
	uint8_t index = 0;
	uint8_t entity = 0;
	uint8_t open_positions = 0;

	memset((void *)&open_indexes, 0x00, 64);
	
	// Count how many positions are open on the board
	for ( int i = 0 ; i < 64; i++ ) {
		entity = entities[i];
		if ( entity == 0) {
			open_indexes[open_positions] = i;
			open_positions += 1;
		} else {
		}
	}
	
	if ( open_positions == 0 ) {
		// This is a win condition!
		gamestate = GAMESTATE_WIN;
		delta_x = 0;
		delta_y = 0;
		return;
	}
	
	index = random(0, open_positions-1);
	if ( index > 63 ) {
		Serial.printf("Index out of bounds!\n");
		return;
	}
	index = open_indexes[index];
	
	// Place a random piece of food on the board
	entities[index] = (eflags);

	if ( set_player == 1 ) {
		player_x = (index / 8);
		player_y = (index % 8);
	}
}

void init_game()
{	
	tail_length = 2;
	memset((void *)&entities, 0x00, 64);
	gamespeed = 1000;
	lastupdate = millis();
	score = 0;
	delta_x = 0;
	delta_y = 0;
	
	place_random_entity(ENTITY_HIGH_MASK | ENTITY_ALIVE | ENTITY_FOOD, 0);
	
	// Place the player away from the food
	place_random_entity(((tail_length << ENTITY_SHIFT_LIFETIME) | ENTITY_ALIVE | ENTITY_SNAKE), 1);
}

void read_controls()
{
	difficulty = readDifficulty();
	if ( readButton(&btn_up) != ERRNO_SUCCESS ||
			 readButton(&btn_left) != ERRNO_SUCCESS ||
			 readButton(&btn_right) != ERRNO_SUCCESS ||
			 readButton(&btn_down) != ERRNO_SUCCESS ) {
		Serial.printf("Failed to read buttons\n");
		return;
	}
	if ( gamestate == GAMESTATE_SCREENCAL ) {
		if ( btn_up.state + btn_down.state + btn_left.state + btn_right.state != 0 ) {
			init_game();
			gamestate = GAMESTATE_PLAY;
		}
	} else {
		// We don't let the player reverse direction back on themselves.
		if ( (delta_x == 0) && (btn_up.state & BUTTON_STATE_DOWN) == BUTTON_STATE_DOWN ) {
			delta_x = 1;
			delta_y = 0;
		}
		if ( (delta_x == 0) && (btn_down.state & BUTTON_STATE_DOWN) == BUTTON_STATE_DOWN ) {
			delta_x = -1;
			delta_y = 0;
		}
		if ( (delta_y == 0) && (btn_right.state & BUTTON_STATE_DOWN) == BUTTON_STATE_DOWN ) {
			delta_y = -1;
			delta_x = 0;
		}
		if ( (delta_y == 0) && (btn_left.state & BUTTON_STATE_DOWN) == BUTTON_STATE_DOWN ) {
			delta_y = 1;
			delta_x = 0;
		}
	}
}

void display_score()
{
	uint8_t integerValues[] = {
		CHAR_7SEG_0,
		CHAR_7SEG_1,
		CHAR_7SEG_2,
		CHAR_7SEG_3,
		CHAR_7SEG_4,
		CHAR_7SEG_5,
		CHAR_7SEG_6,
		CHAR_7SEG_7,
		CHAR_7SEG_8,
		CHAR_7SEG_9,
	};
	uint8_t score_cur = 0;
	int rc = 0;

	if ( gamestate == GAMESTATE_SCREENCAL ) {
		select7SegDisplay(PIN_7SEGMENT_DIGIT4);
		rc = writeIC74HC595Char(&scoreboard, CHAR_7SEG_C, MSBFIRST);
		rc += writeIC74HC595Char(&scoreboard, CHAR_7SEG_OFF, MSBFIRST);
		select7SegDisplay(PIN_7SEGMENT_DIGIT3);
		rc += writeIC74HC595Char(&scoreboard, CHAR_7SEG_A, MSBFIRST);
		rc += writeIC74HC595Char(&scoreboard, CHAR_7SEG_OFF, MSBFIRST);
		select7SegDisplay(PIN_7SEGMENT_DIGIT2);
		rc += writeIC74HC595Char(&scoreboard, CHAR_7SEG_L, MSBFIRST);
		rc += writeIC74HC595Char(&scoreboard, CHAR_7SEG_OFF, MSBFIRST);
		if ( rc != ERRNO_SUCCESS ) {
			Serial.printf("Failed to write scoreboard: %d", errno);
			return;
		}
		return;
	}
	
	if ( difficulty > 0 ) {
		gamespeed = 1000 / difficulty;
	} else {
		gamespeed = 1000;
	}
	score_cur = (score % 10);
	select7SegDisplay(PIN_7SEGMENT_DIGIT1);
	rc = writeIC74HC595Char(&scoreboard, integerValues[score_cur], MSBFIRST);
	rc += writeIC74HC595Char(&scoreboard, CHAR_7SEG_OFF, MSBFIRST);
	if ( rc != ERRNO_SUCCESS ) {
		Serial.printf("Failed to write scoreboard: %d", errno);
		return;
	}	

	score_cur = (score / 10);
	select7SegDisplay(PIN_7SEGMENT_DIGIT2);
	rc = writeIC74HC595Char(&scoreboard, integerValues[score_cur], MSBFIRST);
	rc += writeIC74HC595Char(&scoreboard, CHAR_7SEG_OFF, MSBFIRST);
	if ( rc != ERRNO_SUCCESS ) {
		Serial.printf("Failed to write scoreboard: %d", errno);
		return;
	}	

	select7SegDisplay(PIN_7SEGMENT_DIGIT4);
	rc = writeIC74HC595Char(&scoreboard, integerValues[difficulty], MSBFIRST);
	rc += writeIC74HC595Char(&scoreboard, CHAR_7SEG_OFF, MSBFIRST);
	if ( rc != ERRNO_SUCCESS ) {
		Serial.printf("Failed to write scoreboard: %d", errno);
		return;
	}
}

void gamelogic()
{
	uint32_t updatetime = millis();
	found_food = 0;
	Serial.printf("Score is %d\n", score);
	if ( (updatetime - lastupdate) > gamespeed ) {
		lastupdate = updatetime;
		// Skip all of this if we have no delta (the player isn't playing yet)
		if ( (delta_x + delta_y) != 0 ) {
			move_player();
			collide_player();
			// Add a snake entity to the new location
			if ( found_food == 1 ) {
				tail_length += 1;
				place_random_entity(ENTITY_HIGH_MASK | ENTITY_ALIVE | ENTITY_FOOD, 0);				
			}
			entities[(player_x * 8) + player_y] = ((tail_length << ENTITY_SHIFT_LIFETIME) | ENTITY_ALIVE | ENTITY_SNAKE );
		}
		update_matrix_entities();
		found_food = 0;
	}
}

void setup() {	
	initSerial();
	score = 0;
	if ( init74HC595(&matrixcols) != ERRNO_SUCCESS ) {
		Serial.printf("failed to initialize matrix series 74HC595");
	}
	if ( initButton(&btn_up) != ERRNO_SUCCESS ||
			 initButton(&btn_left) != ERRNO_SUCCESS ||
			 initButton(&btn_right) != ERRNO_SUCCESS ||
			 initButton(&btn_down) != ERRNO_SUCCESS ) {
		Serial.printf("Failed to initialize buttons\n");
	}
	if ( init74HC595(&scoreboard) != ERRNO_SUCCESS ) {
		Serial.printf("Failed to initialize scoreboard 74HC595");
	}
	if ( init7Segment() != ERRNO_SUCCESS ) {
		Serial.printf("Failed to initialize 7 segment display");
	}
	initBuzzer();
	init_game();
	gamestate = GAMESTATE_SCREENCAL;
	
	Serial.printf("\nSETUP READY\n");
}

void loop() {
	int rc = 0;
	read_controls();
	display_score();
	gamelogic();
	draw_entities();
}