#include <SD.h>
#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"

#define TFT_DC 9
#define TFT_CS 8

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

double  posy = 240 / 2;
double  posx = 320 / 2;
double  newposy;
double  newposx;
#define SPI_SPEED SD_SCK_MHZ(4)
#define CS_PIN 10
int bit1 = 0;
int bit2 = 0;
int bit3 = 0;

double conversion = 4.0;

bool  	normalStitch = false;
bool  	jumpStitch = false;
bool  	colorChange = false;
bool  	sequinMode = false;

char *  buf = (char *) malloc (3);

File dataFile;
void setup() {
  tft.begin();


  Serial.begin(115200);
  if (!SD.begin(10)) {
    Serial.println("initialization failed!");
    return;
  }


  //String filename = "cuadros.dst";

  String filename = "320x240.DST";

  dataFile = SD.open( filename, FILE_READ);
  int count = 0;
  if (dataFile) {
    Serial.println( filename);
    dataFile.seek(512);

    while (dataFile.available() && count < 345678 ) {   // the count is just for test
      count++;

      bit1 = dataFile.read();
      bit2 = dataFile.read();
      bit3 = dataFile.read();
      Serial.print(padzeros3(count));




      Serial.print(" 1:");
      printBin(bit1);
      Serial.print(" 2:");
      printBin(bit2);
      Serial.print(" 3:");
      printBin(bit3);

      Serial.print("\tY:");
      Serial.print (Y_collect(bit1, bit2, bit3 ));

      Serial.print("\tX:");
      Serial.print (X_collect(bit1, bit2, bit3 ));

      Serial.print("\tSet:");
      Serial.print(padzeros2( set_collect (bit1, bit2, bit3) ));

      control_codes_collect(bit1, bit2, bit3 );


      Serial.print(" normal=");
      Serial.print(normalStitch);

      Serial.print(" jump =");
      Serial.print(jumpStitch );

      Serial.print(" color=");
      Serial.print(colorChange);

      Serial.print(" sequin=" );
      Serial.print(sequinMode );




      newposy = posy + (Y_collect(bit1, bit2, bit3  ) );
      newposx = posx + (X_collect(bit1, bit2, bit3  ) );

      if (jumpStitch)
      {
       //tft.drawPixel(newposy, newposx, ILI9341_BLACK); 
      tft.drawLine(posy, posx, newposy, newposx, ILI9341_RED);
      }
      if (normalStitch)
      {
      //tft.drawPixel(newposy, newposx, ILI9341_RED); 
      tft.drawLine(posy, posx, newposy, newposx, ILI9341_GREEN);
      }
      posy = newposy;
      posx = newposx;
      Serial.println();
   
    }



  }
  dataFile.close();
}

void loop() {
  Serial.print("END");
  while (1);
}


void control_codes_collect (int bit1_1, int bit2_2, int bit3_3)
{
  byte control_codes_mask = 0B11000000;


  int control_codes = bit3_3 & control_codes_mask;

  control_codes == (0 )   ?   normalStitch = true : normalStitch = false;
  control_codes == (64 )  ?   sequinMode =   true : sequinMode =  false;
  control_codes == (128 ) ?   jumpStitch =   true :  jumpStitch = false;
  control_codes == (192 ) ?   colorChange =  true : colorChange = false;

}




void printBin(byte aByte) {
  for (int8_t aBit = 7; aBit >= 0; aBit--)
    Serial.write(bitRead(aByte, aBit) ? '1' : '0');
}




int set_collect (int bit1_1, int bit2_2, int bit3_3)
{
  int set_mask = 0B00000011;
  int return_set = bit3_3 & set_mask;
  return return_set;
}



double Y_collect(int bit1_1, int bit2_2, int bit3_3)
{
 double  yreturn;
  // byte 1
  yreturn += (bitRead(bit1_1 ,1) * (+1 ));
  yreturn += (bitRead(bit1_1 ,0) * (-1));
  yreturn += (bitRead(bit1_1 , 5) * (9));
  yreturn += (bitRead(bit1_1 , 4) * (-9));
  // byte 2
  yreturn += (bitRead(bit2_2,  7) * (3));
  yreturn += (bitRead(bit2_2 , 6) *(-3));
  yreturn += (bitRead(bit2_2 , 5) * (27));
  yreturn +=( bitRead(bit2_2 , 4) *(-27));
  // byte 3
  yreturn += (bitRead(bit3_3 , 5) * (81));
  yreturn +=( bitRead(bit3_3 , 4) * (-81));

 return yreturn / 1;//conversion;
}

double X_collect(int bit1_1, int bit2_2, int bit3_3)
{
double  xreturn;

// byte 1
  xreturn += bitRead(bit1_1 , 0) * (+1 );
  xreturn += bitRead(bit1_1 , 1) * (-1);
  xreturn += bitRead(bit1_1 , 2) * (9);
  xreturn += bitRead(bit1_1 , 3) * (-9);
  // byte 2
  xreturn += bitRead(bit2_2,  0) * (3);
  xreturn += bitRead(bit2_2 , 1) *(-3);

  xreturn += bitRead(bit2_2 , 2) * (27);
  xreturn += bitRead(bit2_2 , 3) *(-27);
  // byte 3
  xreturn += bitRead(bit3_3 , 2) * (81);
  xreturn += bitRead(bit3_3 , 3) * (-81);
  return xreturn /1;// conversion;
}




char * padzeros2(int digits) {
  byte r;
  r = sprintf(buf, "%02d", digits);
  return buf;
}

char * padzeros3(int digits) {
  byte r;
  r = sprintf(buf, "%03d", digits);
  return buf;
}