#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED
#define OLED_Breite 128
#define OLED_Hoehe 64
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(OLED_Breite, OLED_Hoehe, &Wire, OLED_RESET);

#define DEG2RAD 0.0174532925

long unsigned int startMillis;
short unsigned int iter = 0;              // used to calculate the frames per second (FPS)
int angle = 0;


#define ASFF_BMPWIDTH  64
#define ASFF_BMPHeight 44
const unsigned char testbmp1[] PROGMEM = {
  64, 44,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00111111, B11111111, B11111111, B11111111, B11111111, B11111111, B10000000,
  B00000000, B01100000, B00000000, B00000000, B00000000, B00000000, B00000000, B11000000,
  B00000001, B11000001, B11111110, B00000000, B00000000, B00000000, B00000000, B11000000,
  B00000011, B00000001, B00000110, B00000000, B00000000, B00000000, B00000000, B11000000,
  B00000100, B00000001, B00000010, B00000000, B00000000, B00000000, B00000000, B11000000,
  B00000100, B00000001, B11111110, B00000000, B00000000, B00000000, B00000000, B11000000,
  B00000100, B00000001, B11111100, B00111111, B11100011, B11111100, B11100000, B11000000,
  B00000100, B00000000, B00000000, B01100000, B00110011, B00000000, B01100000, B11000000,
  B00000011, B11111111, B11111100, B01000000, B00010010, B00000000, B00100000, B11000000,
  B00000000, B00110000, B00000110, B01011111, B10010010, B00000000, B00100000, B11000000,
  B00000000, B00100111, B11110010, B01011000, B11010010, B00000000, B00100000, B11000000,
  B00000000, B01001100, B00110010, B01011000, B11010010, B00000000, B00100000, B11000000,
  B00000000, B11011000, B00010010, B01011000, B11010011, B00000000, B00100000, B11000000,
  B00000001, B10110000, B00010010, B01011000, B11010011, B11111111, B11100000, B11000000,
  B00000011, B00100000, B00010010, B01011000, B11010000, B00000000, B00000000, B11000000,
  B00000010, B01000000, B00010010, B01011111, B11010000, B00000000, B00000000, B11000000,
  B00000100, B11000000, B00010010, B01000000, B00010000, B00000000, B00000000, B11000000,
  B00000100, B11111111, B11110010, B01000000, B00010000, B00000000, B00000000, B11000000,
  B00000100, B00000000, B00000010, B01000000, B00010000, B00000000, B00000000, B11000000,
  B00000100, B00000000, B00000010, B01000000, B00010000, B00000000, B00000000, B11000000,
  B00000100, B00000111, B10000010, B01000000, B00010000, B00000011, B11000000, B11000000,
  B00000100, B00111100, B11100010, B01000000, B00010000, B00001110, B01110000, B11000000,
  B00000100, B01100000, B00110010, B01000000, B00010000, B00010000, B00011000, B11000000,
  B00000110, B11001111, B10011110, B11000000, B00011000, B01100111, B11101100, B11000000,
  B00000111, B10011000, B01001111, B11000000, B00011111, B11001100, B00110111, B11000000,
  B00000100, B00110111, B00100010, B01000000, B00010000, B00001001, B10010000, B11000000,
  B00000100, B00100101, B10100010, B01000000, B00010000, B00010010, B11010000, B11000000,
  B00000110, B00101100, B10110110, B11000000, B00110000, B00010010, B01011000, B10000000,
  B00000011, B11100111, B10111111, B11111111, B11111111, B11110011, B11011111, B00000000,
  B00000000, B00110010, B01100000, B00000000, B00000000, B00001001, B10010000, B00000000,
  B00000000, B00011000, B11000000, B00000000, B00000000, B00001100, B01100000, B00000000,
  B00000000, B00001111, B10000000, B00000000, B00000000, B00000111, B11000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000,
  B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000, B00000000
};


const static uint8_t PROGMEM testbmp2[] =
{ 16, 9,
  B00000011, B00000000,
  B00001111, B00000000,
  B00111111, B00000000,
  B01111111, B11111111,
  B11111111, B11111111,
  B01111111, B11111111,
  B00111111, B00000000,
  B00001111, B00000000,
  B00000011, B00000000
};

const static uint8_t PROGMEM bmp5x5[] =
{ 5, 5,
  B11111100, B01101011,
  B00011111, B10000000,
};

static const uint8_t PROGMEM adafruit[] =
{ 16, 16,
  B00000000, B11000000,
  B00000001, B11000000,
  B00000001, B11000000,
  B00000011, B11100000,
  B11110011, B11100000,
  B11111110, B11111000,
  B01111110, B11111111,
  B00110011, B10011111,
  B00011111, B11111100,
  B00001101, B01110000,
  B00011011, B10100000,
  B00111111, B11100000,
  B00111111, B11110000,
  B01111100, B11110000,
  B01110000, B01110000,
  B00000000, B00110000
};


void setup()   {

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    // nix
    for (;;); // Don't proceed, loop forever
  }

}
  void loop() {
    for (int angle = 0; angle < 360; angle += 10) {
      display.clearDisplay(); // Clear the display buffer

      drawRotatedBitmap(64, 32, testbmp1, angle);
      //drawRotatedBitmap(64, 16, adafruit, angle);
      //drawRotatedBitmap(108, 16, bmp5x5, angle);

      display.display();     // Now update the display with the buffer
      delay(250); // Pause so we see it
    }
  }

  void drawRotatedBitmap(int16_t x, int16_t y, const uint8_t *bitmap, uint16_t angle) {

    uint8_t w = pgm_read_byte(bitmap++);
    uint8_t h = pgm_read_byte(bitmap++);

    int16_t newx, newy;
    uint8_t data = 0;

    float  cosa = cos(angle * DEG2RAD), sina = sin(angle * DEG2RAD);

    x = x - ((w * cosa / 2) - (h * sina / 2));
    y = y - ((h * cosa / 2) + (w * sina / 2));

    for (int16_t j = 0; j < h; j++) {
      for (int16_t i = 0; i < w; i++ ) {
        if ((j * w + i) & 7) data <<= 1;
        else      data   = pgm_read_byte(bitmap++);

        newx = 0.5 + x + ((i * cosa) - (j * sina));
        newy = 0.5 + y + ((j * cosa) + (i * sina));

        if (data & 0x80) display.drawPixel(newx, newy, 1);
        //else            display.drawPixel(newx, newy, 0);
      }
    }
  }