#include <Wire.h>
#include <SPI.h>
#include <SD.h>

#define RESET 8         //LDR  
#define MUX   2         //LDR  
#define PROG_MODE A2
#define BLUE A0
#define GREEN A1
#define MMC_CS 10
#define RTX_CS 9
#define POWER 6
#define SELECT_MODE_1 7
#define SELECT_MODE_2 3

#define SHT21_ADDRESS        0x40    // Direction of the sht21  
#define debug true

// set up variables using the SD utility library functions:
Sd2Card card;
SdVolume volume;
SdFile root;

void setup()  
{
  pinMode(RESET, OUTPUT);
  digitalWrite(RESET, LOW); //RESET ON
  delay(100);
  digitalWrite(RESET, HIGH); //RESET OFF
  pinMode(MUX, OUTPUT);
  digitalWrite(MUX, LOW); //NORMAL MODE
  pinMode(PROG_MODE, OUTPUT);
  digitalWrite(PROG_MODE, LOW); //PROG_MODE OFF
  pinMode(GREEN, OUTPUT);
  digitalWrite(GREEN, HIGH); //GREEN ON
  pinMode(BLUE, OUTPUT);
  digitalWrite(BLUE, LOW); //BLUE OFF
  pinMode(MMC_CS, OUTPUT);
  digitalWrite(MMC_CS, LOW); //SPI MMC NO SELECT
  pinMode(RTX_CS, OUTPUT);
  digitalWrite(RTX_CS, LOW); //SPI RTX NO SELECT
//  pinMode(SCK, OUTPUT);
  pinMode(POWER, OUTPUT);
  digitalWrite(POWER, HIGH); //ENABLE POWER RTX
  pinMode(SELECT_MODE_1, INPUT);
  pinMode(SELECT_MODE_2, INPUT);
  Serial.begin(9600);
  Serial1.begin(9600); 
  Wire.begin();
  
  Serial.print("Initializing SD card...");
  if (!SD.begin(MMC_CS)) {
    Serial.println("initialization failed!");
    return;
  }
  Serial.println("initialization done.");
}

int firmware_mode = 0;
int operation_mode = 0;

void loop() // run over and over
{
  if (!digitalRead(SELECT_MODE_2))
    {
      if (firmware_mode == 1) bootloader_flash();
       else firmware_flash();
    } 
   else if (!digitalRead(SELECT_MODE_1))
     {
       if (operation_mode == 0) terminal_mode();
       else
        {
          Serial.begin(9600);
          Serial1.begin(9600); 
          digitalWrite(GREEN, HIGH); //GREEN LED ON
          digitalWrite(BLUE, LOW); //GREEN LED ON
          operation_mode = 0;
          firmware_mode = 0;
        }
     }
  if (firmware_mode==2) blink_led_blue(500);
  if (operation_mode==1) blink_led_green(500);
  
  if (operation_mode==1)
    {
      if (Serial.available())
          Serial1.write(Serial.read());
      if (Serial1.available())
        Serial.write(Serial1.read());
    }
  else 
    {
      getSHT21();
      delay(500);
      check_MMC();
    }
}

void disable_all()
  {
    Serial1.end();
    digitalWrite(MUX, HIGH); //PROGRAM MODE
    digitalWrite(SCK, LOW); //BLUE OFF
    digitalWrite(MISO, LOW); //BLUE OFF
    digitalWrite(MOSI, LOW); //BLUE OFF
    digitalWrite(RESET, LOW); //RESET OFF
  }

void enable_all() //Provisional function
  {
    Serial.begin(115200);
    Serial1.begin(115200); 
  }

void bootloader_flash()
  {
    disable_all();
    digitalWrite(GREEN, HIGH); //GREEN LED ON
    digitalWrite(BLUE, HIGH); //BLUE LED ON
    digitalWrite(POWER, LOW); //DISABLE POWER RTX
    delay(100);
    digitalWrite(PROG_MODE, HIGH);
    digitalWrite(RESET, HIGH); //RESET OFF
    delay(100);
    digitalWrite(POWER, HIGH);//ENABLE POWER RTX
    enable_all();
    delay(4000);
    digitalWrite(PROG_MODE, LOW);
    delay(100);
    digitalWrite(GREEN, LOW); //GREEN LED ON
    firmware_mode = 2;
    operation_mode = 1;
  }

void firmware_flash()
  {
     digitalWrite(RESET, LOW); //RESET
     delay(100);
     digitalWrite(RESET, HIGH); //RESET OFF
     digitalWrite(MUX, HIGH); //PROGRAM MODE
     Serial.begin(115200);
     Serial1.begin(115200); 
     digitalWrite(GREEN, LOW); //GREEN LED OFF
     digitalWrite(BLUE, HIGH); //BLUE LED ON 
     firmware_mode = 1;
     operation_mode = 1;
     delay(500);
  } 
  
void terminal_mode()
  {
    digitalWrite(MUX, LOW); //NORMAL MODE
    Serial.begin(9600);
    Serial1.begin(9600); 
    firmware_mode = 0;
    digitalWrite(GREEN, HIGH); //GREEN LED ON 
    digitalWrite(BLUE, LOW); //BLUE LED OFF 
    operation_mode = 1;
    delay(500);
  }
  
boolean ledStateblue = LOW;
unsigned long previousMillisblue = 0; 

void blink_led_blue(unsigned long interval)
  {
    unsigned long currentMillisblue = millis();
    
    if(currentMillisblue - previousMillisblue >= interval) {
      // save the last time you blinked the LED 
      previousMillisblue = currentMillisblue;   
  
      // if the LED is off turn it on and vice-versa:
      if (ledStateblue == LOW)
        ledStateblue = HIGH;
      else
        ledStateblue = LOW;
  
      // set the LED with the ledState of the variable:
      digitalWrite(BLUE, ledStateblue);
    }
  }

boolean ledStategreen = LOW;
unsigned long previousMillisgreen = 0; 

void blink_led_green(unsigned long interval)
  {
    unsigned long currentMillisgreen = millis();
    
    if(currentMillisgreen - previousMillisgreen >= interval) {
      // save the last time you blinked the LED 
      previousMillisgreen = currentMillisgreen;   
  
      // if the LED is off turn it on and vice-versa:
      if (ledStategreen == LOW)
        ledStategreen = HIGH;
      else
        ledStategreen = LOW;
  
      // set the LED with the ledState of the variable:
      digitalWrite(GREEN, ledStategreen);
    }
  }
  
uint16_t readSHT21(uint8_t type){
      uint16_t DATA = 0;
      Wire.beginTransmission(SHT21_ADDRESS);
      Wire.write(type);
      Wire.endTransmission();
      Wire.requestFrom(SHT21_ADDRESS,2);
      unsigned long time = millis();
      while (!Wire.available()) if ((millis() - time)>500) return 0x00;
      DATA = Wire.read()<<8; 
      while (!Wire.available()); 
      DATA = (DATA|Wire.read()); 
      DATA &= ~0x0003; 
      return DATA;
  }

uint32_t lastHumidity;
uint32_t lastTemperature;

void getSHT21()
 {
      lastTemperature = readSHT21(0xE3);  // RAW DATA for calibration in platform
      lastHumidity    = readSHT21(0xE5);  // RAW DATA for calibration in platform
    #if debug
      Serial.print("SHT21:  ");
      Serial.print("Temperature: ");
      Serial.print(-46.85 + ((175.72 * (float)lastTemperature) / 65536.0) );
//      Serial.print(lastTemperature);
      Serial.print(" C, Humidity: ");
      Serial.print(-6.0 + ((125.0  * (float)lastHumidity) / 65536.0));
//      Serial.print(lastHumidity);
      Serial.println(" %");    
    #endif
  }
  
void check_MMC()
  {
      // we'll use the initialization code from the utility libraries
      // since we're just testing if the card is working!
      if (!card.init(SPI_HALF_SPEED, MMC_CS)) {
        Serial.println("initialization failed. Things to check:");
        Serial.println("* is a card is inserted?");
        Serial.println("* Is your wiring correct?");
        Serial.println("* did you change the chipSelect pin to match your shield or module?");
        return;
      } else {
        Serial.println("Wiring is correct and a card is present.");
      }
  }