#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include "pgmspace.h"
#include "Image.h"
#include <Fonts/FreeSerif9pt7b.h>
#include <Fonts/FreeMono9pt7b.h>

Adafruit_ILI9341 tft(15,2);

uint16_t exp_incr(uint16_t color, int expIncr) {
  uint16_t newColor = color;
  uint16_t tempColor = newColor;
  bool r=true, g=true, b=true;
  int green = 0;
  for(int i=0; i<expIncr; i++) {
    // Blue
    if(b) {
      for(int j=0; j<5; j++) {
        if((tempColor & 1<<j) == 0) {
          tempColor = tempColor | 1<<j; // 0->1
          break;
        }
        else if(j==4) {
          b = false;
          tempColor = newColor;
        } else tempColor = tempColor & ~(1<<j); // 1->0
      }
      if(b) newColor = tempColor;
    }
    // Green
    while(green < 2 && g) {
      for(int j=5; g && j<11; j++) {
        if((tempColor & 1<<j) == 0) {
          tempColor = tempColor | 1<<j; // 0->1
          break;
        }
        else if(j==10) {
          g = false;
          tempColor = newColor;
        } else tempColor = tempColor & ~(1<<j); // 1->0
      }
      if(g) newColor = tempColor;
      else break;
      green++;
    }
    green=0;
    // Red
    if(r) {
      for(int j=11; j<16; j++) {
        if((tempColor & 1<<j) == 0) {
          tempColor = tempColor | 1<<j; // 0->1
          break;
        }
        else if(j==15) {
          r = false;
          tempColor = newColor;
        } else tempColor = tempColor & ~(1<<j); // 1->0
      }
      if(r) newColor = tempColor;
    }
  }
  return newColor;
}

uint16_t exp_decr(uint16_t color, int expIncr) {
  uint16_t newColor = color;
  uint16_t tempColor = newColor;
  bool r=true, g=true, b=true;
  int green = 0;
  for(int i=0; i<expIncr; i++) {
    // Blue
    if(b) {
      for(int j=0; j<5; j++) {
        if((tempColor & 1<<j) != 0) {
          tempColor = tempColor & ~(1<<j); // 1->0
          break;
        }
        else if(j==4) {
          b = false;
          tempColor = newColor;
        } else tempColor = tempColor | 1<<j; // 0->1
      }
      if(b) newColor = tempColor;
    }
    // Green
    while(green < 2 && g) {
      for(int j=5; g && j<11; j++) {
        if((tempColor & 1<<j) != 0) {
          tempColor = tempColor & ~(1<<j); // 1->0
          break;
        }
        else if(j==10) {
          g = false;
          tempColor = newColor;
        } else tempColor = tempColor | 1<<j; // 0->1
      }
      if(g) newColor = tempColor;
      else break;
      green++;
    }
    green=0;
    // Red
    if(r) {
      for(int j=11; j<16; j++) {
        if((tempColor & 1<<j) != 0) {
          tempColor = tempColor & ~(1<<j); // 1->0
          break;
        }
        else if(j==15) {
          r = false;
          tempColor = newColor;
        } else tempColor = tempColor | 1<<j; // 0->1
      }
      if(r) newColor = tempColor;
    }
  }
  return newColor;
}

int sgn(int n) {
  if(n>0) {
    return 1;
  } else if(n<0) {
    return -1;
  }
  return 0;
}

uint16_t brightnessDecr(uint16_t color, int n) {
  int r=0, g=0, b=0;
  for(int i=0; i<5; i++) {
    b += sgn(color & 1<<i)*pow(2,i);
  }
  for(int i=5; i<11; i++) {
    g += sgn(color & 1<<i)*pow(2,i-5);
  }
  for(int i=11; i<16; i++) {
    r += sgn(color & 1<<i)*pow(2,i-11);
  }

  uint16_t newColor = color;

  for(int i=0; i<(b*n)/100; i++) {
    for(int j=0; j<5; j++) {
      if((newColor & 1<<j) != 0) {
        newColor = newColor & ~(1<<j); // 1->0
        break;
      } else newColor = newColor | 1<<j; // 0->1
    }
  }

  for(int i=0; i<(g*n)/100; i++) {
    for(int j=5; j<11; j++) {
      if((newColor & 1<<j) != 0) {
        newColor = newColor & ~(1<<j); // 1->0
        break;
      } else newColor = newColor | 1<<j; // 0->1
    }
  }
  for(int i=0; i<(r*n)/100; i++) {
    for(int j=11; j<16; j++) {
      if((newColor & 1<<j) != 0) {
        newColor = newColor & ~(1<<j); // 1->0
        break;
      } else newColor = newColor | 1<<j; // 0->1
    }
  }
  return newColor;
}

uint16_t exp_change(uint16_t color, int n) {
  if(n>0) {
    return exp_incr(color, n);
  } else if(n<0) {
    return exp_decr(color, -n);
  }
  return color;
}

uint16_t mixer(uint16_t color1, uint16_t color2, int mix) {
  int b2=0, g2=0, r2=0;
  for(int i=0; i<5; i++) {
    b2 += sgn(color2 & 1<<i)*pow(2,i);
  }
  for(int i=5; i<11; i++) {
    g2 += sgn(color2 & 1<<i)*pow(2,i-5);
  }
  for(int i=11; i<16; i++) {
    r2 += sgn(color2 & 1<<i)*pow(2,i-11);
  }

  uint16_t frontColor = brightnessDecr(color1, mix);
  
  for(int i=0; i<(b2*mix)/100; i++) {
    for(int j=0; j<5; j++) {
      if((frontColor & 1<<j) == 0) {
        frontColor = frontColor | 1<<j; // 0->1
        break;
      } else frontColor = frontColor & ~(1<<j); // 1->0
    }
  }
  for(int i=0; i<(g2*mix)/100; i++) {
    for(int j=5; j<11; j++) {
      if((frontColor & 1<<j) == 0) {
        frontColor = frontColor | 1<<j; // 0->1
        break;
      } else frontColor = frontColor & ~(1<<j); // 1->0
    }
  }
  for(int i=0; i<(r2*mix)/100; i++) {
    for(int j=11; j<16; j++) {
      if((frontColor & 1<<j) == 0) {
        frontColor = frontColor | 1<<j; // 0->1
        break;
      } else frontColor = frontColor & ~(1<<j); // 1->0
    }
  }
  return frontColor;
}

void Text1(int x, int y, int size, uint16_t color, char *text) {
  tft.setFont(&FreeSerif9pt7b);
  tft.setCursor(x,y);
  tft.setTextSize(size);
  tft.setTextColor(color);
  tft.print(text);
}
void Text2(int x, int y, int size, uint16_t color, char *text) {
  tft.setFont(&FreeMono9pt7b);
  tft.setCursor(x,y);
  tft.setTextSize(size);
  tft.setTextColor(color);
  tft.print(text);
}

void drawBitmapExp(int x, int y, int expIncr, const uint16_t *bitmap, int w, int h) {
  for(int i=0; i<h; i++) {
    for(int j=0; j<w; j++) {
      tft.drawPixel(x+j, y+i, exp_change(pgm_read_word(&bitmap[i*w+j]), expIncr));
    }
  }
}

void drawBitmapMix(int x, int y, int mix, const uint16_t *bitmap1, const uint16_t *bitmap2, int w, int h) {
  for(int i=0; i<h; i++) {
    for(int j=0; j<w; j++) {
      tft.drawPixel(x+j, y+i, mixer(pgm_read_word(&bitmap1[i*w+j]), pgm_read_word(&bitmap2[i*w+j]), mix));
      //tft.drawPixel(x+j, y+i, mixer(pgm_read_word(&bitmap1[i*w+j]), 0x7ff, mix));
    }
  }
}

float size;

void setup() {
  tft.begin();
  tft.fillScreen(0xFFFF);
  Serial.begin(9600);
}

void loop() {
  Text1(40, 20, 1, 0x000F, "Shree Radha Krishna");
  Serial.println("Printing data...");
  Serial.println(mixer(0xffff, 0x7ff, 25), BIN);
  drawBitmapMix(4,30, 60, Image1, Image2, IMAGE_WIDTH, IMAGE_HEIGHT);
  delay(2000);
  drawBitmapExp(4,30, 0, Image1, IMAGE_WIDTH, IMAGE_HEIGHT);
  delay(2000);
  drawBitmapExp(4,30, 0, Image2, IMAGE_WIDTH, IMAGE_HEIGHT);
  delay(2000);
  //Text2(10, 300, 1, 0x39E7, "File Size :");
  //size = IMAGE_WIDTH*IMAGE_HEIGHT*2;
  //tft.print(size/1024);
  //tft.print(" kB");
}