#include <Adafruit_GFX.h>     // Download: https://electronoobs.com/eng_arduino_Adafruit_GFX.php
#include <Adafruit_ST7735.h>  // Download: https://electronoobs.com/eng_arduino_ST7735.php
#include <SPI.h>              //Arduino IDE by default
#include <SD.h>               //Arduino IDE by default


#define TFT_RST  9            // Reset line for TFT
#define TFT_CS   10           // Chip select line for TFT display
#define TFT_DC   8            // Data/command line for TFT
#define SD_CS    6            // Chip select line for SD card
//D11 -> MOSI SD + SDA TFT    //Connect D11 to MOSI of SD card module but also to DIN of TFT
//D12 -> MISO SD              //Connect D12 to MISO of SD card module
//D13 -> SCK SD + SCL TFT     //SCK is connected to SCK of SD module and TFT display
#define end_stop 7            //Out pin from the end stop switch
#define mot_en   5            //Enable pin of the TMC2209 driver
#define mot_step 4            //Step pin of the TMC2209 driver
#define mot_dir  3            //Direction pin of the TMC2209 driver
#define LED      2            //Pin connected to the abse of the TIP31 transistor

// For 0.96", 1.44" and 1.8" TFT with ST7735 use
Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS,  TFT_DC, TFT_RST);

//Variables
bool homming = false;
bool print_finished = false;
bool success = false;
bool UP = HIGH;
bool DOWN = LOW;
float steps_mm = 140;
float mm_step = 0.007142857;
float lift_print_slow_distance = 2;
float lift_print_distance = 20;
int homming_feedrate = 4000;
int lift_print_feedrate_slow = 10000;
int lift_print_feedrate = 5000;
float layer_height = 0.1;
int LED_on_time = 8000;
float max_height = 200;
float actual_height = 0;
bool first_print = false;
File root;


void setup(void) {
  Serial.begin(9600);
  while (!Serial) {
    delay(10);  // wait for serial console
  }

  pinMode(TFT_CS, OUTPUT);
  digitalWrite(TFT_CS, HIGH);
  pinMode(SD_CS, OUTPUT);
  digitalWrite(SD_CS, HIGH);
  pinMode(end_stop, INPUT);
  pinMode(mot_en, OUTPUT);
  digitalWrite(mot_en, HIGH);   //Start with TMC2209 /disabled
  pinMode(mot_step, OUTPUT);
  pinMode(mot_dir, OUTPUT);
  pinMode(LED, OUTPUT);
  digitalWrite(LED, LOW);       //Start with UV LEDs turned off 

  // Use this initializer if you're using a 1.8" TFT
  tft.initR(INITR_BLACKTAB);
  tft.fillScreen(ST77XX_BLACK);
  Serial.print("Initializing SD card...");
  if (!SD.begin(SD_CS)) {
    Serial.println("failed!");
    return;
  }
  Serial.println("OK!");
  root = SD.open("/");
  delay(100);
}


///////////////////////VOID LOOP////////////////////////////
////////////////////////////////////////////////////////////
void loop() {
  if(!homming){
    home_machine();
  }

  if(!print_finished){
    print_next_bmp();
    if(!print_finished){
      turn_on_LED(LED_on_time);
      lift_print();
      lower_print();
    }
  }
  if(print_finished && !success){
    lift_finished_print();
    success = true;
  }  
}//End void loop
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
void lower_print(){
  int i = 0;
  //Calculate the needed amount of steps
  float lower_print_steps = ((lift_print_slow_distance + lift_print_distance - layer_height) * steps_mm)/2.0;
  Serial.println("Lowering platform");

  while(i < lower_print_steps){
    digitalWrite(mot_en, LOW);
    digitalWrite(mot_dir, DOWN);
    digitalWrite(mot_step, HIGH);
    actual_height = actual_height - mm_step;
    delayMicroseconds(1);
    digitalWrite(mot_step, LOW);
    delayMicroseconds(lift_print_feedrate);
    actual_height = actual_height - mm_step;
    i++;
  }
  Serial.print("Actual height: "); Serial.println(actual_height);
  delay(1000);
}

////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////

void lift_finished_print(){
  Serial.println("Rising finished print");
  while(actual_height < max_height){
    digitalWrite(mot_en, LOW);
    digitalWrite(mot_dir, UP);
    digitalWrite(mot_step, HIGH);
    actual_height = actual_height + mm_step;
    delayMicroseconds(1);
    digitalWrite(mot_step, LOW);
    delayMicroseconds(lift_print_feedrate_slow);
    actual_height = actual_height + mm_step;
  }
  Serial.print("Actual height: "); Serial.println(actual_height);
  Serial.println("FINISHED!");
}

////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////

void lift_print(){
  Serial.println("Rising platform");
  int i = 0;
  //Calculate the needed amount of steps
  float lower_print_steps_slow = (lift_print_slow_distance * steps_mm)/2.0; 
  while (i < lower_print_steps_slow){
    digitalWrite(mot_en, LOW);
    digitalWrite(mot_dir, UP);
    digitalWrite(mot_step, HIGH);
    actual_height = actual_height + mm_step;
    delayMicroseconds(1);
    digitalWrite(mot_step, LOW);
    delayMicroseconds(lift_print_feedrate_slow);
    actual_height = actual_height + mm_step;
    i++;
  }

  i = 0;
  //Calculate the needed amount of steps
  float lower_print_steps = (lift_print_distance * steps_mm)/2.0;
  while (i < lower_print_steps){
    digitalWrite(mot_en, LOW);
    digitalWrite(mot_dir, UP);
    digitalWrite(mot_step, HIGH);
    actual_height = actual_height + mm_step;
    delayMicroseconds(1);
    digitalWrite(mot_step, LOW);
    delayMicroseconds(lift_print_feedrate);
    actual_height = actual_height + mm_step;
    i++;
  }
  Serial.print("Actual height: "); Serial.println(actual_height);
  delay(1000);
}

////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////

void turn_on_LED(int t){
  Serial.print("LEDs turned ON for "); Serial.print(t); Serial.println(" ms");
  digitalWrite(LED, HIGH);
  delay(t);
  digitalWrite(LED, LOW);
  delay(100);
  Serial.println("LEDs turned OFF");
  tft.fillScreen(ST77XX_BLACK);
}

////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////

void print_next_bmp(){
  if(!first_print){
    File entry = root.openNextFile();
    first_print = true;
  }
  File entry = root.openNextFile();
  if(!entry){
    //no more files
    print_finished = true;
    Serial.println("Print finished");
    root.close();
    return;
  }
  uint8_t nameSize = String(entry.name()).length();           //get file name size
  String str1 = String(entry.name()).substring(nameSize - 4); //Save last 4 characters (file extension)
  Serial.print("Drawing "); Serial.println(entry.name());
  if(str1.equalsIgnoreCase(".bmp")){
    bmpDraw(entry.name(), 0, 0);
  }
  entry.close();
  delay(1000);  
}

////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////

void home_machine(){
  if(!homming){
    Serial.println("Homming Machine");
    while(digitalRead(end_stop)){
      digitalWrite(mot_en, LOW);
      digitalWrite(mot_dir, DOWN);
      digitalWrite(mot_step, HIGH);      
      delayMicroseconds(1);
      digitalWrite(mot_step, LOW);
      delayMicroseconds(homming_feedrate);
    }
    homming = true;
    actual_height = 0;
    Serial.println("HOME");
    delay(1000);          
  }
}

////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////

#define BUFFPIXEL 20
void bmpDraw(char *filename, uint8_t x, uint16_t y) {

  File     bmpFile;
  int      bmpWidth, bmpHeight;   // W+H in pixels
  uint8_t  bmpDepth;              // Bit depth (currently must be 24)
  uint32_t bmpImageoffset;        // Start of image data in file
  uint32_t rowSize;               // Not always = bmpWidth; may have padding
  uint8_t  sdbuffer[3*BUFFPIXEL]; // pixel buffer (R+G+B per pixel)
  uint8_t  buffidx = sizeof(sdbuffer); // Current position in sdbuffer
  boolean  goodBmp = false;       // Set to true on valid header parse
  boolean  flip    = true;        // BMP is stored bottom-to-top
  int      w, h, row, col;
  uint8_t  r, g, b;
  uint32_t pos = 0, startTime = millis();

  if((x >= tft.width()) || (y >= tft.height())) return;

  Serial.println();
  Serial.print(F("Loading image '"));
  Serial.print(filename);
  Serial.println('\'');

  // Open requested file on SD card
  if ((bmpFile = SD.open(filename)) == NULL) {
    Serial.print(F("File not found"));
    return;
  }

  // Parse BMP header
  if(read16(bmpFile) == 0x4D42) { // BMP signature
    Serial.print(F("File size: ")); Serial.println(read32(bmpFile));
    (void)read32(bmpFile); // Read & ignore creator bytes
    bmpImageoffset = read32(bmpFile); // Start of image data
    Serial.print(F("Image Offset: ")); Serial.println(bmpImageoffset, DEC);
    // Read DIB header
    Serial.print(F("Header size: ")); Serial.println(read32(bmpFile));
    bmpWidth  = read32(bmpFile);
    bmpHeight = read32(bmpFile);
    if(read16(bmpFile) == 1) { // # planes -- must be '1'
      bmpDepth = read16(bmpFile); // bits per pixel
      Serial.print(F("Bit Depth: ")); Serial.println(bmpDepth);
      if((bmpDepth == 24) && (read32(bmpFile) == 0)) { // 0 = uncompressed

        goodBmp = true; // Supported BMP format -- proceed!
        Serial.print(F("Image size: "));
        Serial.print(bmpWidth);
        Serial.print('x');
        Serial.println(bmpHeight);

        // BMP rows are padded (if needed) to 4-byte boundary
        rowSize = (bmpWidth * 3 + 3) & ~3;

        // If bmpHeight is negative, image is in top-down order.
        // This is not canon but has been observed in the wild.
        if(bmpHeight < 0) {
          bmpHeight = -bmpHeight;
          flip      = false;
        }

        // Crop area to be loaded
        w = bmpWidth;
        h = bmpHeight;
        if((x+w-1) >= tft.width())  w = tft.width()  - x;
        if((y+h-1) >= tft.height()) h = tft.height() - y;

        // Set TFT address window to clipped image bounds
        tft.setAddrWindow(x, y, x+w-1, y+h-1);

        for (row=0; row < h; row++) { // For each scanline...

          // Seek to start of scan line.  It might seem labor-
          // intensive to be doing this on every line, but this
          // method covers a lot of gritty details like cropping
          // and scanline padding.  Also, the seek only takes
          // place if the file position actually needs to change
          // (avoids a lot of cluster math in SD library).
          if(flip) // Bitmap is stored bottom-to-top order (normal BMP)
            pos = bmpImageoffset + (bmpHeight - 1 - row) * rowSize;
          else     // Bitmap is stored top-to-bottom
            pos = bmpImageoffset + row * rowSize;
          if(bmpFile.position() != pos) { // Need seek?
            bmpFile.seek(pos);
            buffidx = sizeof(sdbuffer); // Force buffer reload
          }

          for (col=0; col < w; col++) { // For each pixel...
            // Time to read more pixel data?
            if (buffidx >= sizeof(sdbuffer)) { // Indeed
              bmpFile.read(sdbuffer, sizeof(sdbuffer));
              buffidx = 0; // Set index to beginning
            }

            // Convert pixel from BMP to TFT format, push to display
            b = sdbuffer[buffidx++];
            g = sdbuffer[buffidx++];
            r = sdbuffer[buffidx++];
            tft.pushColor(tft.color565(r,g,b));
          } // end pixel
        } // end scanline
        Serial.print(F("Loaded in "));
        Serial.print(millis() - startTime);
        Serial.println(" ms");
      } // end goodBmp
    }
  }

  bmpFile.close();
  if(!goodBmp) Serial.println(F("BMP format not recognized."));
}


// These read 16- and 32-bit types from the SD card file.
// BMP data is stored little-endian, Arduino is little-endian too.
// May need to reverse subscript order if porting elsewhere.

uint16_t read16(File f) {
  uint16_t result;
  ((uint8_t *)&result)[0] = f.read(); // LSB
  ((uint8_t *)&result)[1] = f.read(); // MSB
  return result;
}

uint32_t read32(File f) {
  uint32_t result;
  ((uint8_t *)&result)[0] = f.read(); // LSB
  ((uint8_t *)&result)[1] = f.read();
  ((uint8_t *)&result)[2] = f.read();
  ((uint8_t *)&result)[3] = f.read(); // MSB
  return result;
}


void printDirectory(File dir, int numTabs) {
  while (true) {

    File entry =  dir.openNextFile();
    if (! entry) {
      // no more files
      break;
    }
    for (uint8_t i = 0; i < numTabs; i++) {
      Serial.print('\t');
    }
    Serial.print(entry.name());
    if (entry.isDirectory()) {
      Serial.println("/");
      printDirectory(entry, numTabs + 1);
    } else {
      // files have sizes, directories do not
      Serial.print("\t\t");
      Serial.println(entry.size(), DEC);
    }
    entry.close();
  }
}