#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <string.h>
#include <termios.h>
#include <signal.h>
#include <fcntl.h>

#define SCREEN_WIDTH 100
#define SCREEN_HEIGHT 50
#define CUBE_WIDTH 10

#define FACE_FRONT  '#'
#define FACE_BACK   '0'
#define FACE_BOTTOM '!'
#define FACE_LEFT   '%'
#define FACE_RIGHT  '@'
#define FACE_TOP    '-'

#define STEP 0.2

#define ACTION_STEP 0.1

#define PITCH_STEP 0.08
#define ROLL_STEP 0.00
#define YAW_STEP 0.01

#define K1 100
#define K2 50

#define _RESET_ANGLE(input) do {\
		Angle.currentTarget = 0;\
		input = 0; \
	} while (0);

#define RESET_A_ANGLE(input) do {\
		Angle.A = Angle.ATarget; \
		_RESET_ANGLE(input); \
	} while (0);
#define RESET_B_ANGLE(input) do {\
		Angle.B = Angle.BTarget; \
		_RESET_ANGLE(input); \
	} while (0);
#define RESET_C_ANGLE(input) do {\
		Angle.C = Angle.CTarget; \
		_RESET_ANGLE(input); \
	} while (0);

struct {
	double A, B, C;
	double ATarget, BTarget, CTarget;
	char currentTarget;
	double cosA, sinA;
	double cosB, sinB;
	double cosC, sinC;
} Angle;

void initAngles() {
	Angle.A = 0, Angle.B = 0, Angle.C = 0;
	Angle.ATarget = 0, Angle.BTarget = 0, Angle.CTarget = 0;
	Angle.currentTarget = 0;
}

double zBuffer[SCREEN_HEIGHT * SCREEN_WIDTH];
char output[SCREEN_HEIGHT * SCREEN_WIDTH];

static volatile char shouldBreak = 1;
static struct termios originalTerm = {};

double calcX(double x, double y, double z){
	return
		x * Angle.cosA * Angle.cosB +
		y * Angle.sinA * Angle.cosB
		-z * Angle.sinB;
}

double calcY(double x, double y, double z){
	return
		x * (Angle.cosA * Angle.sinB * Angle.sinC - Angle.sinA * Angle.cosC) +
		y * (Angle.sinA * Angle.sinB * Angle.sinC + Angle.cosA * Angle.cosC) +
		z * Angle.cosB * Angle.sinC;
}

double calcZ(double x, double y, double z){
	return
		x * (Angle.cosA * Angle.sinB * Angle.cosC + Angle.sinA * Angle.sinC) +
		y * (Angle.sinA * Angle.sinB * Angle.cosC - Angle.cosA * Angle.sinC) +
		z * Angle.cosB * Angle.cosC;
}

void calcAngle(){
	Angle.cosA = cos(Angle.A);
	Angle.sinA = sin(Angle.A);
	Angle.cosB = cos(Angle.B);
	Angle.sinB = sin(Angle.B);
	Angle.cosC = cos(Angle.C);
	Angle.sinC = sin(Angle.C);
}

char *chooseColor(char c){
	switch(c) {
		case '#':
			return "\033[31;1;4m#\033[0m";
		case '0':
			return "\033[32;1;4m0\033[0m";
		case '!':
			return "\033[33;1;4m!\033[0m";
		case '%':
			return "\033[34;1;4m%\033[0m";
		case '@':
			return "\033[35;1;4m@\033[0m";
		case '-':
			return "\033[36;1;4m-\033[0m";
		case ' ':
			return " ";
		default: // unused, but issues no warning
			return "";
	}
}

void printAscii() {
#ifdef __APPLE__
	fcntl(STDOUT_FILENO, F_SETFL, ~O_NONBLOCK);
#endif
	for (int k = 0 ; k < SCREEN_WIDTH * SCREEN_HEIGHT ; ++k)
		printf("%s", k % SCREEN_WIDTH ? chooseColor(output[k]) : "\r\n");
	printf("\r\n");
#ifdef __APPLE__
	fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
#endif
}

void rotateCube(double cubeX, double cubeY, double cubeZ, char ch){
	calcAngle();
	double x = calcX(cubeX, cubeY, cubeZ);
	double y = calcY(cubeX, cubeY, cubeZ);
	double invZ = 1 / (calcZ(cubeX, cubeY, cubeZ) + K2);

	int screenX = (int)(SCREEN_WIDTH / 2)  + floor((x) * K1) * invZ;
	int screenY = (int)(SCREEN_HEIGHT / 2) + floor(((y) * K1 / 2 ) * invZ);
	// TODO luminescence

	if (screenX > SCREEN_WIDTH || screenX < 0) return;

	int idx = screenY * SCREEN_WIDTH + screenX;
	if(idx >= 0 && idx < SCREEN_WIDTH * SCREEN_HEIGHT)
		if(zBuffer[idx] <  invZ) {
			zBuffer[idx] = invZ;
			output[idx] = ch;
		}
}

void intHandler(int unused) {
	shouldBreak = 0;
	tcsetattr(STDIN_FILENO, TCSANOW, &originalTerm);
}

void printBanner(char keyboardInput) {
	printf("Last input character: %c", keyboardInput);
}

void handleAngle(char *input) {
	if (!Angle.currentTarget) {
		Angle.currentTarget = *input;
		switch (*input) {
		case 'W':
			Angle.CTarget -= M_PI_2;
			// pitch angle +pi/2
			break;
		case 'A':
			// yaw angle -pi/2
			Angle.BTarget -= M_PI_2;
			break;
		case 'S':
			// pitch angle -pi/2
			Angle.CTarget += M_PI_2;
			break;
		case 'D':
			// yaw angle +pi/2
			Angle.BTarget += M_PI_2;
			break;
		default:
			Angle.currentTarget = 0;
		}

		// idle animation
		//Angle.A += ROLL_STEP;
		//Angle.B += YAW_STEP;
		//Angle.C += PITCH_STEP;
	}

	else
		switch(Angle.currentTarget) {
		case 'W':
			Angle.C -= ACTION_STEP;
			if (Angle.C <= Angle.CTarget) RESET_C_ANGLE(*input);
			break;
		case 'A':
			Angle.B -= ACTION_STEP;
			if (Angle.B <= Angle.BTarget) RESET_B_ANGLE(*input);
			break;
		case 'S':
			Angle.C += ACTION_STEP;
			if (Angle.C >= Angle.CTarget) RESET_C_ANGLE(*input);
			break;
		case 'D':
			Angle.B += ACTION_STEP;
			if (Angle.B >= Angle.BTarget) RESET_B_ANGLE(*input);
			break;
	}
}

char getInput(char *keyboardInput) {
	char c = getchar();
	if (c == '\033') { // if the first value is esc
		if ((c = getchar()) == '[')
			switch(c = getchar()) { // the real value
			case 'A':
				c = 'W';
				break;
			case 'B':
				c = 'S';
				break;
			case 'C':
				c = 'D';
				break;
			case 'D':
				c = 'A';
				break;
	    	}
	}

	if (c != EOF)
		*keyboardInput = c;
	while((c = getchar()) != '\n' && c != EOF) {} // clean stdin

	handleAngle(keyboardInput);
	return c;
}

int main(){
	char keyboardInput = 0;
   	signal(SIGINT, intHandler);
	tcgetattr(STDIN_FILENO, &originalTerm);
#ifndef __APPLE__
	fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK);
#endif

	struct termios t = {};
	cfmakeraw(&t);
	t.c_lflag |= ISIG;
	t.c_cc[VINTR] = 3;
	tcsetattr(STDIN_FILENO, TCSANOW, &t);

	initAngles();

	while(shouldBreak) {
		memset(output, ' ', SCREEN_WIDTH * SCREEN_HEIGHT);
		memset(zBuffer, 0, SCREEN_WIDTH * SCREEN_HEIGHT * sizeof(double));

		for(double cubeX = -CUBE_WIDTH ; cubeX < CUBE_WIDTH ; cubeX += STEP) {
			for(double cubeY = -CUBE_WIDTH ; cubeY < CUBE_WIDTH ; cubeY += STEP) {
				rotateCube(cubeX, cubeY, -CUBE_WIDTH, FACE_FRONT);//front
				rotateCube(cubeX, cubeY, CUBE_WIDTH, FACE_BACK);//back
				rotateCube(CUBE_WIDTH, -cubeX, cubeY, FACE_RIGHT);//right
				rotateCube(-CUBE_WIDTH, cubeX, cubeY, FACE_LEFT);//left
				rotateCube(cubeX, CUBE_WIDTH, cubeY, FACE_BOTTOM);//bottom
				rotateCube(cubeX, -CUBE_WIDTH, cubeY, FACE_TOP);//top
			}
		}

		printf("\x1b[2J");
		printBanner(keyboardInput);
		printAscii();
		getInput(&keyboardInput);
		usleep(100000);
	}
}