/**
  Raycasting demo 3 for Pokitto.

  Minecraft-like demo, shows an interactive terrain editing and raycasting
  without textures.

  Don't forget to compile with -O3!

  author: Miloslav "drummyfish" Ciz
  license: CC0 1.0
*/

// redefine player's height
#define PLAYER_SPEED (UNITS_PER_SQUARE * 6)
#define HORIZONTAL_FOV ((3 * UNITS_PER_SQUARE) / 10)
#define GRAVITY_ACCELERATION (UNITS_PER_SQUARE * 2)
#define CAMERA_COLL_HEIGHT_BELOW ((3 * UNITS_PER_SQUARE) / 2)
#define FPS 40
#define HEAD_BOB_HEIGHT 150
#define HEAD_BOB_STEP 20

#include "general.hpp"

#define LEVEL_X_RES 29
#define LEVEL_Y_RES 21

Player player;

#define SPRITE_MAX_DISTANCE 5 * UNITS_PER_SQUARE

#define JUMP_SPEED 500

char floorColor = 0;
Vector2D selectedSquare; ///< Coords of a square selected for editing.

/**
  Represents one terrain change against the implicit terrain.
*/
class Change
{
public:
  Vector2D mCoords;
  Unit mHeight;
  int8_t mColor;
};

#define MAX_CHANGES 16 ///< Maximum number of simultaneous terrain changes.

Change changes[MAX_CHANGES]; ///< Terrain change list.

// helpers
bool editReleased = false;
bool editing = false;
uint16_t changeIndex = 0;
int16_t editCounter = 0;

#define SQUARE_COLORS 4
uint8_t squareColors[SQUARE_COLORS];

#define HEIGHT_PROFILE_LENGTH 256

/// Used for terrain generation.
const int8_t heightProfile[] = {
9,9,9,10,10,10,11,11,12,13,13,14,14,14,15,15,15,16,16,16,16,16,15,15,15,14,14,
13,13,12,11,10,10,9,9,9,8,8,8,8,8,8,7,7,7,6,5,4,4,3,3,3,3,3,3,4,5,7,8,10,11,12,
12,13,13,13,14,16,19,20,21,21,22,22,22,23,23,23,23,22,22,21,20,19,18,18,17,17,
17,17,16,16,16,16,16,16,16,15,15,15,14,14,13,12,11,10,8,7,6,5,5,5,4,4,4,4,4,5,
5,5,6,6,7,7,7,8,8,8,8,8,8,8,7,7,7,7,6,6,5,4,4,3,2,2,2,1,1,1,1,2,3,5,6,7,9,13,
13,13,18,18,22,22,22,22,22,22,21,21,20,20,19,16,16,16,14,14,14,15,15,15,15,15,
16,17,17,18,18,18,16,16,16,16,15,15,15,14,14,13,12,8,7,7,7,8,9,11,12,12,13,13,
13,12,11,10,10,10,10,11,11,12,13,13,12,12,11,11,10,10,9,9,9,9,9,9,10,10,11,12,
13,13,14,14,14,14,14,13,13,12,11,11,11,11,10,10,10,10};

const unsigned char imageBackground[] =
{  55, 44 // width, height
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xff
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xff,0xd2,0xd2,0xd2,0xd2,0xd2,0x69
  ,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xff,0xff,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0x69,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xbc,0xbc,0xdb
  ,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb
  ,0xdb,0xdb,0xff,0xff,0xff,0xff,0xbc,0xbc,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xff
  ,0xff,0xbc,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0x3b,0x3b
  ,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xff,0xff,0xbc,0xdb,0xdb,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xff,0xff
  ,0xff,0xff,0xff,0xbc,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b
  ,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xbc,0xbc,0xdb,0xdb
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xff,0xff,0xbc,0xbc,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xbc,0xdb
  ,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0x3b,0xdb,0xdb
  ,0xdb,0xdb,0xff,0xff,0xff,0xff,0xff,0xbc,0xdb,0xdb,0xbc,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xbc
  ,0xff,0xbc,0xdb,0xdb,0xff,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xbc,0xff,0xff,0xbc,0xbc,0xff,0xd2
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xff
  ,0xbc,0xbc,0xff,0xff,0xbc,0xff,0xff,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xbc,0xdb,0xff,0xff,0xff,0xff
  ,0xbc,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xbc,0xbc,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0x69,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xff
  ,0xbc,0xbc,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xbc,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xff,0xbc,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xbc,0xd2
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xbc,0xbc,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0x69,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xbc
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0xbc,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0x3b,0x3b,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0xbc,0xbc
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0xff,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b
  ,0xff,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0xff,0xff,0xbc,0xdb,0xbc,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b
  ,0x3b,0x3b,0xff,0xff,0xbc,0xdb,0xff,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0x3b,0xff,0xbc,0xbc,0xdb
  ,0xff,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0x3b,0x3b,0x3b,0x3b,0xff,0xbc,0xbc,0xdb,0xff,0xbc,0xbc,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xff,0xbc
  ,0xbc,0xbc,0xff,0xff,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xbc,0xbc,0xff,0xbc,0xff,0xff,0xbc,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b
  ,0xbc,0xdb,0xff,0xff,0xff,0xff,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0x3b,0xbc,0xdb,0xff,0xff,0xff,0xbc
  ,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b
  ,0x3b,0x3b,0xbc,0xdb,0xff,0xff,0xdb,0xbc,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0xbc,0xdb,0xff,0xbc
  ,0xdb,0xbc,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0x69,0x3b,0x3b,0xdb,0xdb,0xff,0xdb,0xdb,0xdb,0xbc,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xbc,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b
  ,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2
  ,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xbc,0xd2,0xd2,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xbc,0xbc,0xd2,0xd2,0xd2
  ,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xff,0xff,0xbc,0xbc,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xbc
  ,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xbc,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b
  ,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xff
  ,0xff,0xff,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xff,0xd2,0xd2,0xd2,0xd2
  ,0xd2,0x3b,0x3b,0x3b,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb
  ,0xdb,0xdb,0xff,0xff,0xd2,0xd2,0xd2,0xd2,0xd2,0x69,0x3b,0x3b,0xdb,0xdb
  ,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xdb,0xff,0xff,0xd2,0xd2
  ,0xd2,0xd2,0xd2,0xd2,0x3b,0x3b
};

Unit floorHeightAt(int16_t x, int16_t y)
{
  /*
    This for loop may become a bottleneck, since this function is called
    very often - if more changes are to be kept, optimizations are needed -
    probably a hash table, sorting by coordinates etc.
  */
  for (uint16_t i = 0; i < MAX_CHANGES; ++i)
    if (changes[i].mCoords.x == x && changes[i].mCoords.y == y)
      return changes[i].mHeight;

  return (heightProfile[absVal(x) % HEIGHT_PROFILE_LENGTH] +
         heightProfile[absVal(y + 20) % HEIGHT_PROFILE_LENGTH]) *
         UNITS_PER_SQUARE;
}

Unit colorAt(int16_t x, int16_t y)
{
  for (uint16_t i = 0; i < MAX_CHANGES; ++i)
    if (changes[i].mCoords.x == x && changes[i].mCoords.y == y)
      return changes[i].mColor;

  return min((heightProfile[absVal(x * 2) % HEIGHT_PROFILE_LENGTH] +
         heightProfile[absVal(y) % HEIGHT_PROFILE_LENGTH]) / 10,3);
}

uint16_t previousColumn = 255; ///< Helper for precomputing background.
uint16_t backgroundColumn = 0; ///< Precomputed background column.

/**
  Function for drawing a single pixel (like fragment shader).
*/
inline void pixelFunc(PixelInfo *pixel)
{
  uint8_t c = 0;
  int16_t intensity = 0;

  if (pixel->isWall)
  {
    c = pixel->hit.square.x != selectedSquare.x || pixel->hit.square.y != selectedSquare.y || (editing && pokitto.frameCount % 2) == 0 ?
        squareColors[pixel->hit.type] : 30;
    
    intensity = pixel->depth / (UNITS_PER_SQUARE * 3);
    intensity += pixel->hit.direction % 2 == 0 ? 2 : 0;
  }
  else if (pixel->isFloor)
  {
    c = floorColor;

    if (!pixel->isHorizon)
      intensity = pixel->depth / (UNITS_PER_SQUARE * 3);
  }
  else
  {
    if (previousColumn == pixel->position.x)
    {
      c = imageBackground[2 + backgroundColumn * 22 + min(pixel->position.y,43) / 2];
    }
    else
    {
      backgroundColumn = absVal(pixel->position.x / 2 + (110 * player.mCamera.direction) / UNITS_PER_SQUARE) % 55;
      previousColumn = pixel->position.x;
    }
  }

  if (intensity != 0)
    c = addIntensity(c,intensity);

  putSubsampledPixel // macro
}

bool flyBy = true;

void draw()
{
  RayConstraints c;

  c.maxHits = 6;
  c.maxSteps = 20;
  c.computeTextureCoords = 0;

  player.mCamera.height += player.mHeadBob;
  render(player.mCamera,floorHeightAt,0,colorAt,pixelFunc,c);
  player.mCamera.height -= player.mHeadBob;

  if (flyBy && (pokitto.frameCount >> 3) % 3 != 0)
  {
    pokitto.display.setCursor(25,5);
    pokitto.display.setColor(rgbToIndex(1,0,0));
    pokitto.display.invisiblecolor = 0;
    pokitto.display.print("press any key");
  }
}

void cameraFlyBy(uint32_t dt)
{
  Unit height = floorHeightAt(
    divRoundDown(player.mCamera.position.x,UNITS_PER_SQUARE),
    divRoundDown(player.mCamera.position.y,UNITS_PER_SQUARE)) + UNITS_PER_SQUARE * 3;

  Unit heightDiff = player.mCamera.height - height;

  Unit step = (200 * dt) / 1000;

  if (heightDiff > UNITS_PER_SQUARE * 2)
  {
    player.mCamera.height -= step * heightDiff / 200;
    player.mCamera.shear = max(player.mCamera.shear - 1,-80);
  }
  else
  {
    if (heightDiff < UNITS_PER_SQUARE)
      player.mCamera.height += step * absVal(heightDiff / 70);

    if (player.mCamera.shear < 0 && pokitto.frameCount % 2 == 0)
      player.mCamera.shear++;
  }

  player.mCamera.position.x += step * 30;
  player.mCamera.position.y += step * 15;
  player.mCamera.direction = sinInt(pokitto.frameCount / 8);
}

int main()
{
  initGeneral();

  floorColor = rgbToIndex(4,2,0);

  squareColors[0] = rgbToIndex(0,0,3);
  squareColors[1] = rgbToIndex(7,0,0);
  squareColors[2] = rgbToIndex(0,7,0);
  squareColors[3] = rgbToIndex(4,4,0);

  player.setPositionSquare(4,5);
  player.mCamera.height = floorHeightAt(4,5);

  // init changes

  for (uint16_t i = 0; i < MAX_CHANGES; ++i)
  {
    changes[i].mCoords.x = (i + 1) * 1000;
    changes[i].mCoords.y = 0;
    changes[i].mHeight = floorHeightAt(i,0);
  }

  uint32_t previousTime = 0;
  uint32_t dt;

  int16_t moveDirection;
  int16_t shearDirection;
  int16_t rotationDirection;
  bool strafe;

  while (pokitto.isRunning())
  {
    if (pokitto.update())
    {
      draw();

      moveDirection = 0;
      shearDirection = 0;
      rotationDirection = 0;
      strafe = false;

      uint32_t timeNow = pokitto.getTime();
      dt = timeNow - previousTime;
      previousTime = timeNow;

      if (flyBy)
      {
        cameraFlyBy(dt);

        if (pokitto.aBtn() || pokitto.bBtn() || pokitto.cBtn() ||
            pokitto.upBtn() || pokitto.downBtn() || pokitto.leftBtn() ||
            pokitto.rightBtn())
          flyBy = false;

        continue;
      }

      strafe = pokitto.aBtn();

      if (pokitto.cBtn())
      {
        if (editReleased)
        {
          editing = !editing;

          if (editing)
          {
            Vector2D facingOffset;
            facingOffset.x = 0;
            facingOffset.y = 0;

            if (player.mCamera.direction > (4 * UNITS_PER_SQUARE / 12) &&
              player.mCamera.direction <= (8 * UNITS_PER_SQUARE / 12))
              facingOffset.x = -1;
            else if (player.mCamera.direction < (2 * UNITS_PER_SQUARE / 12) ||
              player.mCamera.direction >= (10 * UNITS_PER_SQUARE / 12))
              facingOffset.x = 1;
            else
              facingOffset.x = 0;

            if (player.mCamera.direction > (UNITS_PER_SQUARE / 12) &&
              player.mCamera.direction <= (5 * UNITS_PER_SQUARE / 12))
              facingOffset.y = -1;
            else if (player.mCamera.direction > (6 * UNITS_PER_SQUARE / 12) &&
              player.mCamera.direction <= (11 * UNITS_PER_SQUARE / 12))
              facingOffset.y = 1;
            else
              facingOffset.y = 0;

            selectedSquare.x = divRoundDown(player.mCamera.position.x,UNITS_PER_SQUARE) + facingOffset.x;
            selectedSquare.y = divRoundDown(player.mCamera.position.y,UNITS_PER_SQUARE) + facingOffset.y;

            changeIndex = (changeIndex + 1) % MAX_CHANGES;

            for (uint16_t i = 0; i < MAX_CHANGES; ++i)
              if (changes[i].mCoords.x == selectedSquare.x &&
                  changes[i].mCoords.y == selectedSquare.y)
              {
                changeIndex = i;
                break;
              }

            changes[changeIndex].mHeight = floorHeightAt(selectedSquare.x,selectedSquare.y);
            changes[changeIndex].mColor = colorAt(selectedSquare.x,selectedSquare.y);
            changes[changeIndex].mCoords.x = selectedSquare.x;
            changes[changeIndex].mCoords.y = selectedSquare.y;
            editCounter = 0;
          }
        
          editReleased = false;
        }
      }
      else
        editReleased = true;

      if (pokitto.upBtn())
      {
        if (editing)
        {
          if (editCounter == 0)
          {
            changes[changeIndex].mHeight += UNITS_PER_SQUARE / 4;
            editCounter = 4;
          }
        }
        else if (strafe)
          shearDirection = 1;
        else
          moveDirection = 1;
      }
      else if (pokitto.downBtn())
      {
        if (editing)
        {
          if (editCounter == 0)
          {
            changes[changeIndex].mHeight -= UNITS_PER_SQUARE / 4;
            editCounter = 4;
          }
        }
        else if (strafe)
          shearDirection = -1;
        else
          moveDirection = -1;
      }
 
      uint16_t colorAddition = 0;

      if (pokitto.rightBtn())
      {
        if (strafe)
          moveDirection = 1;
        else if (!editing)
          rotationDirection = 1;
        else
          colorAddition = 1;      
      }
      else if (pokitto.leftBtn())
      {
        if (strafe)
          moveDirection = - 1;
        else if (!editing)
          rotationDirection = -1;
        else
          colorAddition = -1;      
      }

      if (editing)
      {
        int16_t heightDiff = changes[changeIndex].mHeight - player.mCamera.height;

        if (heightDiff > UNITS_PER_SQUARE / 2)
          shearDirection = 1; 
        else if (heightDiff < -1 * UNITS_PER_SQUARE / 2)
          shearDirection = -1; 

        if (editCounter == 0 && colorAddition != 0)
        {
          changes[changeIndex].mColor = wrap(changes[changeIndex].mColor + colorAddition,SQUARE_COLORS);
          editCounter = 4;
        }
      }

      editCounter = max(0, editCounter - 1);

      player.update(moveDirection,strafe,rotationDirection,pokitto.bBtn(),shearDirection,
        floorHeightAt,0,true,dt);
    }
  }

  return 0;
}