#include <Wire.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

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, LOW); //ENABLE POWER RTX
  pinMode(SELECT_MODE_1, INPUT);
  pinMode(SELECT_MODE_2, INPUT);
  Serial.begin(9600);
  Serial1.begin(9600); 
  Wire.begin();
  }

int firmware_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))terminal_mode();
    
  if (firmware_mode==2) blink_led(BLUE, 500);
  
//  getSHT21();
//  delay(500);
  
  if (Serial.available())
      Serial1.write(Serial.read());
  if (Serial1.available())
    Serial.write(Serial1.read());
}

void disable_all()
  {
    Serial1.end();
    digitalWrite(MUX, HIGH); //PROGRAM MODE
    pinMode(0, OUTPUT);
    pinMode(1, OUTPUT);
    digitalWrite(0, LOW);
    digitalWrite(1, LOW);
    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, HIGH); //DISABLE POWER RTX
    delay(4000);
    digitalWrite(PROG_MODE, HIGH);
    digitalWrite(RESET, HIGH); //RESET OFF
    delay(100);
    digitalWrite(POWER, LOW);//ENABLE POWER RTX
    enable_all();
    delay(4000);
    digitalWrite(PROG_MODE, LOW);
    delay(100);
    digitalWrite(GREEN, LOW); //GREEN LED ON
    firmware_mode = 2;
  }

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;
     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 
  }
  
boolean ledState = LOW;
unsigned long previousMillis = 0; 

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

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 / 65536.0) * (float)lastTemperature));
//      Serial.print(lastTemperature);
      Serial.print(" C, Humidity: ");
      Serial.print(-6.0 + (125.0 / 65536.0) * (float)lastHumidity);
//      Serial.print(lastHumidity);
      Serial.println(" %");    
    #endif
  }