#include "simulation.h"

#include "consts.h"
#include "level.h"

Simulation::Simulation(
  const Level& level) :
    level_(level)
{
  id_type player = emplaceEntity();
  Entity& entity = getEntity(player);
  entity.size = TILE_SIZE;
  entity.speed = 3.0;
  entity.controllable = true;
  entity.colliderType = ColliderType::player;
  entity.colorVal = 180;
  entity.gridPos = vec2s { 1, 5 };

  id_type crateId = emplaceEntity();
  Entity& crate = getEntity(crateId);
  crate.size = TILE_SIZE;
  crate.speed = static_cast<double>(schedule_.getBPM()) / 30.0;
  crate.colliderType = ColliderType::crate;
  crate.canBePushedBy.insert(ColliderType::player);
  crate.canBePushedBy.insert(ColliderType::crate);
  crate.canBePushedBy.insert(ColliderType::train);
  crate.gridPos = vec2s { 4, 5 };

  id_type crateId2 = emplaceEntity();
  Entity& crate2 = getEntity(crateId2);
  crate2.size = TILE_SIZE;
  crate2.speed = static_cast<double>(schedule_.getBPM()) / 30.0;
  crate2.colliderType = ColliderType::crate;
  crate2.canBePushedBy.insert(ColliderType::player);
  crate2.canBePushedBy.insert(ColliderType::crate);
  crate2.canBePushedBy.insert(ColliderType::train);
  crate2.gridPos = vec2s { 6, 7 };

  id_type trainId = emplaceEntity();
  Entity& train = getEntity(trainId);
  train.size = TILE_SIZE;
  train.speed = static_cast<double>(schedule_.getBPM()) / 30.0;
  train.colliderType = ColliderType::train;
  train.scheduled = true;
  train.colorVal = 90;
  train.gridPos = vec2s { 6, 1 };




  for (id_type id : active_)
  {
    Entity& entity = entities_.at(id);

    posCache_.set(id, entity.gridPos);
  }
}

void Simulation::tick(
  double dt,
  const Uint8* keystate)
{
  // Control
  for (id_type id : active_)
  {
    Entity& entity = entities_.at(id);

    if (entity.controllable &&
        !entity.moving)
    {
      if (keystate[SDL_SCANCODE_LSHIFT] || keystate[SDL_SCANCODE_RSHIFT])
      {
        Direction lookDir = Direction::none;

        if (keystate[SDL_SCANCODE_LEFT])
        {
          lookDir = Direction::left;
        }
        else if (keystate[SDL_SCANCODE_UP])
        {
          lookDir = Direction::up;
        }
        else if (keystate[SDL_SCANCODE_RIGHT])
        {
          lookDir = Direction::right;
        }
        else if (keystate[SDL_SCANCODE_DOWN])
        {
          lookDir = Direction::down;
        }

        vec2s lookPos = posInDir(entity.gridPos, lookDir);

        for (id_type blockId : posCache_.at(lookPos))
        {
          Entity& block = entities_.at(blockId);

          if (!block.moving && block.canBePushedBy.count(ColliderType::player))
          {
            moveEntityOnGrid(blockId, lookDir);
          }
        }
      } else {
        if (keystate[SDL_SCANCODE_LEFT] &&
            moveEntityOnGrid(id, Direction::left, true))
        {
          entity.shouldMoveTo = Direction::left;
        }
        else if (keystate[SDL_SCANCODE_UP] &&
                moveEntityOnGrid(id, Direction::up, true))
        {
          entity.shouldMoveTo = Direction::up;
        }
        else if (keystate[SDL_SCANCODE_RIGHT] &&
                moveEntityOnGrid(id, Direction::right, true))
        {
          entity.shouldMoveTo = Direction::right;
        }
        else if (keystate[SDL_SCANCODE_DOWN] &&
                moveEntityOnGrid(id, Direction::down, true))
        {
          entity.shouldMoveTo = Direction::down;
        }
        else
        {
          entity.shouldMoveTo = Direction::none;
        }

        if (entity.shouldMoveTo != Direction::none)
        {
          moveEntityOnGrid(id, entity.shouldMoveTo);
        }
      }
    }
  }

  // Schedule
  schedule_.accumulate(dt);

  if (schedule_.step())
  {
    for (id_type id : active_)
    {
      Entity& entity = entities_.at(id);

      if (entity.scheduled && !entity.moving)
      {
        moveEntityOnGrid(id, Direction::down);
      }
    }
  }







  // Collision




  // Movement
  for (id_type id : active_)
  {
    Entity& entity = entities_.at(id);

    if (entity.moving)
    {
      entity.movementTween += entity.speed * dt;

      if (entity.movementTween >= 1.0)
      {
        entity.moving = false;
        entity.gridPos = entity.destPos;
        posCache_.set(id, entity.gridPos);
        moveToCache_.remove(id);
      }
    }

    if (entity.moving)
    {
      entity.pos.x() =
        TILE_SIZE.x() * entity.destPos.x() * entity.movementTween +
        TILE_SIZE.x() * entity.gridPos.x() * (1.0 - entity.movementTween);

      entity.pos.y() =
        TILE_SIZE.y() * entity.destPos.y() * entity.movementTween +
        TILE_SIZE.y() * entity.gridPos.y() * (1.0 - entity.movementTween);
    } else {
      entity.pos = TILE_SIZE * entity.gridPos;
    }
  }
}

Simulation::id_type Simulation::emplaceEntity()
{
  id_type nextId;

  if (!available_.empty())
  {
    nextId = available_.front();
    available_.pop_front();

    entities_.at(nextId) = Entity();
  } else {
    nextId = entities_.size();
    entities_.emplace_back();
  }

  active_.insert(nextId);

  return nextId;
}

void Simulation::deleteEntity(id_type id)
{
  available_.push_back(id);
  active_.erase(id);
}

bool Simulation::moveEntityOnGrid(
  id_type id,
  Direction moveDir,
  bool validate)
{
  bool actuallyMove = true;

  Entity& entity = entities_.at(id);

  vec2s shouldMoveTo = posInDir(entity.gridPos, moveDir);

  switch (moveDir)
  {
    case Direction::left:
    {
      if (entity.gridPos.x() == 0)
      {
        return false;
      }

      break;
    }

    case Direction::right:
    {
      if (entity.gridPos.x() == level_.getSize().w() - 1)
      {
        return false;
      }

      break;
    }

    case Direction::up:
    {
      if (entity.gridPos.y() == 0)
      {
        return false;
      }

      break;
    }

    case Direction::down:
    {
      if (entity.gridPos.y() == level_.getSize().h() - 1)
      {
        return false;
      }

      break;
    }
  }

  if (!level_.getTileset().canEntityMoveTo(
        entity.colliderType,
        level_.at(shouldMoveTo)))
  {
    return false;
  }

  // Can't move into a space that something else is already moving into.
  if (!moveToCache_.at(shouldMoveTo).empty())
  {
    return false;
  }

  for (id_type blockId : posCache_.at(shouldMoveTo))
  {
    Entity& block = entities_.at(blockId);

    if (!block.moving)
    {
      if (!block.canBePushedBy.count(entity.colliderType))
      {
        return false;
      }

      if (!moveEntityOnGrid(blockId, moveDir, validate))
      {
        return false;
      }
    } else if (block.moveDir != moveDir)
    {
      // Can't move perpendicularly into a space that something else is moving
      // out of.
      return false;
    }

    double entityTimeLeft = 1.0 / entity.speed;
    double blockTimeLeft = (1.0 - block.movementTween) / block.speed;

    if (entityTimeLeft < blockTimeLeft)
    {
      actuallyMove = false;
    }
  }





  if (!validate && actuallyMove)
  {
    entity.moving = true;
    entity.destPos = shouldMoveTo;
    entity.movementTween = 0.0;
    entity.moveDir = moveDir;

    moveToCache_.set(id, entity.destPos);
  }

  return true;
}

vec2s Simulation::posInDir(vec2s orig, Direction dir)
{
  switch (dir)
  {
    case Direction::left: return orig - vec2s { 1, 0 };
    case Direction::right: return orig + vec2s { 1, 0 };
    case Direction::up: return orig - vec2s { 0, 1 };
    case Direction::down: return orig + vec2s { 0, 1 };
    case Direction::none: return orig;
  }
}