diagram.json

#ifndef _constants_h
#define _constants_h

// Key pinout
#define USE_INPUT_PULLUP
#define K_LEFT              6
#define K_RIGHT             7
#define K_UP                8
#define K_DOWN              3
#define K_FIRE              10

// SNES Controller
// uncomment following line to enable snes controller support
// #define SNES_CONTROLLER
constexpr uint8_t DATA_CLOCK   = 11;
constexpr uint8_t DATA_LATCH   = 12;
constexpr uint8_t DATA_SERIAL  = 13;

// Sound
constexpr uint8_t SOUND_PIN   = 9; // do not change, belongs to used timer

// GFX settings
#define OPTIMIZE_SSD1306                // Optimizations for SSD1366 displays

#define FRAME_TIME          66.666666   // Desired time per frame in ms (66.666666 is ~15 fps)
#define RES_DIVIDER         2           // Higher values will result in lower horizontal resolution when rasterize and lower process and memory usage
                                        // Lower will require more process and memory, but looks nicer
#define Z_RES_DIVIDER       2           // Zbuffer resolution divider. We sacrifice resolution to save memory
#define DISTANCE_MULTIPLIER 20          // Distances are stored as uint8_t, multiplying the distance we can obtain more precision taking care
                                        // of keep numbers inside the type range. Max is 256 / MAX_RENDER_DEPTH
#define MAX_RENDER_DEPTH    12
#define MAX_SPRITE_DEPTH    8

#define ZBUFFER_SIZE        SCREEN_WIDTH / Z_RES_DIVIDER + 4

// Level 
#define LEVEL_WIDTH_BASE    6
#define LEVEL_WIDTH         (1 << LEVEL_WIDTH_BASE)
#define LEVEL_HEIGHT        57
#define LEVEL_SIZE          LEVEL_WIDTH / 2 * LEVEL_HEIGHT

// scenes
#define INTRO                 0
#define GAME_PLAY             1
#define DIFF                  2
#define MUS                   3
#define MID                   4
#define SCORE                 5
// Game
#define GUN_TARGET_POS        18
#define GUN_SHOT_POS          GUN_TARGET_POS + 8

#define ROT_SPEED             .12
#define MOV_SPEED             .1
#define MOV_SPEED_INV         2.5           // 1 / MOV_SPEED

#define JOGGING_SPEED         .005
#define ENEMY_SPEED           .04
#define FIREBALL_SPEED        .2
#define FIREBALL_ANGLES       45          // Num of angles per PI

#define MAX_ENTITIES          12           // Max num of active entities
#define MAX_STATIC_ENTITIES   24          // Max num of entities in sleep mode

#define MAX_ENTITY_DISTANCE   200         // * DISTANCE_MULTIPLIER
#define MAX_ENEMY_VIEW        90          // * DISTANCE_MULTIPLIER
#define ITEM_COLLIDER_DIST    6           // * DISTANCE_MULTIPLIER
#define ENEMY_COLLIDER_DIST   4           // * DISTANCE_MULTIPLIER
#define FIREBALL_COLLIDER_DIST 3          // * DISTANCE_MULTIPLIER
#define ENEMY_MELEE_DIST      9           // * DISTANCE_MULTIPLIER
#define WALL_COLLIDER_DIST    .2

#define ENEMY_MELEE_DAMAGE    18
#define ENEMY_FIREBALL_DAMAGE 25
#define GUN_MAX_DAMAGE        80

// display
constexpr uint8_t SCREEN_WIDTH     =  128;
constexpr uint8_t SCREEN_HEIGHT    =  64;
constexpr uint8_t HALF_WIDTH       =  SCREEN_WIDTH/2;
constexpr uint8_t RENDER_HEIGHT    =  56;         // raycaster working height (the rest is for the hud)
constexpr uint8_t HALF_HEIGHT      =  SCREEN_HEIGHT/2;

#endif


constants.h

/* 
todo: Moving this to CPP looks like it takes more Flash storage. Figure out why.
*/
#include "SSD1306.h"
#include "constants.h"

// Reads a char from an F() string
#define F_char(ifsh, ch)    pgm_read_byte(reinterpret_cast<PGM_P>(ifsh) + ch)

// This is slightly faster than bitRead (also bits are read from left to right)
const static uint8_t PROGMEM bit_mask[8] = { 128, 64, 32, 16, 8, 4, 2, 1 };
#define read_bit(b, n)      b & pgm_read_byte(bit_mask + n) ? 1 : 0

void setupDisplay();
void fps();
bool getGradientPixel(uint8_t x, uint8_t y, uint8_t i);
void fadeScreen(uint8_t intensity, bool color);
void drawByte(uint8_t x, uint8_t y, uint8_t b);
uint8_t getByte(uint8_t x, uint8_t y);
void drawPixel(int8_t x, int8_t y, bool color, bool raycasterViewport);
void drawVLine(uint8_t x, int8_t start_y, int8_t end_y, uint8_t intensity);
void drawSprite(int8_t x, int8_t y, const uint8_t bitmap[], const uint8_t mask[], int8_t w, int8_t h, uint8_t sprite, double distance);
void drawChar(int8_t x, int8_t y, char ch);
void drawText(int8_t x, int8_t y, char *txt, uint8_t space = 1);
void drawText(int8_t x, int8_t y, const __FlashStringHelper txt, uint8_t space = 1);

// Initialize screen. Following line is for OLED 128x64 connected by I2C
Adafruit_SSD1306<SCREEN_WIDTH, SCREEN_HEIGHT> display;

// FPS control
double delta = 1;
uint32_t lastFrameTime = 0;

#ifdef OPTIMIZE_SSD1306
// Optimizations for SSD1306 handles buffer directly
uint8_t *display_buf;
#endif

// We don't handle more than MAX_RENDER_DEPTH depth, so we can safety store
// z values in a byte with 1 decimal and save some memory,
uint16_t zbuffer[ZBUFFER_SIZE];

void setupDisplay() {
  // Setup display
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Fixed from 0x3D
    
    while (1); // Don't proceed, loop forever
  }

#ifdef OPTIMIZE_SSD1306
  display_buf = display.getBuffer();
#endif

  // initialize z buffer
  memset(zbuffer, 0xFF, ZBUFFER_SIZE);
}

// Adds a delay to limit play to specified fps
// Calculates also delta to keep movement consistent in lower framerates
void fps() {
  while (millis() - lastFrameTime < FRAME_TIME);
  delta = (double)(millis() - lastFrameTime) / FRAME_TIME;
  lastFrameTime = millis();
}

double getActualFps() {
  return 1000 / (FRAME_TIME * delta);
}

// Faster way to render vertical bits
void drawByte(uint8_t x, uint8_t y, uint8_t b) {
#ifdef OPTIMIZE_SSD1306
  display_buf[(y / 8)*SCREEN_WIDTH + x] = b;
#endif
}

boolean getGradientPixel(uint8_t x, uint8_t y, uint8_t i) {
  if (i == 0) return 0;
  if (i >= GRADIENT_COUNT - 1) return 1;

  uint8_t index = max(0, min(GRADIENT_COUNT - 1, i)) * GRADIENT_WIDTH * GRADIENT_HEIGHT // gradient index
                  + y * GRADIENT_WIDTH % (GRADIENT_WIDTH * GRADIENT_HEIGHT)             // y byte offset
                  + x / GRADIENT_HEIGHT % GRADIENT_WIDTH;                               // x byte offset

  // return the bit based on x
  return read_bit(pgm_read_byte(gradient + index), x % 8);
}

void fadeScreen(uint8_t intensity, bool color = 0) {
  for (uint8_t x = 0; x < SCREEN_WIDTH; x++) {
    for (uint8_t y = 0; y < SCREEN_HEIGHT; y++) {
      if (getGradientPixel(x, y, intensity)) 
        drawPixel(x, y, color, false);
    }
  }
}

// Faster drawPixel than display.drawPixel.
// Avoids some checks to make it faster.
void drawPixel(int8_t x, int8_t y, bool color, bool raycasterViewport = false) {
  // prevent write out of screen buffer
  if (x < 0 || x >= SCREEN_WIDTH || y < 0 || y >= (raycasterViewport ? RENDER_HEIGHT : SCREEN_HEIGHT)) {
    return;
  }

#ifdef OPTIMIZE_SSD1306
  if (color) {
    // white
    display_buf[x + (y / 8)*SCREEN_WIDTH] |= (1 << (y & 7));
  } else {
    // black
    display_buf[x + (y / 8)*SCREEN_WIDTH] &= ~(1 << (y & 7));
  }
#else
  display.drawPixel(x, y, color);
#endif
}

// For raycaster only
// Custom draw Vertical lines that fills with a pattern to simulate
// different brightness. Affected by RES_DIVIDER
void drawVLine(uint8_t x, int8_t start_y, int8_t end_y, uint8_t intensity) {
  int8_t y;
  int8_t lower_y = max(min(start_y, end_y), 0);
  int8_t higher_y = min(max(start_y, end_y), RENDER_HEIGHT - 1);
  uint8_t c;

#ifdef OPTIMIZE_SSD1306
  uint8_t bp;
  uint8_t b;
  for (c = 0; c < RES_DIVIDER; c++) {
    y = lower_y;
    b = 0;
    while (y <= higher_y) {
      bp = y % 8;
      b = b | getGradientPixel(x + c, y, intensity) << bp;

      if (bp == 7) {
        // write the whole byte
        drawByte(x + c, y, b);
        b = 0;
      }

      y++;
    }

    // draw last byte
    if (bp != 7) {
      drawByte(x + c, y - 1, b);
    }
  }
#else
  y = lower_y;
  while (y <= higher_y) {
    for (c = 0; c < RES_DIVIDER; c++) {
      // bypass black pixels
      if (getGradientPixel(x + c, y, intensity)) {
        drawPixel(x + c, y, 1, true);
      }
    }

    y++;
  }
#endif
}

// Custom drawBitmap method with scale support, mask, zindex and pattern filling
void drawSprite(
  int8_t x, int8_t y,
  const uint8_t bitmap[], const uint8_t mask[],
  int16_t w, int16_t h,
  uint8_t sprite,
  double distance
) {
  uint8_t tw = (double) w / distance;
  uint8_t th = (double) h / distance;
  uint8_t byte_width = w / 8;
  uint8_t pixel_size = max(1, 1.0 / distance);
  uint16_t sprite_offset = byte_width * h * sprite;

  bool pixel;
  bool maskPixel;

  // Don't draw the whole sprite if the anchor is hidden by z buffer
  // Not checked per pixel for performance reasons
  if (zbuffer[min(max(x, 0), ZBUFFER_SIZE - 1) / Z_RES_DIVIDER] < distance * DISTANCE_MULTIPLIER) {
    return;
  }

  for (uint8_t ty = 0; ty < th; ty += pixel_size) {
    // Don't draw out of screen
    if (y + ty < 0 || y + ty >= RENDER_HEIGHT) {
      continue;
    }

    uint8_t sy = ty * distance; // The y from the sprite

    for (uint8_t tx = 0; tx < tw; tx += pixel_size) {
      uint8_t sx = tx * distance; // The x from the sprite
      uint16_t byte_offset = sprite_offset + sy * byte_width + sx / 8;

      // Don't draw out of screen
      if (x + tx < 0 || x + tx >= SCREEN_WIDTH) {
        continue;
      }

      maskPixel = read_bit(pgm_read_byte(mask + byte_offset), sx % 8);

      if (maskPixel) {
        pixel = read_bit(pgm_read_byte(bitmap + byte_offset), sx % 8);
        for (uint8_t ox = 0; ox < pixel_size; ox++) {
          for (uint8_t oy = 0; oy < pixel_size; oy++) {
            drawPixel(x + tx + ox, y + ty + oy, pixel, true);
          }
        }
      }
    }
  }
}

// Draw a single character.
// Made for a custom font with some useful sprites. Char size 4 x 6
// Uses less memory than display.print()
void drawChar(int8_t x, int8_t y, char ch) {
  uint8_t c = 0;
  uint8_t n;
  uint8_t bOffset;
  uint8_t b;
  uint8_t line = 0;

  // Find the character
  while (CHAR_MAP[c] != ch && CHAR_MAP[c] != '\0') c++;

  bOffset = c / 2;
  for (; line < CHAR_HEIGHT; line++) {
    b = pgm_read_byte(bmp_font + (line * bmp_font_width + bOffset));
    for (n = 0; n < CHAR_WIDTH; n++)
      if (read_bit(b, (c % 2 == 0 ? 0 : 4) + n))
        drawPixel(x + n, y + line, 1, false);
  }
}

// Draw a string
void drawText(int8_t x, int8_t y, char *txt, uint8_t space) {
  uint8_t pos = x;
  uint8_t i = 0;
  char ch;
  while ((ch = txt[i]) != '\0') {
    drawChar(pos, y, ch);
    i++;
    pos += CHAR_WIDTH + space;

    // shortcut on end of screen
    if (pos > SCREEN_WIDTH) return;
  }
}

// Draw a F() string
void drawText(int8_t x, int8_t y, const __FlashStringHelper *txt_p, uint8_t space = 1) {
  uint8_t pos = x;
  uint8_t i = 0;
  char ch;
  while (pos < SCREEN_WIDTH && (ch = F_char(txt_p, i)) != '\0') {
    drawChar(pos, y, ch);
    i++;
    pos += CHAR_WIDTH + space;
  }
}

// Draw an integer (3 digit max!)
void drawText(uint8_t x, uint8_t y, int16_t num) {
  char buf[4]; // 3 char + \0
  itoa(num, buf, 10);
  drawText(x, y, buf);
}


display.h

doom-nano-brutality.ino

#include <stdint.h>
#include "entities.h"
#include "types.h"
#include "constants.h"

Entity create_entity(uint8_t type, uint8_t x,  uint8_t y, uint8_t initialState, uint8_t initialHealth) {
  UID uid = create_uid(type, x, y);
  Coords pos = create_coords((double) x + .5, (double) y + .5);
  Entity new_entity = { uid, pos, initialState, initialHealth, 0, 0, 0,};
  return new_entity;
}

StaticEntity crate_static_entity(UID uid, uint8_t x,  uint8_t y, bool active) {
  return { uid, x, y, active };
}

entities.cpp

#ifndef _entities_h
#define _entities_h

#include "types.h"

// Shortcuts
#define create_player(x, y)   { \
    create_coords((double) x + 0.5, (double) y + 0.5), \
    create_coords(1, 0), \
    create_coords(0, -0.66), \
    0, \
    100,  \
    10, \
    0, \
    0, \
    0, \
    0, \
    false, \
  }

#define create_enemy(x, y)            create_entity(E_ENEMY, x, y, S_STAND, 100)
#define create_medikit(x, y)          create_entity(E_MEDIKIT, x, y, S_STAND, 0)
#define create_key(x, y)              create_entity(E_KEY, x, y, S_STAND, 0)
#define create_fireball(x, y, dir)    create_entity(E_FIREBALL, x, y, S_STAND, dir)

// entity statuses
#define S_STAND               0
#define S_ALERT               1
#define S_FIRING              2
#define S_MELEE               3
#define S_HIT                 4
#define S_DEAD                5
#define S_HIDDEN              6


struct Player { 
  Coords pos;
  Coords dir;
  Coords plane;
  double velocity;
  uint8_t health;
  uint8_t keys;
  uint8_t secret;
  uint8_t secret2;
  uint8_t secret3;
  int16_t score;
  bool cheats;
};

struct Entity {
  UID uid;
  Coords pos;
  uint8_t state;
  uint8_t health;     // angle for fireballs
  uint8_t distance;
  uint8_t timer;
  bool a;
  bool b;
};

struct StaticEntity  { 
  UID uid;
  uint8_t x;
  uint8_t y;
  bool active;
};

Entity create_entity(uint8_t type, uint8_t x,  uint8_t y, uint8_t initialState, uint8_t initialHealth);
StaticEntity create_static_entity(UID uid, uint8_t x,  uint8_t y, bool active);

#endif



entities.h

#include <Arduino.h>
#include "input.h"
#include "constants.h"

#ifdef USE_INPUT_PULLUP
  #define INPUT_MODE INPUT_PULLUP
  #define INPUT_STATE LOW
#else
  #define INPUT_MODE INPUT
  #define INPUT_STATE HIGH
#endif

#ifdef SNES_CONTROLLER
uint16_t buttons = 0;

void input_setup() {
  // Set DATA_CLOCK normally HIGH
  pinMode(DATA_CLOCK, OUTPUT);
  digitalWrite(DATA_CLOCK, HIGH);

  // Set DATA_LATCH normally LOW
  pinMode(DATA_LATCH, OUTPUT);
  digitalWrite(DATA_LATCH, LOW);

  // Set DATA_SERIAL normally HIGH
  pinMode(DATA_SERIAL, OUTPUT);
  digitalWrite(DATA_SERIAL, HIGH);
  pinMode(DATA_SERIAL, INPUT);
}

void getControllerData(void){
  // Latch for 12us
  digitalWrite(DATA_LATCH, HIGH);
  delayMicroseconds(12);
  digitalWrite(DATA_LATCH, LOW);
  delayMicroseconds(6);
  buttons = 0;
  // Retrieve button presses from shift register by pulling the clock high for 6us
  for(uint8_t i = 0; i < 16; ++i){
    digitalWrite(DATA_CLOCK, LOW);
    delayMicroseconds(6);
    buttons |= !digitalRead(DATA_SERIAL) << i;
    digitalWrite(DATA_CLOCK, HIGH);
    delayMicroseconds(6);
  }
}

bool input_left() {
  return buttons & LEFT;
};

bool input_right() {
  return buttons & RIGHT;
};

bool input_up() {
  return buttons & UP;
};

bool input_down() {
  return buttons & DOWN;
};

bool input_fire() {
  return buttons & Y;
};

bool input_start() {
  return buttons & START;
}
#else

void input_setup() {
  pinMode(K_LEFT, INPUT_MODE);
  pinMode(K_RIGHT, INPUT_MODE);
  pinMode(K_UP, INPUT_MODE);
  pinMode(K_DOWN, INPUT_MODE);
  pinMode(K_FIRE, INPUT_MODE);
}

bool input_left() {
  return digitalRead(K_LEFT) == INPUT_STATE;
};

bool input_right() {
  return digitalRead(K_RIGHT) == INPUT_STATE;
};

bool input_up() {
  return digitalRead(K_UP) == INPUT_STATE;
};

bool input_down() {
  return digitalRead(K_DOWN) == INPUT_STATE;
};

bool input_fire() {
  return digitalRead(K_FIRE) == INPUT_STATE;
};
#endif


input.cpp

#ifndef _input_h
#define _input_h

enum BUTTONS {
  B = 0x0001,
  Y = 0x0002,
  SELECT = 0x0004,
  START = 0x0008,
  UP = 0x0010,
  DOWN = 0x0020,
  LEFT = 0x0040,
  RIGHT = 0x0080,
  A = 0x0100,
  X = 0x0200,
  LB = 0x0400,
  RB = 0x0800
};

void input_setup();
bool input_up();
bool input_down();
bool input_left();
bool input_right();
bool input_fire();

#ifdef SNES_CONTROLLER
bool input_start();
void getControllerData(void);
#endif

#endif


input.h

level.h

/*
   For infos how this works see 
   http://fabiensanglard.net/gebbwolf3d_v2.1.pdf chapter 4.9.5
   and
   http://www.shikadi.net/moddingwiki/Inverse_Frequency_Sound_format
   and
   http://www.shikadi.net/moddingwiki/AudioT_Format
*/

//constexpr uint8_t MUS_S*_SND_LEN = 30;
//constexpr uint8_t mus_s*_snd[] PROGMEM = { };

//constexpr uint8_t MUS_P*_SND_LEN = 15;
//constexpr uint8_t mus_p*_snd[] PROGMEM = { };

#ifndef _sound_h
#define _sound_h

#include <avr/pgmspace.h>
#include "constants.h"

constexpr uint8_t MUS_S1_SND_LEN = 38;
constexpr uint8_t mus_s1_snd[] PROGMEM = {0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x00, 0x00, 0x00, 0x00, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x00, 0x00, 0x00, 0x00,};


constexpr uint8_t MUS_P1_SND_LEN = 19;
constexpr uint8_t mus_p1_snd[] PROGMEM = {0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x00, 0x00, 0x00, 0x00, };

//S1

constexpr uint8_t MUS_P2_SND_LEN = 19;
constexpr uint8_t mus_p2_snd[] PROGMEM = {0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x00, 0x00, 0x00, 0x00, };

//S1

constexpr uint8_t MUS_P3_SND_LEN = 19;
constexpr uint8_t mus_p3_snd[] PROGMEM = {0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x43, 0x00, 0x00, 0x00, 0x00, };

//S1

constexpr uint8_t MUS_P4_SND_LEN = 19;
constexpr uint8_t mus_p4_snd[] PROGMEM = {0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x00, 0x00, 0x00, 0x00, };

//S1

constexpr uint8_t MUS_P5_SND_LEN = 19;
constexpr uint8_t mus_p5_snd[] PROGMEM = {0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x00, 0x00, 0x00, 0x00, };

constexpr uint8_t MUS_P6_SND_LEN = 19;
constexpr uint8_t mus_p6_snd[] PROGMEM = {0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x00, 0x00, 0x00, 0x00, };


//S1 

//P1

//S1
 
//P2

//S1

//P3

//S1

constexpr uint8_t MUS_P7_SND_LEN = 19;
constexpr uint8_t mus_p7_snd[] PROGMEM = {0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, };

//P4




constexpr uint8_t MUS_S21_SND_LEN = 38;
constexpr uint8_t mus_s21_snd[] PROGMEM = {0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x00, 0x00, 0x00, 0x00, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x00, 0x00, 0x00, 0x00, };

constexpr uint8_t MUS_P21_SND_LEN = 19;
constexpr uint8_t mus_p21_snd[] PROGMEM = {0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x15, 0x00, 0x00, 0x00, 0x00, };

//S1

constexpr uint8_t MUS_P22_SND_LEN = 19;
constexpr uint8_t mus_p22_snd[] PROGMEM = {0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x00, 0x00, 0x00, 0x00, };
//S1

constexpr uint8_t MUS_P24_SND_LEN = 19;
constexpr uint8_t mus_p24_snd[] PROGMEM = {0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x00, 0x00, 0x00, 0x00, };

//S1


constexpr uint8_t MUS_P26_SND_LEN = 19;
constexpr uint8_t mus_p26_snd[] PROGMEM = {0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x35, 0x00, 0x00, 0x00, 0x00, };


constexpr uint8_t MUS_P27_SND_LEN = 38;
constexpr uint8_t mus_p27_snd[] PROGMEM = {0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x24, 0x24, 0x24, 0x24, 0x24, 0x24, };

constexpr uint8_t MUS_P28_SND_LEN = 38;
constexpr uint8_t mus_p28_snd[] PROGMEM = {0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x24, 0x24, 0x24, 0x24, 0x24, 0x24, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, };



constexpr uint8_t MUS_P29_SND_LEN = 38;
constexpr uint8_t mus_p29_snd[] PROGMEM = {0x17, 0x17, 0x17, 0x17, 0x17, 0x17, 0x24, 0x24, 0x24, 0x24, 0x24, 0x24, 0x24, 0x24, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, };


constexpr uint8_t MUS_EP_SND_LEN = 38;
constexpr uint8_t mus_ep_snd[] PROGMEM = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };

constexpr uint8_t MUS_EP2_SND_LEN = 19;
constexpr uint8_t mus_ep2_snd[] PROGMEM = {0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, };


constexpr uint8_t JUMP_SND_LEN = 18;
constexpr uint8_t jump_snd[] PROGMEM = {0x80, 0x80, 0x80, 0x80, 0x80, 0x55, 0x55, 0x55, 0x55, 0x55, 0x00, 0x00, 0x00, 0x00, 0x00, 0x25, 0x25, 0x25, 0x25, 0x25, };

constexpr uint8_t S_SND_LEN = 38;
constexpr uint8_t s_snd[] PROGMEM = {0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x30, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, };


constexpr uint8_t R1_SND_LEN = 9;
constexpr uint8_t r1_snd[] PROGMEM = { 0x95, 0x95, 0x95, 0x95, 0x95, 0x95};
constexpr uint8_t R2_SND_LEN = 9;
constexpr uint8_t r2_snd[] PROGMEM = { 0x50, 0x50, 0x50, 0x50, 0x50, 0x50};


constexpr uint8_t SHOT_SND_LEN = 44;
constexpr uint8_t shot_snd[] PROGMEM = { 0x10, 0x10 , 0x10 , 0x6e , 0x2a , 0x20 , 0x28 , 0x28 , 0x9b , 0x28 , 0x20 , 0x20 , 0x21 , 0x57 , 0x20 , 0x20 , 0x20 , 0x67 , 0x20 , 0x20 , 0x29 , 0x20 , 0x73 , 0x20 , 0x20 , 0x20 , 0x89,0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, };


constexpr uint8_t GET_KEY_SND_LEN = 90;
constexpr uint8_t SHOOT_SND_LEN = 27;
constexpr uint8_t shoot_snd[] PROGMEM = { 0x10, 0x10 , 0x10 , 0x6e , 0x2a , 0x20 , 0x28 , 0x28 , 0x9b , 0x28 , 0x20 , 0x20 , 0x21 , 0x57 , 0x20 , 0x20 , 0x20 , 0x67 , 0x20 , 0x20 , 0x29 , 0x20 , 0x73 , 0x20 , 0x20 , 0x20 , 0x89,};
constexpr uint8_t get_key_snd[] PROGMEM = {0x24, 0x24, 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x24 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x20 , 0x20 , 0x20 , 0x20 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x20 , 0x20 , 0x20 , 0x20 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x37 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 , 0x19 };
constexpr uint8_t HIT_WALL_SND_LEN = 8;
constexpr uint8_t hit_wall_snd[] PROGMEM = { 0x83 , 0x83 , 0x82 , 0x8e , 0x8a , 0x89 , 0x86 , 0x84};
constexpr uint8_t WALK1_SND_LEN = 3;
constexpr uint8_t walk1_snd[] PROGMEM = { 0x8f, 0x8e, 0x8e};
constexpr uint8_t WALK2_SND_LEN = 3;
constexpr uint8_t walk2_snd[] PROGMEM = { 0x84, 0x87, 0x84};
constexpr uint8_t MEDKIT_SND_LEN = 69;
constexpr uint8_t medkit_snd[] PROGMEM = {0x55 , 0x20 , 0x3a , 0x3a , 0x3a , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x33 , 0x33 , 0x33 , 0x33 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x26 , 0x26 , 0x26 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x16 , 0x16 , 0x16 , 0x16 , 0x16 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x16 , 0x16 , 0x16 , 0x16 , 0x16 , 0x16 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x20 , 0x15 , 0x15 , 0x15 , 0x15 , 0x15 , 0x15, 0x15};
constexpr uint8_t MELEE_SND_LEN = 9;
constexpr uint8_t melee_snd[] PROGMEM = {0x8f, 0x8e, 0x8e};


uint8_t idx = 0;
bool sound = false;
uint16_t snd_ptr = 0;
uint8_t snd_len = 0;
uint8_t music = 0;

void sound_init() {
  
  pinMode(SOUND_PIN, OUTPUT);

  TCCR2A = (1 << WGM21); // CTC
  TCCR2B = (1 << CS22) | (1 << CS21) | (1 << CS20); // prescaler 1024
  OCR2A = 112 - 1; // 16000000 / 1024 / 112 -> 139,5 Hz
  TIMSK2 = (1 << OCIE2A);
}

void playSound(const uint8_t* snd, uint8_t len) {
  snd_ptr = reinterpret_cast<uint16_t>(snd);
  snd_len = len;
  sound = true;
}

// Set the frequency that we will get on pin OCR1A
void setFrequency(uint16_t freq) {
  uint32_t requiredDivisor = (F_CPU / 2) / (uint32_t)freq;

  uint16_t prescalerVal;
  uint8_t prescalerBits;
  if (requiredDivisor < 65536UL) {
    prescalerVal = 1;
    prescalerBits = 1; // prescaler 1
  } else if (requiredDivisor < 8 * 65536UL) {
    prescalerVal = 8;
    prescalerBits = 2; // prescaler 8
  } else if (requiredDivisor < 64 * 65536UL) {
    prescalerVal = 64;
    prescalerBits = 3; // prescaler 64
  } else if (requiredDivisor < 256 * 65536UL) {
    prescalerVal = 256;
    prescalerBits = 4; // prescaler 256
  } else {
    prescalerVal = 1024;
    prescalerBits = 5; // prescaler 1024
  }
  
  uint16_t top = ((requiredDivisor + (prescalerVal / 2)) / prescalerVal) - 1;
  TCCR1A = _BV(COM1A0) /*+ _BV(COM1B0)*/;
  if (freq == 0){TCCR1A = 0;}
  TCCR1B = (1 << WGM12) | prescalerBits;  // CTC
  TCCR1C = _BV(FOC1A);
  OCR1A = top;
}

void off() {
  TCCR1A = 0;
}

ISR(TIMER2_COMPA_vect) {
  if (sound) {
    if (idx++ < snd_len) {
      uint16_t freq = 1192030 / (60 * (uint16_t) pgm_read_byte(snd_ptr + idx)); // 1193181 
      if(freq > 65534){freq = 0;}
      setFrequency(freq);
      

    } else {
      idx = 0;
      off();
      sound = false;
      if (snd_len == 38 || snd_len == 19) {
        music++;
      }
      if (snd_len == 44) {
        playSound(mus_s1_snd, MUS_S1_SND_LEN);
      }
      if (music == 3  || music == 5 || music == 7 || music == 9 || music == 12 || music == 14 || music == 16 || music == 18 || music == 25  || music == 27 || music == 29 || music == 31 || music == 34 || music == 36 || music == 38 || music == 40) {
        playSound(mus_s1_snd, MUS_S1_SND_LEN);
      }
      else if (music == 2 || music == 13 || music == 24 || music == 35) {
        playSound(mus_p1_snd, MUS_P1_SND_LEN);
      }
      else if (music == 4 || music == 15 || music == 26 || music == 37) {
        playSound(mus_p2_snd, MUS_P2_SND_LEN);
      }
      else if (music == 6 || music == 17 || music == 28 || music == 39) {
        playSound(mus_p3_snd, MUS_P3_SND_LEN);
      }
      else if (music == 8 || music == 30) {
        playSound(mus_p4_snd, MUS_P4_SND_LEN);
      }
      else if (music == 10 || music == 32) {
        playSound(mus_p5_snd, MUS_P5_SND_LEN);
      }
      else if (music == 11 || music == 33) {
        playSound(mus_p6_snd, MUS_P6_SND_LEN);
      }
      else if (music == 19 || music == 20 || music == 21 || music == 41 || music == 42 || music == 43) {
        playSound(mus_p7_snd, MUS_P7_SND_LEN);
      }
      else if (music == 22 || music == 44) {
        playSound(mus_ep_snd, MUS_EP_SND_LEN);
      }
      else if (music == 23 || music == 45) {
        playSound(mus_s1_snd, MUS_S1_SND_LEN);
      }


      else if (music == 47  || music == 49 || music == 51 || music == 53 || music == 56 || music == 58 || music == 60 || music == 62) {
        playSound(mus_s1_snd, MUS_S1_SND_LEN);
      }
      else if (music == 46 || music == 57) {
        playSound(mus_p1_snd, MUS_P1_SND_LEN);
      }
      else if (music == 48 || music == 59) {
        playSound(mus_p2_snd, MUS_P2_SND_LEN);
      }
      else if (music == 50 || music == 61) {
        playSound(mus_p3_snd, MUS_P3_SND_LEN);
      }
      else if (music == 52) {
        playSound(mus_p4_snd, MUS_P4_SND_LEN);
      }
      else if (music == 54) {
        playSound(mus_p5_snd, MUS_P5_SND_LEN);
      }
      else if (music == 55) {
        playSound(mus_p6_snd, MUS_P6_SND_LEN);
      }
      else if (music == 63 || music == 64 || music == 65) {
        playSound(mus_p7_snd, MUS_P7_SND_LEN);
      }
      else if (music == 66) {
        playSound(mus_ep_snd, MUS_EP_SND_LEN);
      }





      else if (music == 68  || music == 70 || music == 72 || music == 74 || music == 77 || music == 79 || music == 81 ) {
        playSound(mus_s21_snd, MUS_S21_SND_LEN);
      }
      else if (music == 67 || music == 78) {
        playSound(mus_p21_snd, MUS_P21_SND_LEN);
      }
      else if (music == 69 || music == 80) {
        playSound(mus_p22_snd, MUS_P22_SND_LEN);
      }
      else if (music == 71 ) {
        playSound(mus_p2_snd, MUS_P2_SND_LEN);
      }
      else if (music == 73) {
        playSound(mus_p24_snd, MUS_P24_SND_LEN);
      }
      else if (music == 75) {
        playSound(mus_p2_snd, MUS_P2_SND_LEN);
      }
      else if (music == 76) {
        playSound(mus_p26_snd, MUS_P26_SND_LEN);
      }
      else if (music == 82 ) {
        playSound(mus_p27_snd, MUS_P27_SND_LEN);
      }
      else if (music == 83 ) {
        playSound(mus_p28_snd, MUS_P28_SND_LEN);
      }
      else if (music == 84) {
        playSound(mus_p29_snd, MUS_P29_SND_LEN);
      }
      else if (music == 85 ) {
        playSound(mus_ep2_snd, MUS_EP2_SND_LEN);
      }
      else if (music == 86 || music == 87 || music == 88) {
        playSound(mus_ep_snd, MUS_EP_SND_LEN);
      }
      else if (music == 89) {
        playSound(mus_s1_snd, MUS_S1_SND_LEN);
        music = 2;
      }
      
      
    }
  }
}

#endif

sound.h

sprites.h

/*!
 * @file Adafruit_SSD1306.cpp
 *
 * @mainpage Arduino library for monochrome OLEDs based on SSD1306 drivers.
 *
 * @section intro_sec Introduction
 *
 * This is documentation for Adafruit's SSD1306 library for monochrome
 * OLED displays: http://www.adafruit.com/category/63_98
 *
 * These displays use I2C or SPI to communicate. I2C requires 2 pins
 * (SCL+SDA) and optionally a RESET pin. SPI requires 4 pins (MOSI, SCK,
 * select, data/command) and optionally a reset pin. Hardware SPI or
 * 'bitbang' software SPI are both supported.
 *
 * Adafruit invests time and resources providing this open source code,
 * please support Adafruit and open-source hardware by purchasing
 * products from Adafruit!
 *
 * @section dependencies Dependencies
 *
 * This library depends on <a href="https://github.com/adafruit/Adafruit-GFX-Library">
 * Adafruit_GFX</a> being present on your system. Please make sure you have
 * installed the latest version before using this library.
 *
 * @section author Author
 *
 * Written by Limor Fried/Ladyada for Adafruit Industries, with
 * contributions from the open source community.
 *
 * @section license License
 *
 * BSD license, all text above, and the splash screen included below,
 * must be included in any redistribution.
 *
 */

#ifdef __AVR__
 #include <avr/pgmspace.h>
#elif defined(ESP8266) || defined(ESP32)
 #include <pgmspace.h>
#else
 #define pgm_read_byte(addr) \
  (*(const unsigned char *)(addr)) ///< PROGMEM workaround for non-AVR
#endif

#if !defined(__ARM_ARCH) && !defined(ENERGIA) && !defined(ESP8266) && !defined(ESP32) && !defined(__arc__)
 #include <util/delay.h>
#endif

#include "SSD1306.h"

// SOME DEFINES AND STATIC VARIABLES USED INTERNALLY -----------------------

constexpr uint16_t WIRE_MAX = 342;

#define ssd1306_swap(a, b) \
  (((a) ^= (b)), ((b) ^= (a)), ((a) ^= (b))) ///< No-temp-var swap operation

// LOW-LEVEL UTILS ---------------------------------------------------------

// Issue single command to SSD1306, using I2C or hard/soft SPI as needed.
// Because command calls are often grouped, SPI transaction and selection
// must be started/ended in calling function for efficiency.
// This is a private function, not exposed (see ssd1306_command() instead).
template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::ssd1306_command1(uint8_t c) {
  uint8_t cmd = 0x00; // Co = 0, D/C = 0
  TWI_Start_Transceiver_With_Data(cmd, &c, 1);
}

// Issue list of commands to SSD1306, same rules as above re: transactions.
// This is a private function, not exposed.
template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::ssd1306_commandList(const uint8_t *c, uint8_t n) {
  uint8_t cmd = 0x00; // Co = 0, D/C = 0
  uint8_t tmp[n];

  uint8_t * tmpptr = static_cast<uint8_t *>(memcpy_P(tmp, c, n));
  TWI_Start_Transceiver_With_Data(cmd, tmpptr, n);
}

// ALLOCATE & INIT DISPLAY -------------------------------------------------

/*!
    @brief  Allocate RAM for image buffer, initialize peripherals and pins.
    @param  vcs
            VCC selection. Pass SSD1306_SWITCHCAPVCC to generate the display
            voltage (step up) from the 3.3V source, or SSD1306_EXTERNALVCC
            otherwise. Most situations with Adafruit SSD1306 breakouts will
            want SSD1306_SWITCHCAPVCC.
    @param  addr
            I2C address of corresponding SSD1306 display (or pass 0 to use
            default of 0x3C for 128x32 display, 0x3D for all others).
            SPI displays (hardware or software) do not use addresses, but
            this argument is still required (pass 0 or any value really,
            it will simply be ignored). Default if unspecified is 0.
    @param  reset
            If true, and if the reset pin passed to the constructor is
            valid, a hard reset will be performed before initializing the
            display. If using multiple SSD1306 displays on the same bus, and
            if they all share the same reset pin, you should only pass true
            on the first display being initialized, false on all others,
            else the already-initialized displays would be reset. Default if
            unspecified is true.
    @param  periphBegin
            If true, and if a hardware peripheral is being used (I2C or SPI,
            but not software SPI), call that peripheral's begin() function,
            else (false) it has already been done in one's sketch code.
            Cases where false might be used include multiple displays or
            other devices sharing a common bus, or situations on some
            platforms where a nonstandard begin() function is available
            (e.g. a TwoWire interface on non-default pins, as can be done
            on the ESP8266 and perhaps others).
    @return true on successful allocation/init, false otherwise.
            Well-behaved code should check the return value before
            proceeding.
    @note   MUST call this function before any drawing or updates!
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
bool Adafruit_SSD1306<WIDTH, HEIGHT>::begin(uint8_t vcs, uint8_t addr) {

  clearDisplay();

  vccstate = vcs;

  // Setup pin directions
    // If I2C address is unspecified, use default
    // (0x3C for 32-pixel-tall displays, 0x3D for all others).
    i2caddr = addr ? addr : ((HEIGHT == 32) ? 0x3C : 0x3D);
    // TwoWire begin() function might be already performed by the calling
    // function if it has unusual circumstances (e.g. TWI variants that
    // can accept different SDA/SCL pins, or if two SSD1306 instances
    // with different addresses -- only a single begin() is needed).
  TWI_Master_Initialise();

  // Init sequence
  static const uint8_t PROGMEM init1[] = {
    SSD1306_DISPLAYOFF,                   // 0xAE
    SSD1306_SETDISPLAYCLOCKDIV,           // 0xD5
    0x80,                                 // the suggested ratio 0x80
    SSD1306_SETMULTIPLEX };               // 0xA8
  ssd1306_commandList(init1, sizeof(init1));
  ssd1306_command1(HEIGHT - 1);

  static const uint8_t PROGMEM init2[] = {
    SSD1306_SETDISPLAYOFFSET,             // 0xD3
    0x0,                                  // no offset
    SSD1306_SETSTARTLINE | 0x0,           // line #0
    SSD1306_CHARGEPUMP };                 // 0x8D
  ssd1306_commandList(init2, sizeof(init2));

  ssd1306_command1((vccstate == SSD1306_EXTERNALVCC) ? 0x10 : 0x14);

  static const uint8_t PROGMEM init3[] = {
    SSD1306_MEMORYMODE,                   // 0x20
    0x00,                                 // 0x0 act like ks0108
    SSD1306_SEGREMAP | 0x1,
    SSD1306_COMSCANDEC };
  ssd1306_commandList(init3, sizeof(init3));

  if((WIDTH == 128) && (HEIGHT == 32)) {
    static const uint8_t PROGMEM init4a[] = {
      SSD1306_SETCOMPINS,                 // 0xDA
      0x02,
      SSD1306_SETCONTRAST,                // 0x81
      0x8F };
    ssd1306_commandList(init4a, sizeof(init4a));
  } else if((WIDTH == 128) && (HEIGHT == 64)) {
    static const uint8_t PROGMEM init4b[] = {
      SSD1306_SETCOMPINS,                 // 0xDA
      0x12,
      SSD1306_SETCONTRAST };              // 0x81
    ssd1306_commandList(init4b, sizeof(init4b));
    ssd1306_command1((vccstate == SSD1306_EXTERNALVCC) ? 0x9F : 0xCF);
  } else if((WIDTH == 96) && (HEIGHT == 16)) {
    static const uint8_t PROGMEM init4c[] = {
      SSD1306_SETCOMPINS,                 // 0xDA
      0x2,    // ada x12
      SSD1306_SETCONTRAST };              // 0x81
    ssd1306_commandList(init4c, sizeof(init4c));
    ssd1306_command1((vccstate == SSD1306_EXTERNALVCC) ? 0x10 : 0xAF);
  } else {
    // Other screen varieties -- TBD
  }

  ssd1306_command1(SSD1306_SETPRECHARGE); // 0xd9
  ssd1306_command1((vccstate == SSD1306_EXTERNALVCC) ? 0x22 : 0xF1);
  static const uint8_t PROGMEM init5[] = {
    SSD1306_SETVCOMDETECT,               // 0xDB
    0x40,
    SSD1306_DISPLAYALLON_RESUME,         // 0xA4
    SSD1306_NORMALDISPLAY,               // 0xA6
    SSD1306_DEACTIVATE_SCROLL,
    SSD1306_DISPLAYON };                 // Main screen turn on
  ssd1306_commandList(init5, sizeof(init5));

  return true; // Success
}

// DRAWING FUNCTIONS -------------------------------------------------------

/*!
    @brief  Set/clear/invert a single pixel. This is also invoked by the
            Adafruit_GFX library in generating many higher-level graphics
            primitives.
    @param  x
            Column of display -- 0 at left to (screen width - 1) at right.
    @param  y
            Row of display -- 0 at top to (screen height -1) at bottom.
    @param  color
            Pixel color, one of: SSD1306_BLACK, SSD1306_WHITE or SSD1306_INVERT.
    @return None (void).
    @note   Changes buffer contents only, no immediate effect on display.
            Follow up with a call to display(), or with other graphics
            commands as needed by one's own application.
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::drawPixel(int16_t x, int16_t y, uint16_t color) {
  if((x >= 0) && (x < WIDTH) && (y >= 0) && (y < HEIGHT)) {
    // Pixel is in-bounds. Rotate coordinates if needed.
    switch(color) {
     case SSD1306_WHITE:   buffer[x + (y/8)*WIDTH] |=  (1 << (y&7)); break;
     case SSD1306_BLACK:   buffer[x + (y/8)*WIDTH] &= ~(1 << (y&7)); break;
     case SSD1306_INVERSE: buffer[x + (y/8)*WIDTH] ^=  (1 << (y&7)); break;
    }
  }
}

/*!
    @brief  Clear contents of display buffer (set all pixels to off).
    @return None (void).
    @note   Changes buffer contents only, no immediate effect on display.
            Follow up with a call to display(), or with other graphics
            commands as needed by one's own application.
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::clearDisplay(void) {
  memset(buffer, 0, WIDTH * ((HEIGHT + 7) / 8));
}

/*!
    @brief  Draw a vertical line. This is also invoked by the Adafruit_GFX
            library in generating many higher-level graphics primitives.
    @param  x
            Column of display -- 0 at left to (screen width -1) at right.
    @param  y
            Topmost row -- 0 at top to (screen height - 1) at bottom.
    @param  h
            Height of line, in pixels.
    @param  color
            Line color, one of: SSD1306_BLACK, SSD1306_WHITE or SSD1306_INVERT.
    @return None (void).
    @note   Changes buffer contents only, no immediate effect on display.
            Follow up with a call to display(), or with other graphics
            commands as needed by one's own application.
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::drawFastVLine(
  int16_t x, int16_t y, int16_t h, uint16_t color) {
  drawFastVLineInternal(x, y, h, color);
}

template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::drawFastVLineInternal(
  int16_t x, int16_t __y, int16_t __h, uint16_t color) {

  if((x >= 0) && (x < WIDTH)) { // X coord in bounds?
    if(__y < 0) { // Clip top
      __h += __y;
      __y = 0;
    }
    if((__y + __h) > HEIGHT) { // Clip bottom
      __h = (HEIGHT - __y);
    }
    if(__h > 0) { // Proceed only if height is now positive
      // this display doesn't need ints for coordinates,
      // use local byte registers for faster juggling
      uint8_t  y = __y, h = __h;
      uint8_t *pBuf = &buffer[(y / 8) * WIDTH + x];

      // do the first partial byte, if necessary - this requires some masking
      uint8_t mod = (y & 7);
      if(mod) {
        // mask off the high n bits we want to set
        mod = 8 - mod;
        // note - lookup table results in a nearly 10% performance
        // improvement in fill* functions
        // uint8_t mask = ~(0xFF >> mod);
        static const uint8_t PROGMEM premask[8] =
          { 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0xFE };
        uint8_t mask = pgm_read_byte(&premask[mod]);
        // adjust the mask if we're not going to reach the end of this byte
        if(h < mod) mask &= (0XFF >> (mod - h));

        switch(color) {
         case SSD1306_WHITE:   *pBuf |=  mask; break;
         case SSD1306_BLACK:   *pBuf &= ~mask; break;
         case SSD1306_INVERSE: *pBuf ^=  mask; break;
        }
        pBuf += WIDTH;
      }

      if(h >= mod) { // More to go?
        h -= mod;
        // Write solid bytes while we can - effectively 8 rows at a time
        if(h >= 8) {
          if(color == SSD1306_INVERSE) {
            // separate copy of the code so we don't impact performance of
            // black/white write version with an extra comparison per loop
            do {
              *pBuf ^= 0xFF;  // Invert byte
              pBuf  += WIDTH; // Advance pointer 8 rows
              h     -= 8;     // Subtract 8 rows from height
            } while(h >= 8);
          } else {
            // store a local value to work with
            uint8_t val = (color != SSD1306_BLACK) ? 255 : 0;
            do {
              *pBuf = val;    // Set byte
              pBuf += WIDTH;  // Advance pointer 8 rows
              h    -= 8;      // Subtract 8 rows from height
            } while(h >= 8);
          }
        }

        if(h) { // Do the final partial byte, if necessary
          mod = h & 7;
          // this time we want to mask the low bits of the byte,
          // vs the high bits we did above
          // uint8_t mask = (1 << mod) - 1;
          // note - lookup table results in a nearly 10% performance
          // improvement in fill* functions
          static const uint8_t PROGMEM postmask[8] =
            { 0x00, 0x01, 0x03, 0x07, 0x0F, 0x1F, 0x3F, 0x7F };
          uint8_t mask = pgm_read_byte(&postmask[mod]);
          switch(color) {
           case SSD1306_WHITE:   *pBuf |=  mask; break;
           case SSD1306_BLACK:   *pBuf &= ~mask; break;
           case SSD1306_INVERSE: *pBuf ^=  mask; break;
          }
        }
      }
    } // endif positive height
  } // endif x in bounds
}

template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::clearRect(uint8_t x, uint8_t y, uint8_t w , uint8_t h) {
  for (int16_t i=x; i<x+w; i++) {
    drawFastVLineInternal(i, y, h, 0);
  }
}

template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::drawBitmap(int16_t x, int16_t y,
  const uint8_t bitmap[], int16_t w, int16_t h, uint16_t color) {

    int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte
    uint8_t byte = 0;

    for(int16_t j=0; j<h; j++, y++) {
        for(int16_t i=0; i<w; i++) {
            if(i & 7) byte <<= 1;
            else      byte   = pgm_read_byte(&bitmap[j * byteWidth + i / 8]);
            if(byte & 0x80) drawPixel(x+i, y, color);
        }
    }
}

/*!
    @brief  Return color of a single pixel in display buffer.
    @param  x
            Column of display -- 0 at left to (screen width - 1) at right.
    @param  y
            Row of display -- 0 at top to (screen height -1) at bottom.
    @return true if pixel is set (usually SSD1306_WHITE, unless display invert mode
            is enabled), false if clear (SSD1306_BLACK).
    @note   Reads from buffer contents; may not reflect current contents of
            screen if display() has not been called.
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
bool Adafruit_SSD1306<WIDTH, HEIGHT>::getPixel(int16_t x, int16_t y) {
  if((x >= 0) && (x < WIDTH) && (y >= 0) && (y < HEIGHT)) {
    // Pixel is in-bounds. Rotate coordinates if needed.
    return (buffer[x + (y / 8) * WIDTH] & (1 << (y & 7)));
  }
  return false; // Pixel out of bounds
}

/*!
    @brief  Get base address of display buffer for direct reading or writing.
    @return Pointer to an unsigned 8-bit array, column-major, columns padded
            to full byte boundary if needed.
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
uint8_t *Adafruit_SSD1306<WIDTH, HEIGHT>::getBuffer(void) {
  return buffer;
}

// REFRESH DISPLAY ---------------------------------------------------------

/*!
    @brief  Push data currently in RAM to SSD1306 display.
    @return None (void).
    @note   Drawing operations are not visible until this function is
            called. Call after each graphics command, or after a whole set
            of graphics commands, as best needed by one's own application.
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::display(void) {
  static const uint8_t PROGMEM dlist1[] = {
    SSD1306_PAGEADDR,
    0,                         // Page start address
    0xFF,                      // Page end (not really, but works here)
    SSD1306_COLUMNADDR,
    0 };                       // Column start address
  ssd1306_commandList(dlist1, sizeof(dlist1));
  ssd1306_command1(WIDTH - 1); // Column end address

  uint16_t count = WIDTH * ((HEIGHT + 7) / 8);
  uint8_t *ptr   = buffer;
  uint8_t cmd = 0x40;
  while(count >= (WIRE_MAX)){
    TWI_Start_Transceiver_With_Data(cmd, ptr, WIRE_MAX);
    count -= (WIRE_MAX);
    ptr += (WIRE_MAX);
  }
  if(count > 0) {
    TWI_Start_Transceiver_With_Data(cmd, ptr, count);
  }
}

// OTHER HARDWARE SETTINGS -------------------------------------------------

/*!
    @brief  Enable or disable display invert mode (white-on-black vs
            black-on-white).
    @param  i
            If true, switch to invert mode (black-on-white), else normal
            mode (white-on-black).
    @return None (void).
    @note   This has an immediate effect on the display, no need to call the
            display() function -- buffer contents are not changed, rather a
            different pixel mode of the display hardware is used. When
            enabled, drawing SSD1306_BLACK (value 0) pixels will actually draw white,
            SSD1306_WHITE (value 1) will draw black.
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
void Adafruit_SSD1306<WIDTH, HEIGHT>::invertDisplay(bool i) {
  ssd1306_command1(i ? SSD1306_INVERTDISPLAY : SSD1306_NORMALDISPLAY);
}


SSD1306.cpp

/*!
 * @file Adafruit_SSD1306.h
 *
 * This is part of for Adafruit's SSD1306 library for monochrome
 * OLED displays: http://www.adafruit.com/category/63_98
 *
 * These displays use I2C or SPI to communicate. I2C requires 2 pins
 * (SCL+SDA) and optionally a RESET pin. SPI requires 4 pins (MOSI, SCK,
 * select, data/command) and optionally a reset pin. Hardware SPI or
 * 'bitbang' software SPI are both supported.
 *
 * Adafruit invests time and resources providing this open source code,
 * please support Adafruit and open-source hardware by purchasing
 * products from Adafruit!
 *
 * Written by Limor Fried/Ladyada for Adafruit Industries, with
 * contributions from the open source community.
 *
 * BSD license, all text above, and the splash screen header file,
 * must be included in any redistribution.
 *
 */
#ifndef _Adafruit_SSD1306_H_
#define _Adafruit_SSD1306_H_

#include "TWI_Master.h"
#include "string.h"
#include "constants.h"

/// The following "raw" color names are kept for backwards client compatability
/// They can be disabled by predefining this macro before including the Adafruit header
/// client code will then need to be modified to use the scoped enum values directly
#ifndef NO_ADAFRUIT_SSD1306_COLOR_COMPATIBILITY
#define BLACK                     SSD1306_BLACK    ///< Draw 'off' pixels
#define WHITE                     SSD1306_WHITE    ///< Draw 'on' pixels
#define INVERSE                   SSD1306_INVERSE  ///< Invert pixels
#endif
        /// fit into the SSD1306_ naming scheme
#define SSD1306_BLACK               0    ///< Draw 'off' pixels
#define SSD1306_WHITE               1    ///< Draw 'on' pixels
#define SSD1306_INVERSE             2    ///< Invert pixels

#define SSD1306_MEMORYMODE          0x20 ///< See datasheet
#define SSD1306_COLUMNADDR          0x21 ///< See datasheet
#define SSD1306_PAGEADDR            0x22 ///< See datasheet
#define SSD1306_SETCONTRAST         0x81 ///< See datasheet
#define SSD1306_CHARGEPUMP          0x8D ///< See datasheet
#define SSD1306_SEGREMAP            0xA0 ///< See datasheet
#define SSD1306_DISPLAYALLON_RESUME 0xA4 ///< See datasheet
#define SSD1306_DISPLAYALLON        0xA5 ///< Not currently used
#define SSD1306_NORMALDISPLAY       0xA6 ///< See datasheet
#define SSD1306_INVERTDISPLAY       0xA7 ///< See datasheet
#define SSD1306_SETMULTIPLEX        0xA8 ///< See datasheet
#define SSD1306_DISPLAYOFF          0xAE ///< See datasheet
#define SSD1306_DISPLAYON           0xAF ///< See datasheet
#define SSD1306_COMSCANINC          0xC0 ///< Not currently used
#define SSD1306_COMSCANDEC          0xC8 ///< See datasheet
#define SSD1306_SETDISPLAYOFFSET    0xD3 ///< See datasheet
#define SSD1306_SETDISPLAYCLOCKDIV  0xD5 ///< See datasheet
#define SSD1306_SETPRECHARGE        0xD9 ///< See datasheet
#define SSD1306_SETCOMPINS          0xDA ///< See datasheet
#define SSD1306_SETVCOMDETECT       0xDB ///< See datasheet

#define SSD1306_SETLOWCOLUMN        0x00 ///< Not currently used
#define SSD1306_SETHIGHCOLUMN       0x10 ///< Not currently used
#define SSD1306_SETSTARTLINE        0x40 ///< See datasheet

#define SSD1306_EXTERNALVCC         0x01 ///< External display voltage source
#define SSD1306_SWITCHCAPVCC        0x02 ///< Gen. display voltage from 3.3V

#define SSD1306_RIGHT_HORIZONTAL_SCROLL              0x26 ///< Init rt scroll
#define SSD1306_LEFT_HORIZONTAL_SCROLL               0x27 ///< Init left scroll
#define SSD1306_VERTICAL_AND_RIGHT_HORIZONTAL_SCROLL 0x29 ///< Init diag scroll
#define SSD1306_VERTICAL_AND_LEFT_HORIZONTAL_SCROLL  0x2A ///< Init diag scroll
#define SSD1306_DEACTIVATE_SCROLL                    0x2E ///< Stop scroll
#define SSD1306_ACTIVATE_SCROLL                      0x2F ///< Start scroll
#define SSD1306_SET_VERTICAL_SCROLL_AREA             0xA3 ///< Set scroll range

/*! 
    @brief  Class that stores state and functions for interacting with
            SSD1306 OLED displays.
*/
template <uint8_t WIDTH, uint8_t HEIGHT>
class Adafruit_SSD1306 {
 public:
  Adafruit_SSD1306() = default;

  ~Adafruit_SSD1306(void) = default;

  bool      begin(uint8_t switchvcc=SSD1306_SWITCHCAPVCC,
                 uint8_t i2caddr=0);
  void         display(void);
  void         clearDisplay(void);
  void         invertDisplay(bool i);
  void         drawPixel(int16_t x, int16_t y, uint16_t color);
  void drawFastVLine(int16_t x, int16_t y, int16_t h, uint16_t color);
  bool      getPixel(int16_t x, int16_t y);
  uint8_t     *getBuffer(void);
  void clearRect(uint8_t, uint8_t, uint8_t, uint8_t);
  void drawBitmap(int16_t x, int16_t y, const uint8_t bitmap[], int16_t w, int16_t h, uint16_t color);

 private:
  void         drawFastVLineInternal(int16_t x, int16_t y, int16_t h,
                 uint16_t color);
  void         ssd1306_command1(uint8_t c);
  void         ssd1306_commandList(const uint8_t *c, uint8_t n);

  uint8_t     buffer[WIDTH * ((HEIGHT + 7) / 8)];
  int8_t       i2caddr, vccstate, page_end;
};

template class Adafruit_SSD1306<SCREEN_WIDTH, SCREEN_HEIGHT>;

#endif // _Adafruit_SSD1306_H_


SSD1306.h

/**
 * \file
 *
 * \brief Application to generate sample driver to AVRs TWI module
 *
 * Copyright (C) 2014-2015 Atmel Corporation. All rights reserved.
 *
 * \asf_license_start
 *
 * \page License
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. The name of Atmel may not be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * 4. This software may only be redistributed and used in connection with an
 *    Atmel micro controller product.
 *
 * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
 * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * \asf_license_stop
 *
 */
/*
 * Support and FAQ: visit <a href="http://www.atmel.com/design-support/">Atmel Support</a>
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include "TWI_Master.h"

static unsigned char TWI_buf[ TWI_BUFFER_SIZE ];    // Transceiver buffer
static uint16_t TWI_msgSize;                   // Number of bytes to be transmitted.
static unsigned char TWI_state = TWI_NO_STATE;      // State byte. Default set to TWI_NO_STATE.
static unsigned char lastTransOK; 

/****************************************************************************
Call this function to set up the TWI master to its initial standby state.
Remember to enable interrupts from the main application after initializing the TWI.
****************************************************************************/
void TWI_Master_Initialise(void)
{
  TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;                                  // Set bit rate register (Baud rate). Defined in header file.Driver presumes prescaler to be 00.                                
  TWDR = 0xFF;                                      // Default content = SDA released.
  TWCR = (1<<TWEN)|                                 // Enable TWI-interface and release TWI pins.
         (0<<TWIE)|(0<<TWINT)|                      // Disable Interrupt.
         (0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|           // No Signal requests.
         (0<<TWWC);                                 //
}    
    
/****************************************************************************
Call this function to test if the TWI_ISR is busy transmitting.
****************************************************************************/
unsigned char TWI_Transceiver_Busy( void )
{
  return ( TWCR & (1<<TWIE) );                  // IF TWI Interrupt is enabled then the Transceiver is busy
}

/****************************************************************************
Call this function to fetch the state information of the previous operation. The function will hold execution (loop)
until the TWI_ISR has completed with the previous operation. If there was an error, then the function 
will return the TWI State code. 
****************************************************************************/
unsigned char TWI_Get_State_Info( void )
{
  while ( TWI_Transceiver_Busy() );             // Wait until TWI has completed the transmission.
  return ( TWI_state );                         // Return error state.
}

/****************************************************************************
Call this function to send a prepared message. The first byte must contain the slave address and the
read/write bit. Consecutive bytes contain the data to be sent, or empty locations for data to be read
from the slave. Also include how many bytes that should be sent/read including the address byte.
The function will hold execution (loop) until the TWI_ISR has completed with the previous operation,
then initialize the next operation and return.
****************************************************************************/
void TWI_Start_Transceiver_With_Data( uint8_t cmd, unsigned char *msg, uint16_t msgSize )
{
  while ( TWI_Transceiver_Busy() );             // Wait until TWI is ready for next transmission.

  TWI_msgSize = msgSize + 2;                        // Number of data to transmit.
  TWI_buf[0]  = TWI_ADDR;                         // Store slave address with R/W setting.
  TWI_buf[1] = cmd;

  for (uint16_t i = 2; msgSize--; i++ ) {
    TWI_buf[i] = *msg++;
  }

  lastTransOK = 0;      
  TWI_state         = TWI_NO_STATE ;
  TWCR = (1<<TWEN)|                             // TWI Interface enabled.
         (1<<TWIE)|(1<<TWINT)|                  // Enable TWI Interrupt and clear the flag.
         (0<<TWEA)|(1<<TWSTA)|(0<<TWSTO)|       // Initiate a START condition.
         (0<<TWWC);                             //
}

/****************************************************************************
Call this function to resend the last message. The driver will reuse the data previously put in the transceiver buffers.
The function will hold execution (loop) until the TWI_ISR has completed with the previous operation,
then initialize the next operation and return.
****************************************************************************/
void TWI_Start_Transceiver( void )
{
  while ( TWI_Transceiver_Busy() );             // Wait until TWI is ready for next transmission.
  lastTransOK = 0;      
  TWI_state         = TWI_NO_STATE ;
  TWCR = (1<<TWEN)|                             // TWI Interface enabled.
         (1<<TWIE)|(1<<TWINT)|                  // Enable TWI Interrupt and clear the flag.
         (0<<TWEA)|(1<<TWSTA)|(0<<TWSTO)|       // Initiate a START condition.
         (0<<TWWC);                             //
}

/****************************************************************************
Call this function to read out the requested data from the TWI transceiver buffer. I.e. first call
TWI_Start_Transceiver to send a request for data to the slave. Then Run this function to collect the
data when they have arrived. Include a pointer to where to place the data and the number of bytes
requested (including the address field) in the function call. The function will hold execution (loop)
until the TWI_ISR has completed with the previous operation, before reading out the data and returning.
If there was an error in the previous transmission the function will return the TWI error code.
****************************************************************************/
unsigned char TWI_Get_Data_From_Transceiver( unsigned char *msg, unsigned char msgSize )
{
  unsigned char i;

  while ( TWI_Transceiver_Busy() );             // Wait until TWI is ready for next transmission.

  if( lastTransOK )               // Last transmission competed successfully.              
  {                                             
    for ( i=0; i<msgSize; i++ )                 // Copy data from Transceiver buffer.
    {
      msg[ i ] = TWI_buf[ i ];
    }
  }
  return lastTransOK;                                   
}

// ********** Interrupt Handlers ********** //
/****************************************************************************
This function is the Interrupt Service Routine (ISR), and called when the TWI interrupt is triggered;
that is whenever a TWI event has occurred. This function should not be called directly from the main
application.
****************************************************************************/
ISR(TWI_vect)
{
  static uint16_t TWI_bufPtr;
  
  switch (TWSR)
  {
    case TWI_START:             // START has been transmitted  
    case TWI_REP_START:         // Repeated START has been transmitted
      TWI_bufPtr = 0;                                     // Set buffer pointer to the TWI Address location
    case TWI_MTX_ADR_ACK:       // SLA+W has been transmitted and ACK received
    case TWI_MTX_DATA_ACK:      // Data byte has been transmitted and ACK received
      if (TWI_bufPtr < TWI_msgSize)
      {
        TWDR = TWI_buf[TWI_bufPtr++];
        TWCR = (1<<TWEN)|                                 // TWI Interface enabled
               (1<<TWIE)|(1<<TWINT)|                      // Enable TWI Interrupt and clear the flag to send byte
               (0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|           //
               (0<<TWWC);                                 //  
      }else                    // Send STOP after last byte
      {
        lastTransOK = TRUE;                 // Set status bits to completed successfully. 
        TWCR = (1<<TWEN)|                                 // TWI Interface enabled
               (0<<TWIE)|(1<<TWINT)|                      // Disable TWI Interrupt and clear the flag
               (0<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|           // Initiate a STOP condition.
               (0<<TWWC);                                 //
      }
      break;
    case TWI_MRX_DATA_ACK:      // Data byte has been received and ACK transmitted
      TWI_buf[TWI_bufPtr++] = TWDR;
    case TWI_MRX_ADR_ACK:       // SLA+R has been transmitted and ACK received
      if (TWI_bufPtr < (TWI_msgSize-1) )                  // Detect the last byte to NACK it.
      {
        TWCR = (1<<TWEN)|                                 // TWI Interface enabled
               (1<<TWIE)|(1<<TWINT)|                      // Enable TWI Interrupt and clear the flag to read next byte
               (1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|           // Send ACK after reception
               (0<<TWWC);                                 //  
      }else                    // Send NACK after next reception
      {
        TWCR = (1<<TWEN)|                                 // TWI Interface enabled
               (1<<TWIE)|(1<<TWINT)|                      // Enable TWI Interrupt and clear the flag to read next byte
               (0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|           // Send NACK after reception
               (0<<TWWC);                                 // 
      }    
      break; 
    case TWI_MRX_DATA_NACK:     // Data byte has been received and NACK transmitted
      TWI_buf[TWI_bufPtr] = TWDR;
      lastTransOK = TRUE;                 // Set status bits to completed successfully. 
      TWCR = (1<<TWEN)|                                 // TWI Interface enabled
             (0<<TWIE)|(1<<TWINT)|                      // Disable TWI Interrupt and clear the flag
             (0<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|           // Initiate a STOP condition.
             (0<<TWWC);                                 //
      break;      
    case TWI_ARB_LOST:          // Arbitration lost
      TWCR = (1<<TWEN)|                                 // TWI Interface enabled
             (1<<TWIE)|(1<<TWINT)|                      // Enable TWI Interrupt and clear the flag
             (0<<TWEA)|(1<<TWSTA)|(0<<TWSTO)|           // Initiate a (RE)START condition.
             (0<<TWWC);                                 //
      break;
    case TWI_MTX_ADR_NACK:      // SLA+W has been transmitted and NACK received
    case TWI_MRX_ADR_NACK:      // SLA+R has been transmitted and NACK received    
    case TWI_MTX_DATA_NACK:     // Data byte has been transmitted and NACK received
    case TWI_BUS_ERROR:         // Bus error due to an illegal START or STOP condition
    default:     
      TWI_state = TWSR;                                 // Store TWSR and automatically sets clears noErrors bit.
                                                        // Reset TWI Interface
      TWCR = (1<<TWEN)|                                 // Enable TWI-interface and release TWI pins
             (0<<TWIE)|(0<<TWINT)|                      // Disable Interrupt
             (0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|           // No Signal requests
             (0<<TWWC);                                 //
  }
}


TWI_Master.cpp

/**
 * \file
 *
 * \brief Application to generate sample driver to AVRs TWI module
 *
 * Copyright (C) 2014-2015 Atmel Corporation. All rights reserved.
 *
 * \asf_license_start
 *
 * \page License
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. The name of Atmel may not be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * 4. This software may only be redistributed and used in connection with an
 *    Atmel micro controller product.
 *
 * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
 * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * \asf_license_stop
 *
 */
/*
 * Support and FAQ: visit <a href="http://www.atmel.com/design-support/">Atmel Support</a>
 */

/****************************************************************************
  TWI Status/Control register definitions
****************************************************************************/
constexpr uint16_t TWI_BUFFER_SIZE = 344;   // Set this to the largest message size that will be sent including address byte.
#define TWI_FREQ 400000UL
#define TWI_ADDR 0x78 // 0x3C << 1

/****************************************************************************
  Function definitions
****************************************************************************/
void TWI_Master_Initialise( void );
unsigned char TWI_Transceiver_Busy( void );
unsigned char TWI_Get_State_Info( void );
void TWI_Start_Transceiver_With_Data( uint8_t cmd, unsigned char * , uint16_t);
void TWI_Start_Transceiver( void );
unsigned char TWI_Get_Data_From_Transceiver( unsigned char *, unsigned char );

/****************************************************************************
  Bit and byte definitions
****************************************************************************/
#define TWI_READ_BIT  0       // Bit position for R/W bit in "address byte".
#define TWI_ADR_BITS  1       // Bit position for LSB of the slave address bits in the init byte.

#define TRUE          1
#define FALSE         0

/****************************************************************************
  TWI State codes
****************************************************************************/
// General TWI Master status codes                      
#define TWI_START                  0x08  // START has been transmitted  
#define TWI_REP_START              0x10  // Repeated START has been transmitted
#define TWI_ARB_LOST               0x38  // Arbitration lost

// TWI Master Transmitter status codes                      
#define TWI_MTX_ADR_ACK            0x18  // SLA+W has been transmitted and ACK received
#define TWI_MTX_ADR_NACK           0x20  // SLA+W has been transmitted and NACK received 
#define TWI_MTX_DATA_ACK           0x28  // Data byte has been transmitted and ACK received
#define TWI_MTX_DATA_NACK          0x30  // Data byte has been transmitted and NACK received 

// TWI Master Receiver status codes  
#define TWI_MRX_ADR_ACK            0x40  // SLA+R has been transmitted and ACK received
#define TWI_MRX_ADR_NACK           0x48  // SLA+R has been transmitted and NACK received
#define TWI_MRX_DATA_ACK           0x50  // Data byte has been received and ACK transmitted
#define TWI_MRX_DATA_NACK          0x58  // Data byte has been received and NACK transmitted

// TWI Slave Transmitter status codes
#define TWI_STX_ADR_ACK            0xA8  // Own SLA+R has been received; ACK has been returned
#define TWI_STX_ADR_ACK_M_ARB_LOST 0xB0  // Arbitration lost in SLA+R/W as Master; own SLA+R has been received; ACK has been returned
#define TWI_STX_DATA_ACK           0xB8  // Data byte in TWDR has been transmitted; ACK has been received
#define TWI_STX_DATA_NACK          0xC0  // Data byte in TWDR has been transmitted; NOT ACK has been received
#define TWI_STX_DATA_ACK_LAST_BYTE 0xC8  // Last data byte in TWDR has been transmitted (TWEA = �0�); ACK has been received

// TWI Slave Receiver status codes
#define TWI_SRX_ADR_ACK            0x60  // Own SLA+W has been received ACK has been returned
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68  // Arbitration lost in SLA+R/W as Master; own SLA+W has been received; ACK has been returned
#define TWI_SRX_GEN_ACK            0x70  // General call address has been received; ACK has been returned
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78  // Arbitration lost in SLA+R/W as Master; General call address has been received; ACK has been returned
#define TWI_SRX_ADR_DATA_ACK       0x80  // Previously addressed with own SLA+W; data has been received; ACK has been returned
#define TWI_SRX_ADR_DATA_NACK      0x88  // Previously addressed with own SLA+W; data has been received; NOT ACK has been returned
#define TWI_SRX_GEN_DATA_ACK       0x90  // Previously addressed with general call; data has been received; ACK has been returned
#define TWI_SRX_GEN_DATA_NACK      0x98  // Previously addressed with general call; data has been received; NOT ACK has been returned
#define TWI_SRX_STOP_RESTART       0xA0  // A STOP condition or repeated START condition has been received while still addressed as Slave

// TWI Miscellaneous status codes
#define TWI_NO_STATE               0xF8  // No relevant state information available; TWINT = �0�
#define TWI_BUS_ERROR              0x00  // Bus error due to an illegal START or STOP condition


TWI_Master.h

#include <stdint.h>
#include <math.h>
#include "types.h"
#include "constants.h"

template <class T>
inline T sq(T value) {
    return value * value;
}

Coords create_coords(double x, double y) {
  return { x, y };
}

uint8_t coords_distance(Coords* a, Coords* b) {
  return sqrt(sq(a->x - b->x) + sq(a->y - b->y)) * DISTANCE_MULTIPLIER;
}

UID create_uid(uint8_t type, uint8_t x, uint8_t y) {
  return ((y << LEVEL_WIDTH_BASE) | x) << 4 | type;
}
  
uint8_t uid_get_type(UID uid) {
  return uid & 0x0F;
}


types.cpp

#ifndef _types_h
#define _types_h

#define UID_null  0

// Entity types (legend applies to level.h)
#define E_FLOOR             0x0   // . (also null)
#define E_WALL              0xF   // #
#define E_DOOR              0xD   
#define E_DOOR2             0xA   
#define E_DOOR3             0xB   
#define E_COLL              0xC   
#define E_PLAYER            0x1   // P
#define E_ENEMY             0x2   // E
#define E_EXIT              0x7   // X
// collectable entities >= 0x8
#define E_MEDIKIT           0x8   // M
#define E_KEY               0x9   // K
#define E_FIREBALL          0xA   // not in map

typedef uint16_t UID;
typedef uint8_t  EType;

struct Coords {
  double x;
  double y;
};

UID create_uid(EType type, uint8_t x, uint8_t y);
EType uid_get_type(UID uid);

Coords create_coords(double x, double y);
uint8_t coords_distance(Coords* a, Coords* b);

#endif

