#include <Adafruit_ILI9341.h>
Adafruit_ILI9341 tft(10, 9);

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

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


void fillCircle(int x0, int y0, int r, uint16_t color) {
  for(int i=0; i<180; i++) {
    float rad = (i*3.14)/180;
    int x = r*cos(rad);
    int y = r*sin(rad);
    for(int j=-y; j<0; j++) {
      tft.drawPixel(x0+x,y0+j, exp_change(color, -(sq(0.55*x)+sq((0.75*45+j)))/50+7));
    }
    tft.drawPixel(x0+2*r+i, 240-y, 0xf800);
  }
}

void setup() {
  tft.begin();
  tft.fillScreen(0xffff);
}

void loop() {
  fillCircle(120, 160, 50, 0x7ff);

}