#include "gaia.hpp"

#include <cstdlib>
#include <algorithm>

#include <SDL_image.h>

#include "texturelibrary.hpp"

namespace gaia {

  void Init(int w, int h, bool vsync, e_WindowMode windowMode, const std::string &windowTitle, int renderContextResolutionMultiplier) {
    int retCode = InitSDL(w, h, vsync, windowMode, windowTitle, renderContextResolutionMultiplier);

    std::srand(SDL_GetTicks());
    assert(retCode == 0);
  }

  void Exit() {
    ExitSDL();
  }


  State::State(Game *game) : game(game) {
  }

  State::~State() {
  }


  Game::Game(int desiredGraphicsTimeStep_ms, int physicsTimeStep_ms) : activeState(nullptr), desiredGraphicsTimeStep_ms(desiredGraphicsTimeStep_ms), physicsTimeStep_ms(physicsTimeStep_ms), physicsTimeStepsDone(0), startTime_ms(0) {
    mainViewport.x = 0;
    mainViewport.y = 0;
    mainViewport.w = GetContextWidth();
    mainViewport.h = GetContextHeight();

    unsigned int numJoys = SDL_NumJoysticks();
    for (unsigned int i = 0; i < numJoys; i++) {
      if (SDL_IsGameController(i)) {
        JoyController joyController;
        joyController.sdlGameController = SDL_GameControllerOpen(i);
        if (joyController.sdlGameController != nullptr) {
          joyController.sdlJoystick = SDL_GameControllerGetJoystick(joyController.sdlGameController);
          joyController.sdlHaptic = SDL_HapticOpenFromJoystick(joyController.sdlJoystick);
          if (joyController.sdlHaptic) {
            /*
            if (SDL_HapticRumbleSupported(joyController.sdlHaptic)) {
              if (SDL_HapticRumbleInit(joyController.sdlHaptic) == 0) { // should always be 0, i guess
            */
                joyController.hasRumble = true;
                //SDL_HapticRumblePlay(joyController.sdlHaptic, 0.5f, 4000);
                //SDL_Delay(1000);
                //printf("bip\n");
            /*
              }
            }
            */
          }
          joyControllers.push_back(joyController);
        } else {
          printf("could not open joycontroller %i: %s\n", i, SDL_GetError());
        }
      }
    }

  }

  Game::~Game() {
    if (!fonts.empty()) printf("warning: fonts library not fully emptied at exit\n");

    for (unsigned int i = 0; i < joyControllers.size(); i++) {
      if (joyControllers.at(i).sdlHaptic != nullptr) SDL_HapticClose(joyControllers.at(i).sdlHaptic);
      SDL_GameControllerClose(joyControllers.at(i).sdlGameController);
    }
    joyControllers.clear();

  }

  void Game::Run() {

    unsigned int time_ms = SDL_GetTicks();
    startTime_ms = time_ms;
    unsigned int prevTime_ms = time_ms;

    bool quit = false;
    while (!quit) { // main loop

      SDL_RenderClear(GetSDLRenderer());

      SDL_Event event;
      while (SDL_PollEvent(&event)) {

        //printf("%i\n", int(event.type));

        switch (event.type) {

          case SDL_QUIT:
            quit = true;
            break;

          case SDL_KEYDOWN:
            OnKeyDown(event.key.keysym);
            //if (activeState) activeState->OnKeyDown(event.key.keysym);

            switch (event.key.keysym.sym) {
              case SDLK_F12:
                quit = true;
                break;
              default:
                break;
            }
            break;

          case SDL_KEYUP:
            OnKeyUp(event.key.keysym);
            //if (activeState) activeState->OnKeyUp(event.key.keysym);
            break;

          case SDL_CONTROLLERAXISMOTION:
            OnJoyControllerAxisEvent((int)event.caxis.which, (SDL_GameControllerAxis)event.caxis.axis, (float)event.caxis.value / 32767.0f);
            //if ((int)event.caxis.which == 0 && (int)event.caxis.axis == 0) printf("%i\n", event.caxis.value);
            break;

          case SDL_CONTROLLERBUTTONDOWN:
            OnJoyControllerButtonEvent((int)event.cbutton.which, (int)event.cbutton.button, (int)event.cbutton.state == SDL_PRESSED ? true : false );
            break;

          case SDL_CONTROLLERBUTTONUP:
            OnJoyControllerButtonEvent((int)event.cbutton.which, (int)event.cbutton.button, (int)event.cbutton.state == SDL_PRESSED ? true : false );
            break;

          default:
            break;

        }

      }

      // decide how many physics frames should be done by now, and step as needed
      int timeDelta_ms = time_ms - startTime_ms;
      unsigned long desiredPhysicsTimeSteps = timeDelta_ms / physicsTimeStep_ms;
      unsigned int physicsTimeStepsDue = std::max(desiredPhysicsTimeSteps - physicsTimeStepsDone, (unsigned long)0);
      //printf("%i\n", physicsTimeStepsDue);
      for (unsigned int i = 0; i < physicsTimeStepsDue; i++) {
        //if (activeState) activeState->OnStepPhysicsTime();
        StepPhysicsTime();
        physicsTimeStepsDone++;
      }
      
      // see what time we need to waste (if any) to get to desired framerate
      if (desiredGraphicsTimeStep_ms > 0) {
        int timeDone_ms = SDL_GetTicks() - prevTime_ms;
        int timeToWaste_ms = std::max(desiredGraphicsTimeStep_ms - timeDone_ms, 0);
        if (timeToWaste_ms > 0) SDL_Delay(timeToWaste_ms);
      }

      //if (activeState) activeState->OnStepGraphicsTime();
      StepGraphicsTime();
      SDL_SetRenderDrawColor(GetSDLRenderer(), 0, 0, 0, SDL_ALPHA_OPAQUE);
      SDL_SetRenderDrawBlendMode(GetSDLRenderer(), SDL_BLENDMODE_NONE);
      SDL_RenderPresent(GetSDLRenderer());

      time_ms = SDL_GetTicks();
      prevTime_ms = time_ms;

    } // end main loop

  }

  void Game::SetMaximizedViewport(float aspectRatio) {
    float physicalAspectRatio = GetContextWidth() / (float)GetContextHeight();
    float scalar = physicalAspectRatio / aspectRatio;
    if (scalar > 1.0f) {
      // physical display is wider than desired viewport; black bands on the sides
      int bandSize = GetContextWidth() - GetContextWidth() / scalar;
      mainViewport.x = bandSize / 2;
      mainViewport.w = GetContextWidth() - bandSize;
    } else if (scalar < 1.0f) {
      // physical display is higher than desired viewport; black bands on top and bottom
      int bandSize = GetContextHeight() - GetContextHeight() * scalar;
      mainViewport.y = bandSize / 2;
      mainViewport.h = GetContextHeight() - bandSize;
    }

/* test
    mainViewport.x = 80;
    mainViewport.y = 50;
    mainViewport.w /= 2.0;
    mainViewport.h /= 1.5;
*/

    SDL_Rect rect;
    rect.x = mainViewport.x;
    rect.y = mainViewport.y;
    rect.w = mainViewport.w;
    rect.h = mainViewport.h;
    SDL_RenderSetViewport(GetSDLRenderer(), &rect);

    //SetActiveViewportID(-1);

    //printf("physics viewport: %i x %i, virtual viewport: %i x %i\n", GetContextWidth(), GetContextHeight(), mainViewport.w, mainViewport.h);
  }

/*
  int Game::CreateVirtualViewport(const Viewport &viewport) {
    virtualViewports.push_back(viewport);
    return virtualViewports.size() - 1;
  }

  void Game::SetActiveVirtualViewportID(int id) {
    activeVirtualViewportID = id;
    Viewport viewport = GetActiveVirtualViewport();
    SDL_Rect rect;
    rect.x = viewport.x;
    rect.y = viewport.y;
    rect.w = viewport.w;
    rect.h = viewport.h;
    SDL_RenderSetViewport(GetSDLRenderer(), &rect);
  }

  Viewport Game::GetActiveVirtualViewport() {
    if (activeVirtualViewportID == -1) return mainViewport; else return virtualViewports.at(activeVirtualViewportID);
  }
*/

  // void Game::SetCamera(const Vector2 &position, float distance = 1.0f, radian rotationAngle, radian viewAngle) {
  //   camera.position = position;
  //   camera.distance = distance;
  //   camera.rotationAngle = angle;
  //   camera.viewAngle = viewAngle;
  // }

  void Game::JoyRumble(int joyControllerID, float strength, unsigned int duration_ms) const {

    assert((signed int)joyControllers.size() > joyControllerID);

    if (joyControllers.at(joyControllerID).hasRumble == true) {

      if (joyControllers.at(joyControllerID).activeSDLHapticEffectID != -1) {
        SDL_HapticDestroyEffect(joyControllers.at(joyControllerID).sdlHaptic, joyControllers.at(joyControllerID).activeSDLHapticEffectID);
      }

      //SDL_HapticRumblePlay(joyControllers.at(joyControllerID).sdlHaptic, strength, time_ms);

      SDL_HapticEffect sdlHapticRumbleEffect;
      memset(&sdlHapticRumbleEffect, 0, sizeof(SDL_HapticEffect));

      //sdlHapticRumbleEffect.type = SDL_HAPTIC_SINE;
      //sdlHapticRumbleEffect.periodic.direction.type = SDL_HAPTIC_POLAR; // Polar coordinates
      //sdlHapticRumbleEffect.periodic.direction.dir[0] = 18000; // Force comes from south

      #ifndef _WIN32
        sdlHapticRumbleEffect.type = SDL_HAPTIC_SINE;
      #else // m$ xinput doesn't support sine FOR SOME STUPID REASON (vendor lock-in?)
        sdlHapticRumbleEffect.type = SDL_HAPTIC_LEFTRIGHT;
      #endif

      Sint16 magnitude = (Sint16)(32767.0f * strength);
      if (sdlHapticRumbleEffect.type == SDL_HAPTIC_SINE) {
        sdlHapticRumbleEffect.periodic.magnitude = magnitude;
        sdlHapticRumbleEffect.periodic.length = duration_ms;
        sdlHapticRumbleEffect.periodic.period = 1000;
        sdlHapticRumbleEffect.periodic.attack_length = 0;
        sdlHapticRumbleEffect.periodic.fade_length = 0;
      } else if (sdlHapticRumbleEffect.type == SDL_HAPTIC_LEFTRIGHT) {
        sdlHapticRumbleEffect.leftright.small_magnitude = sdlHapticRumbleEffect.leftright.large_magnitude = magnitude;
        sdlHapticRumbleEffect.leftright.length = duration_ms;
      }

      // upload the effect
      joyControllers.at(joyControllerID).activeSDLHapticEffectID = SDL_HapticNewEffect(joyControllers.at(joyControllerID).sdlHaptic, &sdlHapticRumbleEffect);
      SDL_HapticRunEffect(joyControllers.at(joyControllerID).sdlHaptic, joyControllers.at(joyControllerID).activeSDLHapticEffectID, 1);
      //printf("%f, %i\n", strength, duration_ms);
      //SDL_HapticRunEffect(joyControllers.at(joyControllerID).sdlHaptic, static_cast<_SDL_Haptic*>(joyControllers.at(joyControllerID).sdlHaptic)->rumble_id, 1);

      //SDL_Delay(5000); // Wait for the effect to finish
      // We destroy the effect, although closing the device also does this
      //SDL_HapticDestroyEffect( joyControllers.at(joyControllerID).sdlHaptic, effect_id );
    }
  }

  TTF_Font *Game::GetFont(int uniqueID) const {
    std::map<int, TTF_Font*>::const_iterator iter = fonts.find(uniqueID);
    return iter->second;
  }

  void Game::CreateFont(int uniqueID, const std::string &filename, int ptsize) {
    TTF_Font *font = TTF_OpenFont(filename.c_str(), ptsize);
    if (font == nullptr) {
      printf("could not load font file %s\n", filename.c_str());
      exit(EXIT_FAILURE);
    }
    fonts.insert(std::pair<int, TTF_Font*>(uniqueID, font));
  }

  void Game::DeleteFont(int uniqueID) {
    std::map<int, TTF_Font*>::iterator iter = fonts.find(uniqueID);
    TTF_CloseFont(iter->second);
    fonts.erase(iter);
  }


  // private functions

  void Game::StepPhysicsTime() {
    OnStepPhysicsTime();
    if (activeState) activeState->OnStepPhysicsTime();
  }

  void Game::StepGraphicsTime() {
    OnStepGraphicsTime();
    if (activeState) activeState->OnStepGraphicsTime();
  }



  // SPRITE


  Sprite::Sprite() : texture(nullptr), flip(SDL_FLIP_NONE), w_cached(0), h_cached(0), alpha(1.0f), xSubsprites(1), ySubsprites(1), selectedXSubsprite(0), selectedYSubsprite(0) {
  }

  Sprite::~Sprite() {
    DeleteTexture();
  }

  void Sprite::LoadFile(const std::string &filename, e_FilterMode filterMode, unsigned int xSubsprites, unsigned int ySubsprites) {
    assert(texture == nullptr);
    texture = TextureLibrary::GetInstance().FetchTexture(filename, filterMode, &w_cached, &h_cached);
    this->xSubsprites = xSubsprites;
    this->ySubsprites = ySubsprites;
  }

  void Sprite::SelectSubsprite(unsigned int x, unsigned int y) {
    assert(x >= 0 && y >= 0);
    assert(x <= xSubsprites && y <= ySubsprites);
    this->selectedXSubsprite = x;
    this->selectedYSubsprite = y;
  }

  void Sprite::CreateText(TTF_Font *font, const std::string &text, const SDL_Color &color, e_FilterMode filterMode) {
    //assert(texture == nullptr);
    DeleteTexture();
    texture = RenderText(font, text, color, filterMode, &w_cached, &h_cached);
  }

  void Sprite::DeleteTexture() {
    if (texture != nullptr) {
      TextureLibrary::GetInstance().ReleaseTexture(texture);      
      w_cached = 0;
      h_cached = 0;
      texture = nullptr;
    }
  }

  void Sprite::Draw(const Spatial &spatial) const {
    /*
    SDL_Rect dst;
    dst.x = spatial.position.coords[0] - spatial.scale.coords[0] / 2;
    dst.y = spatial.position.coords[1] - spatial.scale.coords[1] / 2;
    dst.w = spatial.scale.coords[0];
    dst.h = spatial.scale.coords[1];
    */

    // inverted scales are not supported by sdl, we need to 'convert' them to flip settings
    int tmpFlip = flip;
    Vector2 tmpScale = spatial.scale;
    if (tmpScale.coords[0] < 0) {
      tmpScale.coords[0] = -tmpScale.coords[0];
      tmpFlip |= SDL_FLIP_HORIZONTAL;
    }
    if (tmpScale.coords[1] < 0) {
      tmpScale.coords[1] = -tmpScale.coords[1];
      tmpFlip |= SDL_FLIP_VERTICAL;
    }

    SDL_Rect src;
    src.x = GetWidth() * selectedXSubsprite;
    src.y = GetHeight() * selectedYSubsprite;
    src.w = GetWidth();
    src.h = GetHeight();

    SDL_Rect dst;
    dst.x = floor(spatial.position.coords[0] - tmpScale.coords[0] / 2.0f);
    dst.y = floor(spatial.position.coords[1] - tmpScale.coords[1] / 2.0f);
    dst.w = floor(tmpScale.coords[0]); // used to be ceil()
    dst.h = floor(tmpScale.coords[1]); // used to be ceil()

    //printf("%i, %i, %i, %i\n", dst.x, dst.y, dst.w, dst.h);
    SDL_SetTextureAlphaMod(texture, int(round(alpha * 255.0f)));
    SDL_RenderCopyEx(GetSDLRenderer(), texture, &src, &dst, (spatial.orientation) / pi * 180.0f, nullptr, static_cast<SDL_RendererFlip>(tmpFlip));
  }

  void Sprite::Draw(const Spatial &spatial, const Viewport &viewport, const Camera &camera) const {

    Spatial screenSpatial = WorldToScreenSpatial(spatial, viewport, camera);
    Draw(screenSpatial);
    /*
    SDL_Rect dst;
    dst.x = floor(screenSpatial.position.coords[0] - screenSpatial.scale.coords[0] / 2.0f);
    dst.y = floor(screenSpatial.position.coords[1] - screenSpatial.scale.coords[1] / 2.0f);
    dst.w = ceil(screenSpatial.scale.coords[0]);
    dst.h = ceil(screenSpatial.scale.coords[1]);
    //printf("%i, %i, %i, %i\n", dst.x, dst.y, dst.w, dst.h);
    SDL_RenderCopyEx(GetSDLRenderer(), texture, NULL, &dst, (screenSpatial.orientation) / pi * 180.0f, nullptr, flip);
    */
  }

  void Sprite::SetAlpha(float a) {
    alpha = a;
  }

  void Sprite::SetFlip(SDL_RendererFlip newFlip) {
    flip = newFlip;
  }


  // ANIMATED SPRITE

  AnimatedSprite::AnimatedSprite() : Sprite(), selectedSequence(0), currentFrame(0), timeIntoCurrentFrame_ms(0) {
  }

  AnimatedSprite::~AnimatedSprite() {
  }

  unsigned int AnimatedSprite::AddSequence(const AnimatedSpriteSequence &sequence) {
    sequences.push_back(sequence);
    return sequences.size() - 1;
  }

  void AnimatedSprite::OnStepTime(unsigned int timeStep_ms) {
    timeIntoCurrentFrame_ms += timeStep_ms;

    bool overflow = true;
    while (overflow) {
      int netto = sequences.at(selectedSequence).at(currentFrame).sustain_ms - timeIntoCurrentFrame_ms;
      if (netto < 0) {
        currentFrame++;
        if (currentFrame >= sequences.at(selectedSequence).size()) {
          currentFrame = 0;
        }
        //printf("s: %i, f: %i, x: %i\n", sequences.at(selectedSequence).size(), currentFrame, sequences.at(selectedSequence).at(currentFrame).selectedXSubsprite);
        timeIntoCurrentFrame_ms = -netto;
      } else {
        overflow = false;
      }

      this->SelectSubsprite(sequences.at(selectedSequence).at(currentFrame).selectedXSubsprite,
                            sequences.at(selectedSequence).at(currentFrame).selectedYSubsprite);

    }

  }


/*
  // TEXT


  Text::Text(TTF_Font *font, const std::string &caption) {

  }

  Text::~Text() {
  }

  void Text::Draw(const Spatial &spatial) const {
    SDL_Rect dst;
    dst.x = floor(spatial.position.coords[0] - spatial.scale.coords[0] / 2.0f);
    dst.y = floor(spatial.position.coords[1] - spatial.scale.coords[1] / 2.0f);
    dst.w = floor(spatial.scale.coords[0]); // used to be ceil()
    dst.h = floor(spatial.scale.coords[1]); // used to be ceil()

    //printf("%i, %i, %i, %i\n", dst.x, dst.y, dst.w, dst.h);
    SDL_RenderCopyEx(GetSDLRenderer(), texture, NULL, &dst, (spatial.orientation) / pi * 180.0f, nullptr);
  }

  void Text::Draw(const Spatial &spatial, const Viewport &viewport, const Camera &camera) const {
  }

  void Text::SetAlpha(float a = 1.0f) {
    SDL_SetTextureAlphaMod(texture, int(round(a * 255.0f)));
  }
*/



  // DATAMAP


  DataMap::DataMap() : data(0) {
  }

  DataMap::DataMap(const DataMap &src) {
    this->data = new float[src.w * src.h];
    memcpy(data, src.data, src.w * src.h * sizeof(float));
    w = src.w;
    h = src.h;
  }

  DataMap::~DataMap() {
    if (data) delete [] data;
  }

  void DataMap::ImportFile(const std::string &filename) {
    SDL_Surface *surface = IMG_Load(filename.c_str());
    assert(surface != nullptr);
    w = surface->w;
    h = surface->h;
    data = new float[w * h];

    for (int y = 0; y < h; y++) {
      for (int x = 0; x < w; x++) {
        Uint32 pixel = GetPixel(surface, x, y);
        data[x + y * w] = (Uint8)pixel / 256.0f; // take red component only
        //printf("%1.1f ", data[x + y * w]);
        //printf("%i ", pixel);
      }
    }

    SDL_FreeSurface(surface);
  }

  void DataMap::ResizeFractional(float w_factor, float h_factor) {
    int w_new = w * w_factor;
    int h_new = h * h_factor;
    float *data_new = new float[w_new * h_new];
    for (int x = 0; x < w_new; x++) {
      for (int y = 0; y < h_new; y++) {
        //data_new[x + y * w_new] = data[int(x / w_factor + y / h_factor * w)];
        data_new[x + y * w_new] = data[int(round(x / w_factor)) + int(round(y / h_factor)) * w];
        assert(int(round(x / w_factor)) + int(round(y / h_factor)) * w < w * h);
      }
    }
    delete [] data;
    data = data_new;
    w = w_new;
    h = h_new;
  }

  float DataMap::GetLocationData(const Vector2 &location, bool interpolate) const {

    if (!interpolate) {

      int x = round(location.coords[0] * w);
      int y = round(location.coords[1] * h);
      x = std::min(w, std::max(0, x));
      y = std::min(h, std::max(0, y));
      return data[x + y * w];

    } else {

      printf("interpolated values are untested! unittest this function! y u do dis\n");

      int x1 = floor(location.coords[0] * w);
      int y1 = floor(location.coords[1] * h);
      int x2 = floor(location.coords[0] * w); // used to be ceil()
      int y2 = floor(location.coords[1] * h); // used to be ceil()
      float xbias = location.coords[0] - floor(location.coords[0]);
      float ybias = location.coords[1] - floor(location.coords[1]);
      x1 = std::min(w, std::max(0, x1));
      y1 = std::min(h, std::max(0, y1));
      x2 = std::min(w, std::max(0, x2));
      y2 = std::min(h, std::max(0, y2));
      return ( data[x1 + y1 * w] * (1.0f - xbias) + data[x2 + y1 * w] * (xbias) ) * (1.0f - ybias) +
             ( data[x1 + y2 * w] * (1.0f - xbias) + data[x2 + y2 * w] * (xbias) ) * (ybias);

    }

  }

  float GetHeuristicTargetDistance(const PathNode *from, const PathNode *to) {
    int dx = abs(from->x - to->x);
    int dy = abs(from->y - to->y);
    //return sqrt(dx * dx + dy * dy);
    const float cost = 1.0f;
    const float costDiag = 1.414f;
    return cost * (dx + dy) + (costDiag - 2 * cost) * std::min(dx, dy);
  }

  bool SortPathNodeDistance(const PathNode *comp1, const PathNode *comp2) {
    return comp1->GetTotalDistance() < comp2->GetTotalDistance();
  }

  void DataMap::PathFinding(const std::vector<Vector2> &inputWaypoints, std::vector<Vector2> &outputWaypoints, bool keepInputWaypoints) {

    // pathfinding
    std::list<Vector2> intermediateWaypointList;
    std::vector<Vector2> intermediateWaypoints;
    for (unsigned int waypoint = 1; waypoint < inputWaypoints.size(); waypoint++) {

      PathNode *grid[w * h];
      for (int x = 0; x < w; x++) {
        for (int y = 0; y < h; y++) {
          grid[x + y * w] = new PathNode();
          grid[x + y * w]->x = x;
          grid[x + y * w]->y = y;
        }
      }

      std::list<PathNode*> open;
      std::list<PathNode*> closed;

      //const Vector2 &target = waypoints.at(waypoint);
      PathNode *startNode = grid[int(round(inputWaypoints.at(waypoint - 1).coords[0])) + int(round(inputWaypoints.at(waypoint - 1).coords[1])) * w];
      PathNode *targetNode = grid[int(round(inputWaypoints.at(waypoint).coords[0])) + int(round(inputWaypoints.at(waypoint).coords[1])) * w];

      startNode->open = true;
      open.push_back(startNode);//PathNode(target));
      //openGrid[startNode->x + startNode->y * w] = true;

      while (open.empty() == false && open.front() != targetNode) {

        PathNode *current = open.front();

        //openGrid[open.first.x + open.first.y * w] = false;
        current->open = false;
        open.pop_front();

        current->closed = true;
        closed.push_back(current);
        //closedGrid[closed.back.x + closed.back.y * w] = true;

        std::vector<PathNode*> neighbors;
        if (current->x > 0     && current->y > 0    ) neighbors.push_back(grid[current->x - 1 + (current->y - 1) * w]);
        if (                      current->y > 0    ) neighbors.push_back(grid[current->x     + (current->y - 1) * w]);
        if (current->x < w - 1 && current->y > 0    ) neighbors.push_back(grid[current->x + 1 + (current->y - 1) * w]);
        if (current->x < w - 1                      ) neighbors.push_back(grid[current->x + 1 +  current->y      * w]);
        if (current->x < w - 1 && current->y < h - 1) neighbors.push_back(grid[current->x + 1 + (current->y + 1) * w]);
        if (                      current->y < h - 1) neighbors.push_back(grid[current->x     + (current->y + 1) * w]);
        if (current->x > 0     && current->y < h - 1) neighbors.push_back(grid[current->x - 1 + (current->y + 1) * w]);
        if (current->x > 0                          ) neighbors.push_back(grid[current->x - 1 +  current->y      * w]);

        for (unsigned int n = 0; n < neighbors.size(); n++) {

          float cost = 1.0f;
          if (n == 0 || n == 2 || n == 4 || n == 6) cost = 1.414f;
          float startDistance = current->startDistance + cost;


          if (data[neighbors.at(n)->x + neighbors.at(n)->y * w] > 0.25f && data[neighbors.at(n)->x + neighbors.at(n)->y * w] < 0.75f) {


            // neighbor is open AND cheaper when coming from current? remove from open list (will be added as candidate later)

            if (neighbors.at(n)->open == true && neighbors.at(n)->startDistance > startDistance) {

              neighbors.at(n)->open = false;
              std::list<PathNode*>::iterator iter = open.begin(); while (iter != open.end()) {
                if (*iter == neighbors.at(n)) { open.erase(iter); break; } else iter++;
              }

            }


            // neighbor is closed AND cheaper when coming from current? remove from closed list (will be added as candidate later)

            if (neighbors.at(n)->closed == true && neighbors.at(n)->startDistance > startDistance) {

              neighbors.at(n)->closed = false;
              std::list<PathNode*>::iterator iter = closed.begin(); while (iter != closed.end()) {
                if (*iter == neighbors.at(n)) { closed.erase(iter); break; } else iter++;
              }

            }


            // candidate

            if (neighbors.at(n)->open == false && neighbors.at(n)->closed == false) {

              neighbors.at(n)->startDistance = startDistance;
              neighbors.at(n)->targetDistance = GetHeuristicTargetDistance(neighbors.at(n), targetNode);

              open.push_front(neighbors.at(n));
              neighbors.at(n)->open = true;

              neighbors[n]->parent = current;

            }

          } // non blocking

        }

        open.sort(SortPathNodeDistance); // closest candidates first
        //std::sort(open.begin(), open.end(), SortPathNodeDistance);
      }
      //printf("done\n");

      // traverse back through parent nodes
      PathNode *current = targetNode;
      while (current != startNode) {
        //data[current->x + current->y * w] = 1.0f;
        intermediateWaypointList.push_front(Vector2(current->x, current->y));
        if (!current->parent) break;
        current = current->parent;
      }

      // 'convert' to vector for easier element access
      assert(intermediateWaypointList.size() > 0);
      std::list<Vector2>::iterator waypointIter = intermediateWaypointList.begin();
      while (waypointIter != intermediateWaypointList.end()) {
        intermediateWaypoints.push_back(*waypointIter);
        waypointIter++;
      }

      intermediateWaypointList.clear();

      for (int i = 0; i < w * h; i++) delete grid[i];
    }


    // line smoothing

    bool lineOfSightFound = true;
    while (lineOfSightFound && intermediateWaypoints.size() > 2) {

      for (unsigned int i = 0; i < intermediateWaypoints.size(); i++) {

        bool readonly = false;

        if (keepInputWaypoints) {
          // don't delete input waypoints
          Vector2 target = intermediateWaypoints.at((i + 1) % intermediateWaypoints.size());
          for (unsigned int in = 0; in < inputWaypoints.size(); in++) {
            if (target == inputWaypoints.at(in)) {
              readonly = true;
              break;
            }
          }
        }

        if (!readonly) {

          lineOfSightFound = false;

          Vector2 p1 = intermediateWaypoints.at(i);
          Vector2 p2 = intermediateWaypoints.at((i + 2) % intermediateWaypoints.size());

          bool interrupted = false;
          float lineLength = (p1 - p2).GetLength();
          for (int p = 1; p < lineLength - 1; p++) {
            Vector2 checkPoint = p1 + (p2 - p1) * (p / lineLength);
            /* debug
            if ((i + 1) % intermediateWaypoints.size() == 1) {
              p1.Print();
              p2.Print();
              checkPoint.Print();
              printf("\n");
            }
            */
            /* version that also uses neighboring elements - won't work when the center nodes are 'illegal' according to this already (which they are, since the above neighbor thing allows this)
            for (int x_offset = -1; x_offset < 2; x_offset++) {
              for (int y_offset = -1; y_offset < 2; y_offset++) {
                int x_address = int(round(checkPoint.coords[0] + x_offset * 0.5f));
                int y_address = int(round(checkPoint.coords[1] + y_offset * 0.5f));
                if (x_address > 0 && x_address < w && y_address > 0 && y_address < h) {
                  float value = data[x_address + y_address * w];
                  if (value <= 0.25f || value >= 0.75f) {
                    interrupted = true;
                    break;
                  }
                }
              }
              if (interrupted) break;
            }
            if (interrupted) break;
            */
            float value = data[int(round(checkPoint.coords[0])) + int(round(checkPoint.coords[1])) * w];
            if (value <= 0.25f || value >= 0.75f) {
              interrupted = true;
              break;
            }
          }
          if (!interrupted) lineOfSightFound = true;

          //printf("testing %i (from %i to %i)..", (i + 1) % intermediateWaypoints.size(), i, (i + 2) % intermediateWaypoints.size());
          if (lineOfSightFound) {
            intermediateWaypoints.erase(intermediateWaypoints.begin() + (i + 1) % intermediateWaypoints.size());
            //printf("deleting\n");
            break;
          } //else printf("\n");

        } // /readonly

      }
    }


    // output

    for (unsigned int i = 0; i < intermediateWaypoints.size(); i++) {
      outputWaypoints.push_back(intermediateWaypoints.at(i));
    }

  }



  // OBJECT


  float Object::GetDataMapLocationData(const Vector2 &location, bool interpolate) const {
    // location is in real world coordinates. we need to convert this to a pixel location on the datamap.

    Spatial derivedSpatial = this->GetDerivedSpatial(); // this is position/scale etc data from this object
    Vector2 dataMapLocation;
    dataMapLocation = location - derivedSpatial.position;
    dataMapLocation = dataMapLocation / derivedSpatial.scale;
    dataMapLocation.Rotate(-derivedSpatial.orientation);
    //dataMapLocation *= 0.5f;
    dataMapLocation += 0.5f;

    //location.Print();
    //dataMapLocation.Print();
    assert(dataMap);
    return dataMap->GetLocationData(dataMapLocation, interpolate);
  }



  // PARTICLE SYSTEM


  ParticleSystem::ParticleSystem(float emissionPerSecond, int timeToLive_ms, int timeToLiveVariation_ms, const Vector2 &startPosition, const Vector2 &startMomentum, Sprite *sprite) : emissionPerSecond(emissionPerSecond), emissionRemainder(0), timeToLive_ms(timeToLive_ms), timeToLiveVariation_ms(timeToLiveVariation_ms), sprite(sprite) {
    spatialNode = new SpatialNode();
    spatialNode->spatial.position = startPosition;
    momentum = startMomentum;
  }

  ParticleSystem::~ParticleSystem() {
    std::list<Particle>::iterator particleIter = particles.begin();
    while (particleIter != particles.end()) {
      delete particleIter->object;
      particleIter = particles.erase(particleIter);
    }
    delete spatialNode;
  }

  void ParticleSystem::OnStepPhysicsTime(const Game *game) {

    int timeStep_ms = game->GetPhysicsTimeStep_ms();

    // new
    float emission = emissionPerSecond * (timeStep_ms * 0.001f) + emissionRemainder;
    int emissionInteger = int(floor(emission));
    emissionRemainder = emission - emissionInteger;
    for (int i = 0; i < emissionInteger; i++) {
      Particle particle;
      particle.object = new Object(0, sprite);
      particle.object->spatial = spatialNode->spatial;
      particle.alpha = 1.0f;
      particle.momentum = this->momentum;
      particle.timeToLive_ms = timeToLive_ms + int(round(timeToLiveVariation_ms * Random(-0.5f, 0.5f)));
      particles.push_back(particle);
    }

    // update
    std::list<Particle>::iterator particleIter = particles.begin();
    while (particleIter != particles.end()) {

      particleIter->object->spatial.position += particleIter->momentum * timeStep_ms * 0.001f;

      UpdateParticle(*particleIter, timeStep_ms);

      particleIter->timeToLive_ms -= timeStep_ms;
      if (particleIter->timeToLive_ms <= 0) {
        delete particleIter->object;
        particleIter = particles.erase(particleIter);
      } else {
        particleIter++;
      }
    }

  }

  void ParticleSystem::Draw(const Viewport &viewport, const Camera &camera) {
    std::list<Particle>::iterator particleIter = particles.begin();
    while (particleIter != particles.end()) {
      particleIter->object->GetSprite()->SetAlpha(particleIter->alpha);
      particleIter->object->Draw(viewport, camera);
      particleIter++;
    }
  }

}
