/************************************************************************
* Copyright (c) 2005-2007 tok@openlinux.org.uk                          *
*                                                                       *
* This software is provided as-is, without any express or implied       *
* warranty. In no event will the authors be held liable for any         *
* damages arising from the use of this software.                        *
*                                                                       *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute   *
* it freely, subject to the following restrictions:                     *
*                                                                       *
* 1. The origin of this software must not be misrepresented; you must   *
* not claim that you wrote the original software. If you use this       *
* software in a product, an acknowledgment in the product documentation *
* would be appreciated but is not required.                             *
*                                                                       *
* 2. Altered source versions must be plainly marked as such, and must   *
* not be misrepresented as being the original software.                 *
*                                                                       *
* 3. This notice may not be removed or altered from any source          *
* distribution.                                                         *
************************************************************************/

#define _USE_MATH_DEFINES
#include <cmath>

#include "entity_controller.h"
#include "log.h"
#include "m_exceptions.h"

#include "opengta.h"
#include "dataholder.h"


float slope_height_offset(unsigned char slope_type, float dx, float dz);

namespace OpenGTA {

	float heightOverTerrain(Eigen::Vector3f v) {
		float x, y, z;
		x = floor(v.x());
		y = floor(v.y());
		z = floor(v.z());
		PHYSFS_uint8 x_b, z_b;
		x_b = (PHYSFS_uint8)x;
		z_b = (PHYSFS_uint8)z;
		if (y < 0.0f) {
			//ERROR << "Below level! at coords: " << v.x << ", " << v.y << ", " << v.z << std::endl;
			return 1.0f;
		}
		if (x < 0 || x > 255 || z < 0 || z > 255) {
			//ERROR << "x = " << x << "(" << v.x << ") z = " << z << " (" << v.z << ")" << std::endl;
			throw E_OUTOFRANGE("invalid x/z pos");
		}
		if (y > 20) {
			INFO << y << " seems a bit high; going to 20" << std::endl;
			INFO << x << " " << z << std::endl;
			y = 20;
		}
		OpenGTA::Map & map = OpenGTA::MapHolder::Instance().get();
		while (y >= map.getNumBlocksAtNew(x_b, z_b) && y > 0.0f)
			y -= 1.0f;
		while (y < map.getNumBlocksAtNew(x_b, z_b) && y > 0.0f) {
			OpenGTA::Map::BlockInfo * block = map.getBlockAtNew(x_b, z_b, (PHYSFS_uint8)y);
			assert(block);
			if (block->blockType() > 0) {
				float bz = slope_height_offset(block->slopeType(), v.x() - x, v.z() - z);
				if (block->slopeType() == 0 && (block->blockType() != 5 &&
					block->blockType() != 6))
					bz -= 1.0f;
				//INFO << "hit " << int(block->blockType()) << " at " << int(y) << std::endl;
				return v.y() - (y + bz);
			}
			y -= 1.0f;
		}
		y = floor(v.y()) + 1.0f;
		while (y < map.getNumBlocksAtNew(x_b, z_b) && y > 0.0f) {
			OpenGTA::Map::BlockInfo * block = map.getBlockAtNew(x_b, z_b, (PHYSFS_uint8)y);
			assert(block);
			if (block->blockType() > 0) {
				float bz = slope_height_offset(block->slopeType(), v.x() - x, v.z() - z);
				if (block->slopeType() == 0 && (block->blockType() != 5
					&& block->blockType() != 6))
					bz -= 1.0f;
				//INFO << "hit " << int(block->blockType()) << " at " << int(y) << std::endl;
				return v.y() - (y + bz);
			}
			y += 1.0f;
		}
		INFO << "should this be reached?" << std::endl;
		return 1.0f;
	}


	PlayerPedController::PlayerPedController()
	{
		pos = Eigen::Vector3f{ 12.0f, 6.1f, 12.0f };
	}

	void PlayerPedController::setMoveForward()
	{
		pressedButtons = pressedButtons | 0b00000001;
	}


	void PlayerPedController::setMoveBack()
	{
		pressedButtons = pressedButtons | 0b00000010;
	}

	void PlayerPedController::setTurnLeft()
	{
		pressedButtons = pressedButtons | 0b00000100;
	}

	void PlayerPedController::setTurnRight()
	{
		pressedButtons = pressedButtons | 0b00001000;
	}



	void PlayerPedController::releaseMoveForward()
	{
		pressedButtons = pressedButtons & 0b11111110;
	}




	void PlayerPedController::releaseMoveBack()
	{
		pressedButtons = pressedButtons & 0b11111101;
	}

	void PlayerPedController::releaseTurnLeft()
	{
		pressedButtons = pressedButtons & 0b1111011;
	}


	void PlayerPedController::releaseTurnRight()
	{
		pressedButtons = pressedButtons & 0b11110111;
	}


	bool PlayerPedController::isMoveForward()
	{
		return (pressedButtons & 0b00000001) > 0;
	}
	bool PlayerPedController::isMoveBack()
	{
		return (pressedButtons & 0b00000010) > 0;
	}
	bool PlayerPedController::isTurnLeft()
	{
		return (pressedButtons & 0b00000100) > 0;
	}
	bool PlayerPedController::isTurnRight()
	{
		return (pressedButtons & 0b00001000) > 0;
	}


	int PlayerPedController::getMove()
	{
		if (isMoveForward() && isMoveBack())
		{
			return 0;
		}
		else if (isMoveForward())
		{
			return 1;
		}
		else if (isMoveBack())
		{
			return -1;
		}
		else
		{
			return 0;
		}
	}
	bool PlayerPedController::getRunning()
	{
		return 1;
	}

	int PlayerPedController::getTurn()
	{

		if (isTurnLeft() && isTurnRight())
		{
			return 0;
		}
		else if (isTurnLeft())
		{
			return 1;
		}
		else if (isTurnRight())
		{
			return -1;
		}
		else
		{
			return 0;
		}
	}


	void PlayerPedController::updatePos(uint32_t delta)
	{

		static const float CONST_ROTATE_VELOCITY = 0.2f;
		static const float CONST_MOVE_VELOCITY = 0.001f;

		Eigen::Vector3f moveDelta{ 0, 0, 0 };


		switch (getTurn()) {
		case -1:
			rot -= CONST_ROTATE_VELOCITY * delta;
			break;
		case 1:
			rot += CONST_ROTATE_VELOCITY  * delta;
			break;
		case 0:
			break;
		}
		if (rot >= 360.0f)
		{
			rot -= 360.0f;
		}
		if (rot < 0.0f)
		{
			rot += 360.0f;
		}

	
		switch (getMove()) {
		case -1:
			moveDelta(0) -= sin(rot * M_PI / 180.0f) * CONST_MOVE_VELOCITY * delta;
			moveDelta(2) -= cos(rot * M_PI / 180.0f) * CONST_MOVE_VELOCITY * delta;
			break;
		case 1:
			moveDelta(0) += sin(rot * M_PI / 180.0f) * CONST_MOVE_VELOCITY * delta;
			moveDelta(2) += cos(rot * M_PI / 180.0f) * CONST_MOVE_VELOCITY * delta;
			break;
		case 2:
			moveDelta(0) += sin(rot * M_PI / 180.0f) * CONST_MOVE_VELOCITY * delta;
			moveDelta(2) += cos(rot * M_PI / 180.0f) * CONST_MOVE_VELOCITY * delta;
			break;
		case 0:
			break;
		}

		auto newPos = pos + moveDelta;

		int inGroundContact = checkInGroundContact(newPos);
		if (inGroundContact)
		{
			tryMove(newPos);
		}

		if (!inGroundContact) {
			speedForces(1) += 0.0005f *delta;
			pos(1) -= speedForces(1);
			if (speedForces(1) < 0.2f)
			{
				INFO << "bridge step? height: " << pos.y() << " speed: " << speedForces.y() << std::endl;
			}
			else
			{
				INFO << "FALLING " << pos.y() << " speed " << speedForces.y() << std::endl;
			}
		}
		else {
			if (speedForces(1) > 0.1)
			{
				INFO << "impacting with speed: " << speedForces.y() << std::endl;
			}
			speedForces(1) = 0.0f;
		}
	}

	int PlayerPedController::checkInGroundContact(Eigen::Vector3f nPos)
	{
		int inGroundContact;

		float hot = heightOverTerrain(nPos);
		if (hot > 0.3f)
			inGroundContact = 0;
		else if (hot < 0.0) {
			WARN << "gone below: " << hot << " at " << nPos.x() << ", " << nPos.y() << ", " << nPos.z() << std::endl;
			nPos(1) -= (hot - 0.3f);
			//nPos.y += 1;
			//INFO << nPos.y << std::endl;
			inGroundContact = 1;
		}
		else {
			inGroundContact = 1;
			nPos(1) -= hot - 0.1f;
		}

		return inGroundContact;
	}

	void PlayerPedController::tryMove(Eigen::Vector3f nPos) {
		float x, y, z;
		x = floor(nPos.x());
		y = floor(nPos.y());
		z = floor(nPos.z());


		OpenGTA::Map & map = OpenGTA::MapHolder::Instance().get();
		OpenGTA::GraphicsBase & graphics = OpenGTA::StyleHolder::Instance().get();

		

		if (y < map.getNumBlocksAtNew(PHYSFS_uint8(x), PHYSFS_uint8(z)) && y > 0.0f) {
			OpenGTA::Map::BlockInfo * block = map.getBlockAtNew(PHYSFS_uint8(x), PHYSFS_uint8(z), PHYSFS_uint8(y));
			assert(block);
			if (block->left && graphics.isBlockingSide(block->left)) {
				if (block->isFlat()) {
					if (x - pos.x() < 0 && x - pos.x() > -0.2f) {
						nPos(0) = (nPos.x() < pos.x()) ? pos.x() : nPos.x();
					}
					else if (x - pos.x() > 0 && x - pos.x() < 0.2f)
						nPos(0) = pos.x();
				}
				else {
#ifdef DEBUG_OLD_PED_BLOCK
					INFO << "xblock left: " << x - pos.x() << " tex: " << int(block->left) << std::endl;
#endif
					if (x - pos.x() > 0 && x - pos.x() < 0.2f)
						nPos(0) = pos.x();
					else if (x - pos.x() < 0 && x - pos.x() > -0.2f)
						nPos(0) = (nPos.x() < pos.x()) ? pos.x() : nPos.x();
				}
			}
			if (block->right && block->isFlat() == false) {
#ifdef DEBUG_OLD_PED_BLOCK
				INFO << "xblock right: " << pos.x() - x - 1 << " tex: " << int(block->right) << std::endl;
#endif
				if (pos.x() - x - 1 > 0 && pos.x() - x - 1 < 0.2f) {
					nPos(0) = pos.x();
				}
				else if (pos.x() - x - 1 < 0 && pos.x() - x - 1 > -0.2f)
					nPos(0) = (nPos.x() > pos.x()) ? pos.x() : nPos.x();
			}
			if (block->top && graphics.isBlockingSide(block->top)) {
				if (block->isFlat()) {
#ifdef DEBUG_OLD_PED_BLOCK
					INFO << "zblock top: " << z - pos.z() << " tex: " << int(block->top) << std::endl;
#endif
					if (z - pos.z() > 0 && z - pos.z() < 0.2f)
						nPos(2) = pos.z();
					else if (z - pos.z() < 0 && z - pos.z() > -0.2f)
						nPos(2) = (nPos.z() < pos.z()) ? pos.z() : nPos.z();
				}
				else {
#ifdef DEBUG_OLD_PED_BLOCK
					INFO << "zblock top: " << z - pos.z() << " tex: " << int(block->top) << std::endl;
#endif
					if (z - pos.z() > 0 && z - pos.z() < 0.2f)
						nPos(2) = pos.z();
					else if (z - pos.z() < 0 && z - pos.z() > -0.2f)
						nPos(2) = (nPos.z() < pos.z()) ? pos.z() : nPos.z();
				}
			}
			if (block->bottom && block->isFlat() == false) {
#ifdef DEBUG_OLD_PED_BLOCK
				INFO << "zblock bottom: " << pos.z() - z - 1 << " tex: " << int(block->bottom) << std::endl;
#endif
				if (pos.z() - z - 1 > 0 && pos.z() - z - 1 < 0.2f) {
					nPos(2) = pos.z();
				}
				else if (pos.z() - z - 1 < 0 && pos.z() - z - 1 > -0.2f)
					nPos(2) = (nPos.z() > pos.z()) ? pos.z() : nPos.z();
			}
			if (x >= 1 && y < map.getNumBlocksAtNew(PHYSFS_uint8(x - 1), PHYSFS_uint8(z))) {
				block = map.getBlockAtNew(PHYSFS_uint8(x - 1), PHYSFS_uint8(z), PHYSFS_uint8(y));
				if (block->right && block->isFlat() == false) {
#ifdef DEBUG_OLD_PED_BLOCK
					INFO << "xblock right: " << pos.x() - x << " tex: " << int(block->right) << std::endl;
#endif
					if (pos.x() - x < 0.2f) {
						nPos(0) = (nPos.x() < pos.x() ? pos.x() : nPos.x());
					}
				}
			}
			if (x < 255 && y < map.getNumBlocksAtNew(PHYSFS_uint8(x + 1), PHYSFS_uint8(z))) {
				block = map.getBlockAtNew(PHYSFS_uint8(x + 1), PHYSFS_uint8(z), PHYSFS_uint8(y));
				if (block->left && graphics.isBlockingSide(block->left)) {
#ifdef DEBUG_OLD_PED_BLOCK
					INFO << "xblock left: " << x + 1 - pos.x() << " tex: " << int(block->left) << std::endl;
#endif
					if (block->isFlat()) {
						if (x + 1 - pos.x() > 0 && x + 1 - pos.x() < 0.2f)
							nPos(0) = (nPos.x() < pos.x() ? nPos.x() : pos.x());
					}
					else {
						if (x + 1 - pos.x() < 0.2f)
							nPos(0) = (nPos.x() < pos.x() ? nPos.x() : pos.x());
					}
				}
			}
			if (z >= 1 && y < map.getNumBlocksAtNew(PHYSFS_uint8(x), PHYSFS_uint8(z - 1))) {
				block = map.getBlockAtNew(PHYSFS_uint8(x), PHYSFS_uint8(z - 1), PHYSFS_uint8(y));
				if (block->bottom && block->isFlat() == false) {
#ifdef DEBUG_OLD_PED_BLOCK
					INFO << "zblock bottom: " << pos.z() - z << " tex: " << int(block->bottom) << std::endl;
#endif
					if (pos.z() - z < 0.2f) {
						nPos(2) = (nPos.z() < pos.z() ? pos.z() : nPos.z());
					}
				}
			}
			if (z < 255 && y < map.getNumBlocksAtNew(PHYSFS_uint8(x), PHYSFS_uint8(z + 1))) {
				block = map.getBlockAtNew(PHYSFS_uint8(x), PHYSFS_uint8(z + 1), PHYSFS_uint8(y));
				if (block->top && graphics.isBlockingSide(block->top)) {
#ifdef DEBUG_OLD_PED_BLOCK
					INFO << "zblock top: " << z + 1 - pos.z() << " tex: " << int(block->top) << std::endl;
#endif
					if (block->isFlat()) {
						if (z + 1 - pos.z() > 0 && z + 1 - pos.z() < 0.2f)
							nPos(2) = (nPos.z() < pos.z() ? nPos.z() : pos.z());
					}
					else {
						if (z + 1 - pos.z() < 0.2f)
							nPos(2) = (nPos.z() < pos.z() ? nPos.z() : pos.z());
					}
				}
			}
			//if (inGroundContact)
			//  pos = nPos;
		}

		
		bool obj_blocked = false;

		/*
		std::list<Car> & list = SpriteManagerHolder::Instance().getList<Car>();
		for (std::list<Car>::iterator i = list.begin(); i != list.end(); i++) {
			if (isBoxInBox(*i)) {
				if (Util::distance(pos(), i->pos()) > Util::distance(nPos, i->pos()))
					obj_blocked = true;
			}
		}*/
		
		if (obj_blocked == false)
		{
			pos = nPos;
		}

	}










  EntityController::EntityController() :
    rawData(0),
    dataSet(sizeof(rawData) * 8, (unsigned char*)&rawData) {}

  EntityController::EntityController(const EntityController & other) :
    rawData(other.rawData),
    dataSet(sizeof(rawData) * 8, (unsigned char*)&rawData) {}

  void EntityController::zero() {
    rawData = 0;
  }

  void EntityController::setRaw(Storage_T v) {
    rawData = v;
  }

  bool ControllerWithMemory::statusChanged() {
    bool res = (rawData != lastRawData);
    if (res)
      lastRawData = rawData;
    return res; 
  }

  void PedController::setTurnLeft(bool press) {
    dataSet.set_item(0, press);
  }

  void PedController::setTurnRight(bool press) {
    dataSet.set_item(1, press);
  }

  void PedController::setMoveForward(bool press) {
    dataSet.set_item(2, press);
  }

  void PedController::setMoveBack(bool press) {
    dataSet.set_item(3, press);
  }

  void PedController::setAction(bool press) {
    dataSet.set_item(4, press);
  }

  void PedController::setJump(bool press) {
    dataSet.set_item(5, press);
  }

  void PedController::setFireWeapon(bool press) {
    dataSet.set_item(6, press);
  }

  signed char PedController::getTurn() {
    if (dataSet.get_item(0) && dataSet.get_item(1)) // special: straight ahead
      return 0;
    if (dataSet.get_item(0))
      return 1;
    else if (dataSet.get_item(1))
      return -1;
    return 0;
  }

  signed char PedController::getMove() {
    if (dataSet.get_item(2) && dataSet.get_item(3)) // special: evens out
      return 0;
    if (dataSet.get_item(2))
      return 1;
    else if (dataSet.get_item(3))
      return -1;
    return 0;
  }

  bool PedController::getAction() {
    return dataSet.get_item(4);
  }

  bool PedController::getJump() {
    return dataSet.get_item(5);
  }

  bool PedController::getFireWeapon() {
    return dataSet.get_item(6);
  }

  void PedController::setRunning(bool yes) {
    dataSet.set_item(7, yes);
  }

  bool PedController::getRunning() {
    return dataSet.get_item(7);
  }

  unsigned char PedController::getActiveWeapon() {
    // 0 .. k
    unsigned char r = 0;
    for (int j = 0; j < 3; ++j) {
      if (dataSet.get_item(j + 8))
        r += (1 << j);
    }
    return r;
  }

  void PedController::setActiveWeapon(unsigned char k) {
    if (k > 7) {
      throw E_OUTOFRANGE("foo");
    }
    for (int j = 0; j < 3; ++j) {
      if (k & 1u)
        dataSet.set_item(j + 8, true);
      else
        dataSet.set_item(j + 8, false);
      k >>= 1;
    }
  }

#if 0
#include <SDL_events.h>
#include "localplayer.h"
class ConfigurableKeyConsumer {
    static const SDLKey sym_array[];
    static const int TURN_LEFT = 0;
    static const int TURN_RIGHT = 1;
    static const int MOVE_FORWARD = 2;
    static const int MOVE_BACKWARD = 3;
  public:
    void handle(const SDL_keysym & ks, bool press) {
      PedController & pc = LocalPlayer::Instance().getCtrl();
      if (ks.sym == sym_array[TURN_LEFT]) {
        pc.setTurnLeft(true);
        return;
      }
      else if (ks.sym == sym_array[TURN_RIGHT]) {
        pc.setTurnRight(true);
        return;
      }
    }
};
#endif

#if 0
#include <SDL_events.h>
#include "localplayer.h"

  template <class ENTITY>
  class ClassicKeyConsumer {
    public:
      bool handle(const SDL_keysym & ks, bool press);
  };

  template <> bool ClassicKeyConsumer<PedController>::handle(const SDL_keysym & ks, bool press) {
    bool swallow_event = true;
    PedController & pc = LocalPlayer::Instance().getEntity().m_control;
    switch(ks.sym) {
      case SDLK_LEFT:
        pc.setTurnLeft(press);
        break;
      case SDLK_RIGHT:
        pc.setTurnRight(press);
        break;
      case SDLK_UP:
        pc.setMoveForward(press);
        break;
      case SDLK_DOWN:
        pc.setMoveBack(press);
        break;
      case SDLK_RETURN:
        pc.setAction(press);
        break;
      case SDLK_SPACE:
        pc.setJump(press);
        break;
      default:
        swallow_event = false;
        break;
    }
    return swallow_event;
  }
#endif
}