#include <vector>
#include <list>
#include <algorithm>
#include <iostream>

/** This class can be extended to contain graphics information
    and physics as well.   Right now it contains the minimum
    necessary to do group collision tests */
class SpriteRect
{
public:
  SpriteRect(int x_, int y_, int w_, int h_)
    : x(x_), y(y_), w(w_), h(h_), midx(x_+w_/2), midy(y_+h_/2) {}
  // Position
  int x,y;
  // Size
  int w,h;
  // Middle point
  int midx,midy;
};

typedef std::vector<SpriteRect>           SpriteVector;
typedef std::vector<SpriteRect>::iterator SpriteIterator;
typedef std::list<SpriteRect*>            SpriteList;
typedef std::list<SpriteRect*>::iterator  SpriteListIterator;

void FullCollisionTest(const SpriteRect& s1, const SpriteRect& s2)
{
  // Here we'll plug in the full collision test.
  std::cout << "Testing for collision between: " <<
    s1.x << ',' << s1.y << ',' << s1.w << ',' << s1.h << "   and   " <<
    s2.x << ',' << s2.y << ',' << s2.w << ',' << s2.h << std::endl;
}

/** Represents a single cell in the grid.
    The cell will hold a list of sprites that it currently contains */
class GridCell
{
public:
  /** */
  void Add(SpriteRect* sprite)
  {
    m_List.push_back(sprite);
  }

  void Remove(SpriteRect* sprite)
  {
    SpriteListIterator i=FindIterator(sprite);
    if (i==m_List.end()) return;
    m_List.erase(i);
  }

  bool Exists(SpriteRect* sprite)
  {
    return (!(FindIterator(sprite) == m_List.end()));
  }

  bool IsPopulated() const
  {
    return (!m_List.empty());
  }

  bool MultipleSprites() const
  {
    return (m_List.size() > 1);
  }

  void Check(GridCell& c)
  {
    for(SpriteListIterator i=m_List.begin();i!=m_List.end();i++)
    {
      for(SpriteListIterator j=c.m_List.begin();j!=c.m_List.end();j++)
        FullCollisionTest(**i,**j);
    }
  }

  void CheckSelf()
  {
    for(SpriteListIterator i=m_List.begin();i!=m_List.end();i++)
    {
      for(SpriteListIterator j=i;++j!=m_List.end();)
        FullCollisionTest(**i,**j);
    }
  }

private:
  SpriteListIterator FindIterator(SpriteRect* sprite)
  {
    return std::find(m_List.begin(),m_List.end(),sprite);
  }

  SpriteList m_List;
};



class Grid
{
public:
  Grid(int max_sprite_size_x, int max_sprite_size_y, 
       int world_left, int world_top, int world_right, int world_bottom)
   : m_Left(world_left),
     m_Top(world_top),
     m_Right(world_right),
     m_Bottom(world_bottom),
     m_CellWidth(max_sprite_size_x),
     m_CellHeight(max_sprite_size_y),
     m_CellsNumberX((world_right-world_left)/max_sprite_size_x),
     m_CellsNumberY((world_bottom-world_top)/max_sprite_size_y),
     m_Grid(new GridCell[m_CellsNumberX*m_CellsNumberY])
  {}

  void Add(SpriteRect* sprite)
  {
    int x=CalculateCellXPosition(sprite->midx);
    int y=CalculateCellYPosition(sprite->midy);
    GetCell(x,y).Add(sprite);
  }

  void Remove(SpriteRect* sprite)
  {
    int x=CalculateCellXPosition(sprite->midx);
    int y=CalculateCellYPosition(sprite->midy);
    GetCell(x,y).Remove(sprite);
  }

  void Move(SpriteRect* sprite, int newx_pos, int newy_pos)
  {
    // Calculate old cell position
    int old_x=CalculateCellXPosition(sprite->midx);
    int old_y=CalculateCellYPosition(sprite->midy);
    // Calculate new sprite's middle position
    int new_midx=newx_pos+sprite->w/2;
    int new_midy=newy_pos+sprite->h/2;
    // Calculate new cell position (after move)
    int new_x=CalculateCellXPosition(new_midx);
    int new_y=CalculateCellYPosition(new_midy);
    // Check if the sprite moved to a different cell
    if (old_x!=new_x || old_y!=new_y)
    {
      GetCell(old_x,old_y).Remove(sprite);
      GetCell(new_x,new_y).Add(sprite);
    }
    sprite->x=newx_pos;
    sprite->y=newy_pos;
    sprite->midx=new_midx;
    sprite->midy=new_midy;
  }

  // Go over all the grid and check for collisions
  void CheckForCollisions()
  {
    for(int y=0;y<m_CellsNumberY;y++)
    {
      for(int x=0;x<m_CellsNumberX;x++)
      {
        GridCell& cur=GetCell(x,y);
        // Check if there is any sprite in this cell.
        if (!cur.IsPopulated()) continue;
        // If there is more than one, check all sprites in this cell
        if (cur.MultipleSprites()) cur.CheckSelf();
        // Right and bottom edge of grid have no appropriate neighbors
        if (x<m_CellsNumberX-1 && y<m_CellsNumberY-1)
        {
          // Check all four neighbors.  Three if y=0
          if (y>0)
            if (GetCell(x+1,y-1).IsPopulated()) cur.Check(GetCell(x+1,y-1));
          if (GetCell(x+1,y  ).IsPopulated()) cur.Check(GetCell(x+1,y  ));
          if (GetCell(x+1,y+1).IsPopulated()) cur.Check(GetCell(x+1,y+1));
          if (GetCell(x  ,y+1).IsPopulated()) cur.Check(GetCell(x  ,y+1));
        }
      }
    }
  }

private:
  // Retrieve a cell according to its position in the grid
  GridCell& GetCell(int x, int y) { return m_Grid[y*m_CellsNumberX+x]; }

  int CalculateCellXPosition(int pixel_position)
  {
    int x=(pixel_position - m_Left)/m_CellWidth;
    // Check for bounds.  Grid should be designed so that sprites
    // never move out of its area.
    if (x<0 || x>=m_CellsNumberX) throw;
    return x;
  }

  int CalculateCellYPosition(int pixel_position)
  {
    int y=(pixel_position - m_Top)/m_CellHeight;
    // Check for bounds.  Grid should be designed so that sprites
    // never move out of its area.
    if (y<0 || y>=m_CellsNumberY) throw;
    return y;
  }

  int       m_Left;
  int       m_Top;
  int       m_Right;
  int       m_Bottom;
  int       m_CellWidth;
  int       m_CellHeight;
  int       m_CellsNumberX;
  int       m_CellsNumberY;
  GridCell* m_Grid;
};


// Test example
int main(int argc, char* argv[])
{
  SpriteVector objects;
  objects.push_back(SpriteRect(20,6,3,2));
  objects.push_back(SpriteRect(7,9,3,2));
  objects.push_back(SpriteRect(19,1,3,2));
  objects.push_back(SpriteRect(2,4,3,2));
  objects.push_back(SpriteRect(18,7,3,2));
  objects.push_back(SpriteRect(12,2,3,2));
  Grid g(3,2,0,0,25,15);
  for(int i=0;i<objects.size();i++)
    g.Add(&objects[i]);
  for(int j=0;j<11;j++)
  {
    std::cout << "---------------------------------------\n";
    g.CheckForCollisions();
    SpriteRect& s=objects[2];
    g.Move(&s,s.x,s.y+1);
  }
  return 0;
}