/* Copyright (C) 2007  Mikko Sysikaski <mikko.sysikaski@gmail.com>
 * This program is free software distributed under GNU General Public License
 * version 3 or (at your option) any later version.
 * No warranty of any kind is provided. See the accompanying file COPYING for details.
 */

#include "physics.h"
#include "board.h"
#include "lcdcounter.h"
#include <vector>
#include <math.h>
#include <algorithm>
#include "SDL.h"

static const float JTURN = 1.0f/200.0f;
static const float ACCEL_SPEED = 0.05f*JTURN;
static const float MOVE_SPEED = 150.0f;
static const float MAX_DIFF = BALL_SIZE + BLOCK_SIZE;
static const float FRICTION = 3.0f;
static const float GRAVITY = 0.25f;

Physics::Physics()
{
}

Physics::Physics(Board* board)
{
	setBoard(board);
}

void Physics::setBoard(Board* board)
{
	m_board = board;
	m_rotX = m_rotY = 0.0f;
	resetBoard();
}
void Physics::resetBoard()
{
	m_place = m_board->startPlace();
	m_place.x += 0.5f;
	m_place.y += 0.5f;
	m_speed.x = m_speed.y = 0.0f;
	m_height = 0.0f;

	m_winning = 0;

	m_counter.reset(m_board->getMinTime());
	m_counter.start();
}

inline float gravity(float height, float sec)
{
	return 10.0f*sec*sqrt(2.0f*GRAVITY*-height);
}

Physics::UpdateState Physics::update(uint msec)
{
	float sec = msec * 0.001f;
	
	if (m_height < -0.5f*BALL_SIZE) {
		if (m_winning) {
			m_counter.stop();
		//	printf("Board completed in %.2f s\n", m_counter.getTime()*0.001f);
		//	m_winning = 0;
		}
		
		m_place.x += sec*MOVE_SPEED*m_speed.x;
		m_place.y += sec*MOVE_SPEED*m_speed.y;
	//	m_height -= sec*2.0f;
		m_height -= gravity(m_height, sec);
		
		if (m_height < -3.0f) {
			return m_winning ? WIN : DIE;
		}
		return NORMAL;
	}
	
	m_speed.x += sec*m_rotX*ACCEL_SPEED;
	m_speed.y += sec*m_rotY*ACCEL_SPEED;

	m_speed.x *= 1.0f - FRICTION*sec;
	m_speed.y *= 1.0f - FRICTION*sec;
	
	m_place.x += sec*MOVE_SPEED*m_speed.x;
	m_place.y += sec*MOVE_SPEED*m_speed.y;
	
	checkLimits();
	checkHoles(sec);

	return NORMAL;
}
void Physics::updateCounter()
{
	m_counter.update();
}

inline void reflect(float& x, float limit)
{
	x = 2*limit-x;
}

inline
void Physics::checkLimits()
{
	using std::min;
	using std::max;
	Board& b = *m_board;
	int ix = int(m_place.x), iy = int(m_place.y);
	float fxi = ix, fyi = iy;
	float fx = m_place.x-fxi, fy = m_place.y-fyi;
	if ((iy==b.getY()-1	|| b[iy][ix]&1)	&& fy>1.0f-MAX_DIFF) {
		reflect(m_place.y, fyi+1.0f-MAX_DIFF);
		m_speed.y = -m_speed.y;
	}
	else if ((iy==0 || b[iy-1][ix]&1) && fy<MAX_DIFF) {
		reflect(m_place.y, fyi+MAX_DIFF);
		m_speed.y = -m_speed.y;
	}
	if ((ix==b.getX()-1 || b[iy][ix]&2) && fx>1.0f-MAX_DIFF) {
		reflect(m_place.x, fxi+1.0f-MAX_DIFF);
		m_speed.x = -m_speed.x;
	}
	else if ((ix==0 || b[iy][ix-1]&2) && fx<MAX_DIFF) {
		reflect(m_place.x, fxi+MAX_DIFF);
		m_speed.x = -m_speed.x;
	}
	
	const float BLEN=M_SQRT1_2*BLOCK_SIZE;
	const float BLEN_SQ=M_SQRT2*BLOCK_SIZE;
	const float BALL_OOS=M_SQRT1_2*BALL_SIZE;
	const float XSIZE = BLEN+BALL_OOS;
	
	static std::vector<Coord2> corners(20);
	corners.clear();
	if (ix>0) {
		if (iy > 0) {
			if (b[iy-1][ix-1] & 1)
				corners.push_back(Coord2(fxi, min(m_place.y, fyi+BLOCK_SIZE)));
			if (b[iy-1][ix-1] & 2)
				corners.push_back(Coord2(min(m_place.x, fxi+BLOCK_SIZE), fyi));
			if (b[iy-1][ix-1] & 4) {
				if (fx > XSIZE) {
					corners.push_back(Coord2(fxi+BLEN, fyi-BLEN));
				} else if (fy > XSIZE) {
					corners.push_back(Coord2(fxi-BLEN, fyi+BLEN));
				} else {
					float a = 0.5f*(fx+fy);
					corners.push_back(Coord2(m_place.x-a, m_place.y-a));
				}
			}
		}
		if (iy < b.getY()-1) {
			if (b[iy][ix-1] & 1)
				corners.push_back(Coord2(fxi, max(m_place.y, fyi+1.0f-BLOCK_SIZE)));
			if (b[iy+1][ix-1] & 2)
				corners.push_back(Coord2(min(m_place.x, fxi+BLOCK_SIZE), fyi+1.0));
			if (b[iy+1][ix-1] & 8) {
				if (fx > XSIZE) {
					corners.push_back(Coord2(fxi+BLEN, fyi+1.0f+BLEN));
				} else if (fy < 1.0f-XSIZE) {
					corners.push_back(Coord2(fxi-BLEN, fyi+1.0f-BLEN));
				} else {
					float a = 0.5f*(1.0f+fx-fy);
					corners.push_back(Coord2(m_place.x-a, m_place.y+a));
				}
			}
		}
		if (b[iy][ix-1] & 4) {
			if (fx > XSIZE) {
				corners.push_back(Coord2(fxi+BLEN, fyi+1.0f-BLEN));
			} else {
				float a = 0.5f*(1.0f+fx-fy-BLEN_SQ);
				corners.push_back(Coord2(m_place.x-a, m_place.y+a));
			}
		}
		if (b[iy][ix-1] & 8) {
			if (fx > XSIZE) {
				corners.push_back(Coord2(fxi+BLEN, fyi+BLEN));
			} else {
				float a = 0.5f*(BLEN_SQ-fx-fy);
				corners.push_back(Coord2(m_place.x+a, m_place.y+a));
			}
		}
	}
	if (ix < b.getX()-1) {
		if (iy > 0) {
			if (b[iy-1][ix+1] & 1)
				corners.push_back(Coord2(fxi+1.0f, min(m_place.y, fyi+BLOCK_SIZE)));
			if (b[iy-1][ix] & 2)
				corners.push_back(Coord2(max(m_place.x, fxi+1.0f-BLOCK_SIZE), fyi));
			if (b[iy-1][ix+1] & 8) {
				if (fx < 1.0f-XSIZE) {
					corners.push_back(Coord2(fxi+1.0f-BLEN, fyi-BLEN));
				} else if (fy > XSIZE) {
					corners.push_back(Coord2(fxi+1.0f+BLEN, fyi+BLEN));
				} else {
					float a = 0.5f*(1.0f+fy-fx);
					corners.push_back(Coord2(m_place.x+a, m_place.y-a));
				}
			}
		}
		if (iy < b.getY()-1) {
			if (b[iy][ix+1] & 1)
				corners.push_back(Coord2(fxi+1.0f, max(m_place.y, fyi+1.0f-BLOCK_SIZE)));
			if (b[iy+1][ix] & 2)
				corners.push_back(Coord2(max(m_place.x, fxi+1.0f-BLOCK_SIZE), fyi+1.0));
			if (b[iy+1][ix+1] & 4) {
				if (fx < 1.0f-XSIZE) {
					corners.push_back(Coord2(fxi+1.0f-BLEN, fyi+1.0f+BLEN));
				} else if (fy < 1.0f-XSIZE) {
					corners.push_back(Coord2(fxi+1.0f+BLEN, fyi+1.0f-BLEN));
				} else {
					float a = 0.5f*(2.0f-fx-fy);
					corners.push_back(Coord2(m_place.x+a, m_place.y+a));
				}
			}
		}
		if (b[iy][ix+1] & 4) {
			if (fx < 1.0f-XSIZE) {
				corners.push_back(Coord2(fxi+1.0f-BLEN, fyi+BLEN));
			} else {
				float a = 0.5f*(1.0f+fy-fx-BLEN_SQ);
				corners.push_back(Coord2(m_place.x+a, m_place.y-a));
			}
		}
		if (b[iy][ix+1] & 8) {
			if (fx < 1.0f-XSIZE) {
				corners.push_back(Coord2(fxi+1.0f-BLEN, fyi+1.0f-BLEN));
			} else {
				float a = 0.5f*(2.0f-fx-fy-BLEN_SQ);
				corners.push_back(Coord2(m_place.x+a, m_place.y+a));
			}
		}
	}
	if (iy > 0) {
		if (b[iy-1][ix] & 4) {
			if (fy > XSIZE) {
				corners.push_back(Coord2(fxi+1.0f-BLEN, fyi+BLEN));
			} else {
				float a = 0.5f*(1.0f+fy-fx-BLEN_SQ);
				corners.push_back(Coord2(m_place.x+a, m_place.y-a));
			}
		}
		if (b[iy-1][ix] & 8) {
			if (fy > XSIZE) {
				corners.push_back(Coord2(fxi+BLEN, fyi+BLEN));
			} else {
				float a = 0.5f*(BLEN_SQ-fx-fy);
				corners.push_back(Coord2(m_place.x+a, m_place.y+a));
			}
		}
	}
	if (iy < b.getY()-1) {
		if (b[iy+1][ix] & 4) {
			if (fy < 1.0f-XSIZE) {
				corners.push_back(Coord2(fxi+BLEN, fyi+1-BLEN));
			} else {
				float a = 0.5f*(1.0f+fx-fy-BLEN_SQ);
				corners.push_back(Coord2(m_place.x-a, m_place.y+a));
			}
		}
		if (b[iy+1][ix] & 8) {
			if (fy < 1.0f-XSIZE) {
				corners.push_back(Coord2(fxi+1.0f-BLEN, fyi+1.0f-BLEN));
			} else {
				float a = 0.5f*(2.0f-fx-fy-BLEN_SQ);
				corners.push_back(Coord2(m_place.x+a, m_place.y+a));
			}
		}
	}
	if (b[iy][ix] & 4) {
		float a = 0.5f*(fy-fx-BLEN_SQ);
	//	printf("a: %f, adding: %f %f\n", a, m_place.x+a, 1.0f-m_place.y+a);
		if (a < 0)
			a = 0.5f*(fy-fx+BLEN_SQ);
		corners.push_back(Coord2(m_place.x+a, m_place.y-a));
	}
	if (b[iy][ix] & 8) {
		float a = 0.5f*(1.0f-fx-fy-BLEN_SQ);
		if (a < 0)
			a = 0.5f*(1.0f-fx-fy+BLEN_SQ);
		corners.push_back(Coord2(m_place.x+a, m_place.y+a));
	}
//	printf("lol start testing\n");
	for(std::vector<Coord2>::iterator i=corners.begin(); i!=corners.end(); ++i) {
	//	printf("lol testing %f %f\n", i->y, i->x);
		float dx = m_place.x - i->x;
		float dy = m_place.y - i->y;
		float diff2 = dx*dx + dy*dy;
		if (diff2 < BALL_SIZE*BALL_SIZE) {
		//	printf("Lol collision %f %f\n", i->y, i->x);
			float ax = dx, ay = dy;
			
			float ood = 1.0f/sqrt(diff2);
			dx *= ood;
			dy *= ood;
			
			m_place.x += dx*BALL_SIZE-ax;
			m_place.y += dy*BALL_SIZE-ay;
			
			float pl = -m_speed.x*dx - m_speed.y*dy;
			float px = pl * dx;
			float py = pl * dy;

			m_speed.x += 2*px;
			m_speed.y += 2*py;
		}
	}
}
static const float BALLS2 = BALL_SIZE*BALL_SIZE;
inline void Physics::checkHoles(float sec)
{
//	for(Board::HoleIter i=m_board->hbegin(); i != m_board->hend(); ++i) {
	int ix = int(m_place.x), iy = int(m_place.y);
	BlockT block = m_board->getBlock(int(m_place.y), int(m_place.x));
	if (block == HOLE || block == END) {
		float dx = ix+0.5f - m_place.x;
		float dy = iy+0.5f - m_place.y;
		
		float len2 = dx*dx + dy*dy;
		if (len2 < HOLE_SIZE*HOLE_SIZE) {
			float len = sqrt(len2);
			float tmpX = HOLE_SIZE-len;
			if (tmpX > BALL_SIZE) {
				m_height -= gravity(m_height, sec);
				return;
			}
			float ballY = sqrt(BALLS2 - tmpX*tmpX);
			m_height = ballY - BALL_SIZE;
			
			float accel = GRAVITY*ballY*tmpX / BALLS2;
			
			m_speed.x += dx/len*sec*accel;
			m_speed.y += dy/len*sec*accel;
		//	printf("new height: %f\n", m_height);
		//	break;

			if (block == END)
				m_winning = 1;
			else
				m_winning = 0;
		}
	} else if (block == SHOLE) {
		// true infinity might break with -ffast-math
		const float inf = std::numeric_limits<float>::max();
		float fx = m_place.x-ix, fy = m_place.y-iy;
		float lenX;
		float lenZ;
		if (fx < 0.5f) {
			if (ix > 0 && m_board->getBlock(iy, ix-1)!=SHOLE)
				lenX = fx;
			else lenX = inf;
		} else {
			if (ix < m_board->getX()-1 && m_board->getBlock(iy, ix+1)!=SHOLE)
				lenX = 1.0f-fx;
			else
				lenX = inf;
		}
		if (fy < 0.5f) {
			if (iy > 0 && m_board->getBlock(iy-1, ix)!=SHOLE)
				lenZ = fy;
			else lenZ = inf;
		} else {
			if (iy < m_board->getY()-1 && m_board->getBlock(iy+1, ix)!=SHOLE)
				lenZ = 1.0f-fy;
			else
				lenZ = inf;
		}

		float len = std::min(lenX, lenZ);
		if (len > BALL_SIZE) {
			m_height -= gravity(m_height, sec);
			return;
		}
		float ballH = sqrt(BALLS2 - len*len);
		float accel = sec * GRAVITY*ballH*len / BALLS2;

		m_height = ballH - BALL_SIZE;
		
		if (lenX < lenZ) {
			if (fx < 0.5f)
				m_speed.x += accel;
			else
				m_speed.x -= accel;
		} else {
			if (fy < 0.5f)
				m_speed.y += accel;
			else
				m_speed.y -= accel;
		}
	}
}
