#include <Wire.h>
#include "TFTLCD.h"
#include "TouchScreen.h"
#include <SD.h>

#define YP A3  // must be an analog pin, use "An" notation!
#define XM A2  // must be an analog pin, use "An" notation!
#define YM 9   // can be a digital pin
#define XP 8   // can be a digital pin

#define TS_MINX 150
#define TS_MINY 120
#define TS_MAXX 920
#define TS_MAXY 940

int xPos, yPos, zPos;             // home
int offset = 3;                   // fuzzy
static uint8_t nunchuck_buf[6];   // array to store nunchuck data,

// For better pressure precision, we need to know the resistance
// between X+ and X- Use any multimeter to read it
// For the one we're using, its 300 ohms across the X plate
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);

// The control pins can connect to any pins but we'll use the 
// analog lines since that means we can double up the pins
// with the touch screen (see the TFT paint example)
#define LCD_CS A3    // Chip Select goes to Analog 3
#define LCD_CD A2    // Command/Data goes to Analog 2
#define LCD_WR A1    // LCD Write goes to Analog 1
#define LCD_RD A0    // LCD Read goes to Analog 0
/* For the 8 data pins:
Duemilanove/Diecimila/UNO/etc ('168 and '328 chips) microcontoller:
D0 connects to digital 8
D1 connects to digital 9
D2 connects to digital 2
D3 connects to digital 3
D4 connects to digital 4
D5 connects to digital 5
D6 connects to digital 6
D7 connects to digital 7

For Mega's use pins 22 thru 29 (on the double header at the end)
*/

// For Arduino Uno/Duemilanove, etc
//  connect the SD card with DI going to pin 11, DO going to pin 12 and SCK going to pin 13 (standard)
//  Then pin 10 goes to CS (or whatever you have set up)
#define SD_CS 10     // Set the chip select line to whatever you use (10 doesnt conflict with the library)

// In the SD card, place 24 bit color BMP files (be sure they are 24-bit!)
// There are examples in the sketch folder

// optional
#define LCD_RESET A4

// Color definitions
#define	BLACK               0 //0x0000
#define	BLUE               31 //0x001F
#define	GREEN            2016 //0x07E0
#define CYAN             2047 //0x07FF
#define	RED             63488 //0xF800
#define MAGENTA         63519 //0xF81F
#define YELLOW          65504 //0xFFE0 
#define WHITE           65535 //0xFFFF

// our TFT wiring
TFTLCD tft(LCD_CS, LCD_CD, LCD_WR, LCD_RD, LCD_RESET);

// information we extract about the bitmap file
int penRadius, bmpWidth, bmpHeight, oldcolor, currentcolor, c;
uint8_t bmpDepth, bmpImageoffset;


int pageWidth = 320;
int pageHeight = 240;
int numBoxesRow = 8;
int numBoxesCol = 8;
int boxHeight = 40;
int boxWidth = pageHeight/numBoxesCol;

// the file itself
File bmpFile;

#define MINPRESSURE 10
#define MAXPRESSURE 1000
boolean paused = false;
int colors[8] = {0, 31, 2016, 2047, 63488, 63519, 65504, 65535};
const int numF = 10;
int Fy[numF];
int Fx[numF]; 
int Fw[numF]; 
int Fc[numF]; 
int y;
void setup()
{
  randomSeed(analogRead(0));

  Serial.begin(115200);
  Serial.println("Little boxes made of ticky-tacky\n\nBy:\ncbwp.net!\n\n");
 
  tft.reset();
  
  // find the TFT display
  uint16_t identifier = tft.readRegister(0x0);
  if (identifier == 0x9325) {
    Serial.println("Found ILI9325");
  } else if (identifier == 0x9328) {
    Serial.println("Found ILI9328");
  } else {
    Serial.print("Unknown driver chip ");
    Serial.println(identifier, HEX);
    while (1);
  }  
 
  tft.initDisplay();
  Serial.print("Initializing SD card...");
  pinMode(10, OUTPUT);

/*
  if (!SD.begin(10)) {
    Serial.println("SD Init failed!");
  } else {
    Serial.println("SD OK!");
  }
  */
  /*
    bmpFile = SD.open("ziva.bmp");

    if (! bmpFile) {
      Serial.println("didnt find image");
    } else if (! bmpReadHeader(bmpFile)) { 
       Serial.println("bad bmp");
    } else {
      Serial.print("image size "); 
      Serial.print(bmpWidth, DEC);
      Serial.print(", ");
      Serial.println(bmpHeight, DEC);
 
      tft.fillScreen(BLACK);
      bmpdraw(bmpFile, 0, 0);
      bmpFile.close();
      delay(2000);
    }
  }
  */
  
  tft.fillScreen(BLACK);
/* dont put out my menu now we are hacking some basics first  
  //where 0,0 = top left and width = 240
  drawBox();
  paletteBox();
 
  tft.drawRect(0, 0, boxWidth, boxHeight, WHITE);
  currentcolor = RED;
  */
//tft.drawPixel(120, 160, 1234);  // unique color to find = 1234 (hex) it seems to ba a cyanish color
//tft.drawPixel(121, 160, 1234);  // unique color to find = 1234 (hex) it seems to ba a cyanish color
//tft.drawPixel(120, 161, 1234);  // unique color to find = 1234 (hex) it seems to ba a cyanish color
//tft.drawPixel(121, 161, 1234);  // unique color to find = 1234 (hex) it seems to ba a cyanish color
//int cRGB = tft.Color565(255, 165, 00);//orange rgb(FF, A5, 00)
//tft.fillRect(110, 150, 20, 20, cRGB);
  tft.drawPixel(110, 150, 10345);
  tft.drawPixel(111, 150, 11345);
  tft.drawPixel(110, 151, 12345);
  tft.drawPixel(111, 151, 13345);

  
  int c1 = getPixel(110, 150);
  int c2 = getPixel(111, 150);
  int c3 = getPixel(110, 151);
  int c4 = getPixel(111, 151);
//  tft.writeRegister(0x0022, 65535); // GRAM Address Set (Horizontal Address) (R20h)
/*
  SD.remove("ziva.txt");
  bmpFile = SD.open("ziva.txt", FILE_WRITE);

  // if the file opened okay, write to it:
  if (bmpFile) {
    Serial.print("Writing to ziva.txt...");
    bmpFile.println(c1);
    bmpFile.println(c2);
    bmpFile.println(c3);
    bmpFile.println(c4);
	// close the file:
    bmpFile.close();
    Serial.println("done.");
  } else {
    // if the file didn't open, print an error:
    Serial.println("error opening ziva.txt");
  }
*/  
  Serial.print(c1);
  Serial.print(", ");
  Serial.print(c2);
  Serial.print(", ");
  Serial.print(c3);
  Serial.print(", ");
  Serial.println(c4);

  delay(1000);
  tft.fillScreen(BLACK);
/*
  // re-open the file for reading:
  bmpFile = SD.open("ziva.txt");
  if (bmpFile) {
    Serial.println("ziva.txt:");
    
    // read from the file until there's nothing else in it:
    int place = 0; 
    int xP, yP, cP;
    
    while (bmpFile.available()) {
      
      if(place == 0){
        int xP = 110;
        int yP = 150;
        place++;
      } else if(place == 1){
        int xP = 111;
        int yP = 150;
        place++;
      } else if(place == 2){
        int xP = 110;
        int yP = 151;
        place++;
      } else if(place == 3){
        int xP = 111;
        int yP = 151;
        place++;
      } else {
        int xP = 0;
        int yP = 0;
      }
     int lColor = bmpFile.read();
//      Serial.print(place);
//      Serial.print(", ");
     cP = lColor - 48;
     if(place == 6){
       Serial.println(cP);
       place = 0;
     } else {
      Serial.print(cP);
      place++;
     }
     // tft.drawPixel(xP, yP, lColor);
    }
    // close the file:
    bmpFile.close();
  } else {
  	// if the file didn't open, print an error:
    Serial.println("error opening ziva.txt");
  }
*/
  pinMode(11, INPUT); // ir in
  pinMode(13, OUTPUT);
  pinMode(19, INPUT); // get penRadius from analog input

  for(int i=0;i<numF;i++){ 
    another(i);
  }
  
  nunchuck_init();     // get to i2c
  delay(100);
  nunchuck_get_data();
  static int i=0;
  int joy_x_axis = nunchuck_buf[0];
  int joy_y_axis = nunchuck_buf[1];
  int accel_x_axis = nunchuck_buf[2]; // * 2 * 2; 
  int accel_y_axis = nunchuck_buf[3]; // * 2 * 2;
  int accel_z_axis = nunchuck_buf[4]; // * 2 * 2;

  int z_button = 0;
  int c_button = 0;

  // byte nunchuck_buf[5] contains bits for z and c buttons
  // it also contains the least significant bits for the accelerometer data
  // so we have to check each bit of byte outbuf[5]
  if ((nunchuck_buf[5] >> 0) & 1) 
    z_button = 1;
  if ((nunchuck_buf[5] >> 1) & 1)
    c_button = 1;

  if ((nunchuck_buf[5] >> 2) & 1) 
    accel_x_axis += 2;
  if ((nunchuck_buf[5] >> 3) & 1)
    accel_x_axis += 1;

  if ((nunchuck_buf[5] >> 4) & 1)
    accel_y_axis += 2;
  if ((nunchuck_buf[5] >> 5) & 1)
    accel_y_axis += 1;

  if ((nunchuck_buf[5] >> 6) & 1)
    accel_z_axis += 2;
  if ((nunchuck_buf[5] >> 7) & 1)
    accel_z_axis += 1;


  Serial.print(i,DEC);
  Serial.print("\t");
  Serial.print("home:");
  Serial.print(accel_x_axis, DEC);
  Serial.print(",");
  Serial.print(accel_y_axis, DEC);
  Serial.print(",");
  Serial.println(accel_z_axis, DEC);
  i++;
  xPos = accel_x_axis;
  yPos = accel_y_axis;
  zPos = accel_z_axis;
  
  Serial.print ("Finished setup\n");
}
int getPixel(int x, int y){
  tft.writeRegister(0x0020, x); // GRAM Address Set (Horizontal Address) (R20h)
  tft.writeRegister(0x0021, y); // GRAM Address Set (Horizontal Address) (R20h)
  int junk = tft.readRegister(0x0022);
  int c = tft.readRegister(0x0022);
  return c;
}

void loop(){
  nunchuck_get_data();
  nunchuck_print_data();
  if(!digitalRead(11)){
    tft.fillScreen(BLACK);
  }
  for(int i=0;i<numF;i++){ // this makes random squares each loop
    another(i);
  }
  
  for(int j=0;j<numF;j++){ 
    tft.drawRect(Fx[j], Fy[j], Fw[j], Fw[j], Fc[j]); // fill or draw squares
    delay(50);
    //debug...
    //Serial.print(Fx[j]);Serial.print(",");Serial.print(Fy[j]);Serial.print(" w=");Serial.print(Fw[j]);Serial.print(" h=");Serial.print(Fw[j]);Serial.print(" c=");Serial.println(Fc[j]);
    int nextX = Fx[j]+1;
    if(nextX == 240){
      another(j);
    } else {
      Fx[j] = nextX;
    }
  }
}
void another(int i){
  boolean searching = true;
  while(searching){
    y = random(320);
    for(int j=0;j<numF;j++){
      if(Fy[j] == y){
        break;
      }else{
        searching = false;
      }
    }
  } 
  Fy[i] = y;
  Fx[i] = random(240);
  Fw[i] = random(25);
  Fc[i] = colors[random(8)];
}
void nullCrap() // ignore my old code for some hacking...
{
  digitalWrite(13, HIGH);
  Point p = ts.getPoint();
  digitalWrite(13, LOW);

  // if you're sharing pins, you'll need to fix the directions of the touchscreen pins!
  //pinMode(XP, OUTPUT);
  pinMode(XM, OUTPUT);
  pinMode(YP, OUTPUT);
  //pinMode(YM, OUTPUT);

  // we have some minimum pressure we consider 'valid'
  // pressure of 0 means no pressing!

  if (p.z > MINPRESSURE && p.z < MAXPRESSURE) {
    /*
    Serial.print("X = "); Serial.print(p.x);
    Serial.print("\tY = "); Serial.print(p.y);
    Serial.print("\tPressure = "); Serial.println(p.z);
    */
    if (p.y < (TS_MINY-5)) {
      Serial.println("erase");
     // press the bottom of the screen to erase 
      tft.fillRect(0,  boxHeight, tft.width(), tft.height()-boxHeight, BLACK);
      drawBox();
    }
    // turn from 0->1023 to tft.width
    p.x = map(p.x, TS_MINX, TS_MAXX, tft.width(), 0);
    p.y = map(p.y, TS_MINY, TS_MAXY, tft.height(), 0);
    /*
    Serial.print("("); Serial.print(p.x);
    Serial.print(", "); Serial.print(p.y);
    Serial.println(")");
    */
    boolean changed = false;
    if ((p.x < boxWidth*1) && (p.y < boxHeight*8) && (p.y > boxHeight*7)){
      paused = !paused;  delay(200);  changed = true;
    }
    if (paused){
      if(changed){
        pauseBox();
      }
    } else if (p.y < boxHeight) {
      oldcolor = currentcolor;
      paletteBox();

      if (p.x < boxWidth*1) { 
        currentcolor = RED; 
        tft.drawRect(boxWidth*0, 0, boxWidth, boxHeight, WHITE);
      } else if (p.x < boxWidth*2) {
        currentcolor = YELLOW; 
        tft.drawRect(boxWidth*1, 0, boxWidth, boxHeight, WHITE);
      } else if (p.x < boxWidth*3) {
        currentcolor = GREEN; 
        tft.drawRect(boxWidth*2, 0, boxWidth, boxHeight, WHITE);
      } else if (p.x < boxWidth*4) {
        currentcolor = CYAN; 
        tft.drawRect(boxWidth*3, 0, boxWidth, boxHeight, WHITE);
      } else if (p.x < boxWidth*5) {
        currentcolor = BLUE; 
        tft.drawRect(boxWidth*4, 0, boxWidth, boxHeight, WHITE);
      } else if (p.x < boxWidth*6) {
        currentcolor = MAGENTA; 
        tft.drawRect(boxWidth*5, 0, boxWidth, boxHeight, WHITE);
      } else if (p.x < boxWidth*7) {
        currentcolor = BLACK; 
        tft.drawRect(boxWidth*6, 0, boxWidth, boxHeight, WHITE);
      } else if (p.x < boxWidth*8) {
        currentcolor = WHITE; 
        tft.drawRect(boxWidth*7, 0, boxWidth, boxHeight, RED);
      }
    } else {
      penRadius =  analogRead(5);
      penRadius = map(penRadius, 0, 1023, 1, 5);

      if (((p.y-penRadius) > boxHeight) && ((p.y+penRadius) < tft.height())) {
        tft.fillCircle(p.x, p.y, penRadius, currentcolor);
      }
      if (changed){
        drawBox();
      }
    }
  }
}

void paletteBox(){
  tft.fillRect(boxWidth*0, 0, boxWidth, boxHeight, RED);
  tft.fillRect(boxWidth*1, 0, boxWidth, boxHeight, YELLOW);
  tft.fillRect(boxWidth*2, 0, boxWidth, boxHeight, GREEN);
  tft.fillRect(boxWidth*3, 0, boxWidth, boxHeight, CYAN);
  tft.fillRect(boxWidth*4, 0, boxWidth, boxHeight, BLUE);
  tft.fillRect(boxWidth*5, 0, boxWidth, boxHeight, MAGENTA);
  tft.fillRect(boxWidth*6, 0, boxWidth, boxHeight, BLACK);
  tft.fillRect(boxWidth*7, 0, boxWidth, boxHeight, WHITE);
}
void drawBox(){
  tft.drawRect(boxWidth*0+0, boxHeight*7+0, boxWidth-0, boxHeight-0, GREEN);
  tft.fillRect(boxWidth*0+1, boxHeight*7+1, boxWidth-2, boxHeight-2, BLACK);
  tft.drawRect(boxWidth*0+2, boxHeight*7+2, boxWidth-4, boxHeight-4, GREEN);
  tft.setRotation(3);
  tft.setCursor(10, 12);
  tft.setTextColor(GREEN);
  tft.setTextSize(1);
  tft.println("DRAW");
  tft.setRotation(0);
}
void pauseBox(){
  tft.drawRect(boxWidth*0+0, boxHeight*7+0, boxWidth-0, boxHeight-0, RED);
  tft.fillRect(boxWidth*0+1, boxHeight*7+1, boxWidth-2, boxHeight-2, BLACK);
  tft.drawRect(boxWidth*0+2, boxHeight*7+2, boxWidth-4, boxHeight-4, RED);
  tft.setRotation(3);
  tft.setCursor(5, 12);
  tft.setTextColor(RED);
  tft.setTextSize(1);
  tft.println("PAUSE");
  tft.setRotation(0);
}
/*********************************************/
// This procedure reads a bitmap and draws it to the screen
// its sped up by reading many pixels worth of data at a time
// instead of just one pixel at a time. increading the buffer takes
// more RAM but makes the drawing a little faster. 20 pixels' worth
// is probably a good place

#define BUFFPIXEL 20

void bmpdraw(File f, int x, int y) {
  bmpFile.seek(bmpImageoffset);
  
  uint32_t time = millis();
  uint16_t p;
  uint8_t g, b;
  int i, j;
  
  uint8_t sdbuffer[3 * BUFFPIXEL];  // 3 * pixels to buffer
  uint8_t buffidx = 3*BUFFPIXEL;
  
  Serial.print("rotation = "); Serial.println(tft.getRotation(), DEC);
  
  for (i=0; i< bmpHeight; i++) {
    // bitmaps are stored with the BOTTOM line first so we have to move 'up'

    if (tft.getRotation() == 3) {
      tft.goTo(x, y+bmpHeight-i); 
    } else if  (tft.getRotation() == 2) {
      tft.goTo(x+i, y);
    } else if  (tft.getRotation() == 1) {
        tft.goTo(x+bmpWidth-i, y+bmpHeight); 
    } else if  (tft.getRotation() == 0) {
      tft.goTo(x+bmpWidth, y+i); 
    }
    
    for (j=0; j<bmpWidth; j++) {
      // read more pixels
      if (buffidx >= 3*BUFFPIXEL) {
        bmpFile.read(sdbuffer, 3*BUFFPIXEL);
        buffidx = 0;
      }
      
      // convert pixel from 888 to 565
      b = sdbuffer[buffidx++];     // blue
      g = sdbuffer[buffidx++];     // green
      p = sdbuffer[buffidx++];     // red
      
      p >>= 3;
      p <<= 6;
      
      g >>= 2;
      p |= g;
      p <<= 5;
      
      b >>= 3;
      p |= b;
     
       // write out the 16 bits of color
      tft.writeData(p);
    }
  }
  Serial.print(millis() - time, DEC);
  Serial.println(" ms");
}

boolean bmpReadHeader(File f) {
   // read header
  uint32_t tmp;
  
  if (read16(f) != 0x4D42) {
    // magic bytes missing
    return false;
  }
 
  // read file size
  tmp = read32(f);  
  Serial.print("size 0x"); Serial.println(tmp, HEX);
  
  // read and ignore creator bytes
  read32(f);
  
  bmpImageoffset = read32(f);  
  Serial.print("offset "); Serial.println(bmpImageoffset, DEC);
  
  // read DIB header
  tmp = read32(f);
  Serial.print("header size "); Serial.println(tmp, DEC);
  bmpWidth = read32(f);
  bmpHeight = read32(f);

  
  if (read16(f) != 1)
    return false;
    
  bmpDepth = read16(f);
  Serial.print("bitdepth "); Serial.println(bmpDepth, DEC);

  if (read32(f) != 0) {
    // compression not supported!
    return false;
  }
  
  Serial.print("compression "); Serial.println(tmp, DEC);

  return true;
}

/*********************************************/

// These read data from the SD card file and convert them to big endian 
// (the data is stored in little endian format!)

// LITTLE ENDIAN!
uint16_t read16(File f) {
  uint16_t d;
  uint8_t b;
  b = f.read();
  d = f.read();
  d <<= 8;
  d |= b;
  return d;
}


// LITTLE ENDIAN!
uint32_t read32(File f) {
  uint32_t d;
  uint16_t b;
 
  b = read16(f);
  d = read16(f);
  d <<= 16;
  d |= b;
  return d;
}

//
// Nunchuck functions
//


/*
// Uses port C (analog in) pins as power & ground for Nunchuck
static void nunchuck_setpowerpins()
{
#define pwrpin PORTC3
#define gndpin PORTC2
    DDRC |= _BV(pwrpin) | _BV(gndpin);
    PORTC &=~ _BV(gndpin);
    PORTC |=  _BV(pwrpin);
    delay(100);  // wait for things to stabilize        
}
*/

// initialize the I2C system, join the I2C bus,
// and tell the nunchuck we're talking to it
void nunchuck_init()
{ 
  Wire.begin();	                // join i2c bus as master
  Wire.beginTransmission(0x52);	// transmit to device 0x52
  Wire.send(0x40);		// sends memory address
  Wire.send(0x00);		// sends sent a zero.  
  Wire.endTransmission();	// stop transmitting
}

// Send a request for data to the nunchuck
// was "send_zero()"
void nunchuck_send_request()
{
  Wire.beginTransmission(0x52);	// transmit to device 0x52
  Wire.send(0x00);		// sends one byte
  Wire.endTransmission();	// stop transmitting
}

// Receive data back from the nunchuck, 
int nunchuck_get_data()
{
    int cnt=0;
    Wire.requestFrom (0x52, 6);	// request data from nunchuck
    while (Wire.available ()) {
      // receive byte as an integer
      nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.receive());
      cnt++;
    }
    nunchuck_send_request();  // send request for next data payload
    // If we recieved the 6 bytes, then go print them
    if (cnt >= 5) {
     return 1;   // success
    }
    return 0; //failure
}

// Print the input data we have recieved
// accel data is 10 bits long
// so we read 8 bits, then we have to add
// on the last 2 bits.  That is why I
// multiply them by 2 * 2
void nunchuck_print_data()
{ 
  static int i=0;
  int joy_x_axis = nunchuck_buf[0];
  int joy_y_axis = nunchuck_buf[1];
  int accel_x_axis = nunchuck_buf[2]; // * 2 * 2; 
  int accel_y_axis = nunchuck_buf[3]; // * 2 * 2;
  int accel_z_axis = nunchuck_buf[4]; // * 2 * 2;

  int z_button = 0;
  int c_button = 0;

  // byte nunchuck_buf[5] contains bits for z and c buttons
  // it also contains the least significant bits for the accelerometer data
  // so we have to check each bit of byte outbuf[5]
  if ((nunchuck_buf[5] >> 0) & 1) 
    z_button = 1;
  if ((nunchuck_buf[5] >> 1) & 1)
    c_button = 1;

  if ((nunchuck_buf[5] >> 2) & 1) 
    accel_x_axis += 2;
  if ((nunchuck_buf[5] >> 3) & 1)
    accel_x_axis += 1;

  if ((nunchuck_buf[5] >> 4) & 1)
    accel_y_axis += 2;
  if ((nunchuck_buf[5] >> 5) & 1)
    accel_y_axis += 1;

  if ((nunchuck_buf[5] >> 6) & 1)
    accel_z_axis += 2;
  if ((nunchuck_buf[5] >> 7) & 1)
    accel_z_axis += 1;

  Serial.print(i,DEC);
  Serial.print("\t");
  
  Serial.print("joy:");
  Serial.print(joy_x_axis,DEC);
  Serial.print(",");
  Serial.print(joy_y_axis, DEC);
  Serial.print("  \t");

  Serial.print("acc:");
  Serial.print(accel_x_axis, DEC);
  Serial.print(",");
  Serial.print(accel_y_axis, DEC);
  Serial.print(",");
  Serial.print(accel_z_axis, DEC);
  Serial.print("\t");

  Serial.print("but:");
  Serial.print(z_button, DEC);
  Serial.print(",");
  Serial.print(c_button, DEC);

  Serial.print("\r\n");  // newline
  i++;
  if((accel_x_axis < xPos - offset) || (accel_y_axis < yPos - offset) || (accel_z_axis < zPos - offset) || (accel_x_axis > xPos + offset) || (accel_y_axis > yPos + offset) || (accel_z_axis > zPos + offset)){
    digitalWrite(13, HIGH);
    delay(20);
    digitalWrite(13, LOW);
    delay(20);
  }
}

// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
char nunchuk_decode_byte (char x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}