#include <KS0108_GLCD.h>
#include <delfin_am_eintauchen_56x56.h>   // Bitmap
#include <string.h> // Noetig fuer memset
/*
 * Bitmap Rotation
 * Programm rotates a bitmap from PROGMEM, stores the rotated image in an array in RAM
 * and displays the rotated image. Non-square images might get cropped.
 */

long unsigned int startMillis;
short unsigned int iter = 0;              // used to calculate the frames per second (FPS)
int winkel = 0;      // angle the bitmap is to be rotated by
void setup(){
  Serial.begin(57600);
  GLCD.Init(NON_INVERTED);   // initialise the library, non inverted writes pixels onto a clear screen
  GLCD.ClearScreen();  
  GLCD.SelectFont(System5x7); // switch to fixed width system font 
}

void Rotate_and_Draw_Bitmap(const uint8_t * bitmap, int winkel, uint8_t x, uint8_t y, uint8_t color){
  uint8_t width, height;
  width = ReadPgmData(bitmap++);            // Read the image width from the array in PROGMEM
  height = ReadPgmData(bitmap++);           // Read the height width from the array in PROGMEM

  int altes_x, altes_y, neues_x, neues_y; // old and new (rotated) Pixel-Coordinates

  int drehpunkt_x = width / 2;      // Calculate the (rotation) center of the image (x fraction)
  int drehpunkt_y = height / 2;     // Calculate the (rotation) center of the image (y fraction)

  float winkel_rad = winkel / 57.3;

  float sin_winkel = sin(winkel_rad);   // Lookup the sinus
  float cos_winkel = cos(winkel_rad);   // Lookup the cosinus

  uint8_t gedrehtes_bild[height/8*width+2]; // Image array in RAM (will contain the rotated image)
  memset(gedrehtes_bild,0,sizeof(gedrehtes_bild)); // Clear the array with 0
  
  int i, j, counter = 0;

  gedrehtes_bild[0] = width;                // First byte of the rotated image contains (as the original) the width
  gedrehtes_bild[1] = height;               // Second byte of the rotated image contains (as the original) the height

  for(i = 0; i < height * width / 8; i++) { // i goes through all the Bytes of the image
         uint8_t displayData = ReadPgmData(bitmap++);  // Read the image data from PROGMEM
       for(j = 0; j < 8; j++) {           // j goes through all the Bits of a Byte
                   if(displayData & (1 << j)){ // if a Bit is set, rotate it
                     altes_x = ((i % width) + 1) - drehpunkt_x;                     // Calculate the x-position of the Pixel to be rotated
                     altes_y = drehpunkt_y - (((int)(i/width))*8+j+1);              // Calculate the y-position of the Pixel to be rotated
                     neues_x = (int) (altes_x * cos_winkel - altes_y * sin_winkel); // Calculate the x-position of the rotated Pixel
                     neues_y = (int) (altes_y * cos_winkel + altes_x * sin_winkel); // Calculate the y-position of the rotated Pixel
                     
                     // Check if the rotated pixel is withing the image (important if non-square images are used). If not, continue with the next pixel.
                     if (neues_x <= (drehpunkt_x - 1) && neues_x >= (1 - drehpunkt_x) && neues_y <= (drehpunkt_y - 1) && neues_y >= (1 - drehpunkt_y)){ 
                       // Write the rotated bit to the array (gedrehtes_bild[]) in RAM
                       gedrehtes_bild[(neues_x + drehpunkt_x)%width + ((int)((drehpunkt_y - neues_y - 1) / 8)*width) +2] |= (1 << (drehpunkt_y - neues_y - 1)%8); 
                     }
                   }
       }
  }
  GLCD.DrawRamBitmap(gedrehtes_bild,x,y,color); // Draw the rotated image
}



void loop(){   // run over and over again
  iter = 0;
  startMillis = millis();
  while( millis() - startMillis < 1000){ // loop for one second
    winkel += 2;                         // increase angle by 4 degrees
    Rotate_and_Draw_Bitmap(delfin_am_eintauchen_56x56, winkel, 50,8, BLACK); 
    iter++;
  } 
  //display number of iterations in one second
  
}