/* PyGamer/PyBadge Sprite Routines v2 - see http://www.technoblogy.com/show?33W0

   David Johnson-Davies - www.technoblogy.com - 13th August 2020
   For Adafruit PyGamer/PyBadge ST7735 TFT displays
   
   CC BY 4.0
   Licensed under a Creative Commons Attribution 4.0 International license: 
   http://creativecommons.org/licenses/by/4.0/
*/

#include <Adafruit_GFX.h>    // Core graphics library
#include <Adafruit_ST7735.h> // Hardware-specific library for ST7735
#include <SPI.h>
#include <Wire.h>

// Adafruit PyBadge/PyGamer
#define TFT_CS        44  // Chip select
#define TFT_RST       46  // Display reset
#define TFT_DC        45  // Display data/command select
#define TFT_BACKLIGHT 47  // Display backlight pin
#define TFT_MOSI      41  // Data out
#define TFT_SCLK      42  // Clock out

class Technoblogy_ST7735 : public Adafruit_ST7735 {       
public:
  Technoblogy_ST7735(int8_t cs, int8_t dc, int8_t mosi, int8_t sclk, int8_t rst);
  uint16_t getPixel(uint16_t x, uint16_t y);
  void xorPixel(uint16_t x, uint16_t y, uint16_t color);
  void xorSprite(uint16_t x, uint16_t y, uint64_t sprite, uint16_t color);
  bool hitSprite(uint16_t x, uint16_t y, uint64_t sprite, uint16_t color);
  void moveSprite(uint16_t x, uint16_t y, uint64_t sprite, int dx, int dy, uint16_t color);
};

Technoblogy_ST7735::Technoblogy_ST7735(int8_t cs, int8_t dc, int8_t mosi, int8_t sclk, int8_t rst)
    : Adafruit_ST7735(cs, dc, mosi, sclk, rst) {}

Technoblogy_ST7735 tft = Technoblogy_ST7735(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);

// Return the colour of the pixel at x, y
uint16_t Technoblogy_ST7735::getPixel (uint16_t x, uint16_t y) {
  uint32_t ret = 0;
  startWrite();
  setAddrWindow(x, y, 1, 1);
  writeCommand(ST77XX_RAMRD);
  pinMode(TFT_MOSI, INPUT);
  pinMode(TFT_SCLK, OUTPUT);
  for (int i=0; i<33; i++) {
    digitalWrite(TFT_SCLK, HIGH);
    ret = ret<<1 | digitalRead(TFT_MOSI);
    digitalWrite(TFT_SCLK, LOW);
  }
  pinMode(TFT_MOSI, OUTPUT);
  endWrite();
  return ((ret & 0xf80000)>>8 | (ret & 0xfc00)>>5 | (ret & 0xf8)>>3);
}

// Plot a pixel at x, y by exclusive-ORing the colour with the display
void Technoblogy_ST7735::xorPixel (uint16_t x, uint16_t y, uint16_t color) {
  uint16_t lastcolor = getPixel(x, y);
  drawPixel(x, y, color ^ lastcolor);
}

// Plot an 8x8 sprite at x, y by exclusive-ORing the colour with the display
void Technoblogy_ST7735::xorSprite (uint16_t x0, uint16_t y0, uint64_t sprite, uint16_t color) {
  bool bit;
  if ((x0 >= 0) && (x0+7 < width()) && (y0 >= 0) && (y0+7 < height())) {
    for (int y=0; y<8; y++) {
      for (int x=0; x<8; x++) {
        bit = sprite>>(63 - x - y*8) & 1;
        if (bit) xorPixel(x0+x, y0+y, color);
      }
    }
  }
}

// Collision detection between an 8x8 sprite at x, y with a specified colour
bool Technoblogy_TFT::hitSprite (uint16_t x0, uint16_t y0, uint64_t sprite, uint16_t color) {
  uint16_t row[8];
  uint32_t col16;
  bool bit;
  if ((x0 >= 0) && (x0+7 < width()) && (y0 >= 0) && (y0+7 < height())) {
    for (int y=0; y<8; y++) {
      for (int x=0; x<8; x++) {
        col16 = readPixel(x0+x,y0+y);
        bit = sprite>>(63 - x - y*8) & 1;
        if (bit && (col16 == color)) {
          return true;
        }
      } 
    }
  }
  return false;
}

// Move a sprite from x, y by one pixel in any direction by exclusive-ORing only the changed pixels with the display
void Technoblogy_ST7735::moveSprite(uint16_t x0, uint16_t y0, uint64_t sprite, int dx, int dy, uint16_t color) {
  int oldbit, newbit;
  if ((x0 >= 0) && (x0+7 < width()) && (y0 >= 0) && (y0+7 < height())) {
    dx = (dx > 0) - (dx < 0); dy = (dy > 0) - (dy < 0); 
    for (int y=0; y<10; y++) {
      for (int x=0; x<10; x++) {
        // Sprite's previous position
        int xs = x - 1, ys = y - 1;
        if (xs >= 0 && xs <= 7 && ys >=0 && ys <= 7) {
          oldbit = sprite>>(63 - ys*8 - xs) & 1;
        } else oldbit = 0;
        // Sprite's new position
        int xn = x - 1 - dx, yn = y - 1 - dy;
        if (xn >= 0 && xn <= 7 && yn >=0 && yn <= 7) {
          newbit = sprite>>(63 - yn*8 - xn) & 1;
        } else newbit = 0;
        if (oldbit != newbit) xorPixel(x0-1+x, y0-1+y, color);
      }
    }
  }
}

// Simple rolling ball maze using the PyGamer/PyBadge accelerometer **********************************************

// Accelerometer (LIS3DH)

const int LIS3DH = 0x19;

// Set sample rate; n = 2 is 10Hz
void lis3dhRate (int n) {
  Wire.beginTransmission(LIS3DH);
  Wire.write(0x20);
  Wire.write(n<<4 | 7);
  Wire.endTransmission(true);
}

// Read register; r = 0 is X, 1 is Y, 2 is Z
int lis3dhXYZ (int r) {
  Wire.beginTransmission(LIS3DH);
  Wire.write(0x28 + 2*r + 0x80);
  Wire.endTransmission(false);
  Wire.requestFrom(LIS3DH, 4);
  uint8_t lo = Wire.read();
  uint8_t hi = Wire.read();
  int d = lo | hi<<8;
  return d - ((d & 0x8000)<<1);
}

// The rolling ball maze

const int Sens = 1024; // Accelerometer sensitivity
const uint16_t Maze = ST77XX_MAGENTA;
const uint16_t Ball = ST77XX_WHITE;
const uint16_t Board = ST77XX_BLACK;
const uint64_t Sprite = 0b\
00111100\
01111110\
11111111\
11111111\
11111111\
11111111\
01111110\
00111100;

void setup() {
  tft.initR(INITR_BLACKTAB);
  tft.setRotation(1);
  pinMode(TFT_BACKLIGHT, OUTPUT);
  digitalWrite(TFT_BACKLIGHT, HIGH);
  tft.fillScreen(ST77XX_BLACK);
  // Draw ball maze
  for (int c=4; c>=0; c--) {
    tft.fillCircle(80, 64, (c*14+7), Maze);
    if (c) tft.fillCircle(80, 64, (c*14+5), Board);
  }
  // Gaps
  tft.fillCircle(80, 112, 6, Board);
  tft.fillCircle(32, 64, 6, Board);
  tft.fillCircle(56, 88, 6, Board);
  tft.fillCircle(94, 50, 6, Board);
  Wire.begin();
  lis3dhRate(2);
}

void loop() {
  int x = 77, y = 115, dx, dy, x1, y1;
  tft.xorSprite(x, y, Sprite, Ball);
  for(;;) {
    // Move ball?
    int sx = lis3dhXYZ(0) / Sens;
    int sy = lis3dhXYZ(1) / Sens;
    // Set dx and dy to -1, 0, or +1
    dx = (sx > 0) - (sx < 0);
    dy = (sy > 0) - (sy < 0);
    if (tft.hitSprite(x+dx, y, Sprite, Maze)) dx = 0;
    if (tft.hitSprite(x, y+dy, Sprite, Maze)) dy = 0;
    tft.moveSprite(x, y, Sprite, dx, dy, Ball);
    x = x + dx; y = y + dy;
    delay(10);
  }
}