/*
Olympus Heavy Industries - Tricorder version 5 fw O.10b
https://hackaday.io/project/132098-science-tricorder-by-olympus-heavy-industries
Based on loads of code, mostly not mine.
Uses ~14% memory space with Feather M4
*/

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <string.h>
#include "SPI.h"
#include "Adafruit_GFX.h"
#include "MPU9250.h" //MPU9250 MEC/MAG

#include "Statistic.h"

#include <SD.h> //Storage (Feather uSD Reader)

#include <Adafruit_CAP1188.h> //Touch (buttons)
//#include <Adafruit_BME280.h> //ATM (temp, bar pressure, humidity)
#include <Adafruit_TSL2561_U.h> //LUX (single sensor)
#include <Adafruit_MLX90614.h> //IR TEMP
#include "RTClib.h" //DATA (RealTimeClock)
#include "Adafruit_ILI9341.h" //TFT 320x240
#include "Adafruit_SI1145.h" //UV_IR_VIS
#include "Adafruit_VEML6070.h" //UV_REAL
#include "Adafruit_TCS34725.h" //COLOR
#include <Adafruit_AMG88xx.h> //GRIDEYE (ADAFruit)
#include "DFRobot_BMP280.h" //ATM BMP280
#include "RadiationWatch.h" //RAD

MPU9250 mpu;

#include "FastLED.h" //PIXELS

#define TCAADDR 0x70 //i2C Multiplexer

FASTLED_USING_NAMESPACE;
RTC_DS3231 rtc; //DATA clock
//Adafruit_BME280 bme; // ATM
Adafruit_TSL2561_Unified tsl = Adafruit_TSL2561_Unified(TSL2561_ADDR_HIGH, 12345); //LUX
Adafruit_SI1145 uv = Adafruit_SI1145(); //UV_IR_VIS
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X); //COLOR
Adafruit_VEML6070 uv2 = Adafruit_VEML6070(); //UV_REAL
Adafruit_MLX90614 mlx = Adafruit_MLX90614(); //IR TEMP
Adafruit_AMG88xx amg; //GRIDEye
RadiationWatch radiationWatch; //RAD
//MPU9250 myIMU; //9DOF

const byte interruptPin = 5;
int timerCounter = 0;
boolean inReset = false;

byte gammatable[256];

//------------------------------GridEYE----------------------
//low range of the sensor (this will be blue on the screen) IN DEG CELCIUS
#define MINTEMP 20

//high range of the sensor (this will be red on the screen) IN DEG CELCIUS
#define MAXTEMP 40

//the colors we will be using
const uint16_t camColors[] = {0x480F,
0x400F,0x400F,0x400F,0x4010,0x3810,0x3810,0x3810,0x3810,0x3010,0x3010,
0x3010,0x2810,0x2810,0x2810,0x2810,0x2010,0x2010,0x2010,0x1810,0x1810,
0x1811,0x1811,0x1011,0x1011,0x1011,0x0811,0x0811,0x0811,0x0011,0x0011,
0x0011,0x0011,0x0011,0x0031,0x0031,0x0051,0x0072,0x0072,0x0092,0x00B2,
0x00B2,0x00D2,0x00F2,0x00F2,0x0112,0x0132,0x0152,0x0152,0x0172,0x0192,
0x0192,0x01B2,0x01D2,0x01F3,0x01F3,0x0213,0x0233,0x0253,0x0253,0x0273,
0x0293,0x02B3,0x02D3,0x02D3,0x02F3,0x0313,0x0333,0x0333,0x0353,0x0373,
0x0394,0x03B4,0x03D4,0x03D4,0x03F4,0x0414,0x0434,0x0454,0x0474,0x0474,
0x0494,0x04B4,0x04D4,0x04F4,0x0514,0x0534,0x0534,0x0554,0x0554,0x0574,
0x0574,0x0573,0x0573,0x0573,0x0572,0x0572,0x0572,0x0571,0x0591,0x0591,
0x0590,0x0590,0x058F,0x058F,0x058F,0x058E,0x05AE,0x05AE,0x05AD,0x05AD,
0x05AD,0x05AC,0x05AC,0x05AB,0x05CB,0x05CB,0x05CA,0x05CA,0x05CA,0x05C9,
0x05C9,0x05C8,0x05E8,0x05E8,0x05E7,0x05E7,0x05E6,0x05E6,0x05E6,0x05E5,
0x05E5,0x0604,0x0604,0x0604,0x0603,0x0603,0x0602,0x0602,0x0601,0x0621,
0x0621,0x0620,0x0620,0x0620,0x0620,0x0E20,0x0E20,0x0E40,0x1640,0x1640,
0x1E40,0x1E40,0x2640,0x2640,0x2E40,0x2E60,0x3660,0x3660,0x3E60,0x3E60,
0x3E60,0x4660,0x4660,0x4E60,0x4E80,0x5680,0x5680,0x5E80,0x5E80,0x6680,
0x6680,0x6E80,0x6EA0,0x76A0,0x76A0,0x7EA0,0x7EA0,0x86A0,0x86A0,0x8EA0,
0x8EC0,0x96C0,0x96C0,0x9EC0,0x9EC0,0xA6C0,0xAEC0,0xAEC0,0xB6E0,0xB6E0,
0xBEE0,0xBEE0,0xC6E0,0xC6E0,0xCEE0,0xCEE0,0xD6E0,0xD700,0xDF00,0xDEE0,
0xDEC0,0xDEA0,0xDE80,0xDE80,0xE660,0xE640,0xE620,0xE600,0xE5E0,0xE5C0,
0xE5A0,0xE580,0xE560,0xE540,0xE520,0xE500,0xE4E0,0xE4C0,0xE4A0,0xE480,
0xE460,0xEC40,0xEC20,0xEC00,0xEBE0,0xEBC0,0xEBA0,0xEB80,0xEB60,0xEB40,
0xEB20,0xEB00,0xEAE0,0xEAC0,0xEAA0,0xEA80,0xEA60,0xEA40,0xF220,0xF200,
0xF1E0,0xF1C0,0xF1A0,0xF180,0xF160,0xF140,0xF100,0xF0E0,0xF0C0,0xF0A0,
0xF080,0xF060,0xF040,0xF020,0xF800,};

unsigned long delayTime;
float pixels[AMG88xx_PIXEL_ARRAY_SIZE];
uint16_t displayPixelWidth, displayPixelHeight;

//------------------------------ATM BMP280-------------------
#define SEA_LEVEL_PRESSURE    1015.0f   // sea level pressure
typedef DFRobot_BMP280_IIC    BMP;    // ******** use abbreviations instead of full names ********
BMP   bmp(&Wire, BMP::eSdoLow);

//--------------------------------Gas Sensor-----------------
#define CH_VALUE_NH3 1
#define CH_VALUE_CO 2
#define CH_VALUE_NO2 3
#define CMD_CONTROL_LED 10

unsigned char dta_test[20];
unsigned int readChAdcValue(int ch);
unsigned int adcValueR0_NH3_Buf;
unsigned int adcValueR0_CO_Buf;
unsigned int adcValueR0_NO2_Buf;
    
uint16_t res0[3];       //sensors res0
uint16_t res[3];        //sensors res

enum{CO, NO2, NH3, C3H8, C4H10, CH4, H2, C2H5OH};

inline unsigned int get_addr_dta(unsigned char addr_reg);
inline unsigned int get_addr_dta(unsigned char addr_reg, unsigned char __dta);
inline void write_i2c(unsigned char addr, unsigned char *dta, unsigned char dta_len);
//-----------------------------------------------------------------

//#define NUM_LEDS 1
#define DATA_PIN A2 
#define LED_TYPE    WS2811
#define COLOR_ORDER GRB
#define NUM_LEDS    2

#define VBATPIN A6 //Batt Sense
const int PIN_TONE = A4;

String TricVersion = "4o.10c-M4"; 
int debug = 0; //-----------------DEBUG FLAG HERE!!!!!!!

char daysOfTheWeek[7][12] = {"Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday"};
int RTCFail = 0;
int ScanMode = 1;
int ScanLight[3];
int EnterCount = 0;
int ClearData = 1;
int BattPercent;
int AlarmMode;
int MagRead;
int TempRead;
int TempRead2;
int TempAverage;
int pixel[64];
int pixelX = 0;
int pixelY = 0;
int gridStartX = 226;
int gridStartY = 95;
int pixelWidth = 11;
int pixelHeight = 11;
int pixelXend = 8;
int pixelYend = 8;
float PixelTemp;
int i2cScan = 0;
int DevCount = 25;
int OldScanMode = 1;
int AltMode = 0;
int GridCen;
int GridCenA;
int GridCenB;
int GridCenC;
int GridCenD;
int GridAve;
int ButtonAct = 0;
int nineDOFstart = 0;
int RadPulse = 0;
int RadStr;
int RadError;
int RadStartSec;
int RadStart = 0;
int RadPulseCount;
int RadPulseTime;
float RadTick;
float RadPulseAvg = 0;
float RadPulseAvg2 = 0;
//int startupindicator;

Statistic myStats1;  // for MEC Acceleration
Statistic myStats2;  // for MEC Translation


uint32_t start;
uint32_t stop;

int pinSpeaker = A4;

Adafruit_CAP1188 cap = Adafruit_CAP1188(); //Touch

#define TFT_DC A0
#define TFT_CS 6 //TFT LCD

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC); //TFT LCD


CRGB leds[NUM_LEDS]; //PIXELS

#if defined(ARDUINO_SAMD_ZERO) && defined(SERIAL_PORT_USBVIRTUAL)
  #define Serial SERIAL_PORT_USBVIRTUAL
#endif


//---------------------------------------------VOID SETUP----------------------------------------------------------------------------------------------------
void setup() {
  Wire.begin();
  pinMode(A1, OUTPUT); //PCB Green led (heatbeat)
  pinMode(8, OUTPUT); //SD Card LED
  pinMode(19, OUTPUT); //Color Sensor LED Control
  digitalWrite(19, LOW);//Color Sensor LED Control (low=off)
  
  pinMode(pinSpeaker, OUTPUT);
  
  tft.begin();
  Serial.begin(115200);
  playTone(1700, 150);
    
  myStats1.clear(); //explicitly start stats clean
  myStats2.clear(); //explicitly start stats clean
  
  FastLED.addLeds<LED_TYPE,DATA_PIN,COLOR_ORDER>(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);
  FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);
  leds[0] = CRGB(75,30,0); 
  FastLED.show(); //PIXEL Start

  tsl.setGain(TSL2561_GAIN_16X);    //LUX Start
  tsl.setIntegrationTime(TSL2561_INTEGRATIONTIME_13MS);
  
  tft.fillScreen(ILI9341_BLACK); //TFT LCD Start
  DrawFrame();  //DrawDate();
  tft.setCursor(135,120);  tft.setTextSize(1);  tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK); tft.print("Starting.");


  
  //-----------CAP--------------------
  cap.begin(0x28);
  cap.writeRegister(0x2A, 0x80);  // 0x2A default 0x80 use 0x41  — Set multiple touches back to off
  //cap.writeRegister(0x41, 0x39);  // 0x41 default 0x39 use 0x41  — Set "speed up" setting back to off
  //cap.writeRegister(0x72, 0x00);  // 0x72 default 0x00  — Sets LED links back to off (default)
  cap.writeRegister(0x44, 0x41);  // 0x44 default 0x40 use 0x41  — Set interrupt on press but not release
  cap.writeRegister(0x28, 0x00);  // 0x28 default 0xFF use 0x00  — Turn off interrupt repeat on button hold

  
  //-------------setup for color sense
  for (int i=0; i<256; i++) {
    float x = i;
    x /= 255;
    x = pow(x, 2.5);
    x *= 255;
    gammatable[i] = x;
  }

  uv2.begin(VEML6070_1_T);
  
  pinMode(5, INPUT_PULLDOWN); //TouchInterrupt
  //pinMode(5, INPUT_PULLUP);

  playTone(1000, 50);
  
  tcaselect(3); //i2c multiplexer set to ATM BMP280

  //---------------------GRIDEye--------------------------------------
  tcaselect(0); //i2c multiplexer set to GRIDEYE
  amg.begin(); //GRIDEye
  displayPixelWidth = tft.width() / 8;
  displayPixelHeight = tft.height() / 8;
  bool status;
  
  leds[0] = CRGB(75,15,18); 
  
  
  FastLED.show(); //PIXEL Start
  //---------------------Startup checks-------------------------------
  tcaselect(3); 
  //if (imu.begin() != INV_SUCCESS){Serial.println("No MPU-9250");}
  //if(!bme.begin()){Serial.println("No BME280 (ATM) found");}
  if(!tsl.begin()){Serial.println("No TSL2561 (LUX) found");}
  if(!uv.begin()){Serial.println("No Si1145 (UV,IR,VIS) found");}
  if(!tcs.begin()){Serial.println("No TCS34725 (COLOR) found");}
  if(!mlx.begin()){Serial.println("No MLX90614 (IR-Temp) found");}
  if (!rtc.begin()) {Serial.println("No RTC Found");}
  if(!bmp.begin() != BMP::eStatusOK) {Serial.println("No ATM BMP280 found");printLastOperateStatus(bmp.lastOperateStatus);}
  if(!mpu.setup(0x68)){Serial.println("no MPU9250 (MEC/MAG) Found");}

  tft.setCursor(188,120);  tft.setTextSize(1);  tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK); tft.print(".");

  //------------------------------------------------------------------
  
  playTone(1000, 50);  
  radiationWatch.setup(); radiationWatch.registerRadiationCallback(&onRadiation); radiationWatch.registerNoiseCallback(&onNoise);
  powerOn();ledOff(); //Gas Sensor

  leds[0] = CRGB(25,15,65); 
  FastLED.show(); //PIXEL Start
  
  leds[0] = CRGB(0,75,0); 
  FastLED.show();

  int pixel[64]; //GRIDEYE
  start = millis();
  
  attachInterrupt(5, ButtonPress, FALLING);

  //----------------------------------------------------------------------
  
  playTone(1450, 100); //Startup Complete!
  tft.setCursor(192,120);  tft.setTextSize(1);  tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK); tft.print(".");

}


//---------------------------------------------VOID LOOP-----------------------------------------------------------------------------------------------------
void loop() {
  EnterCount = 0;

  if (debug == 1){
  tft.setCursor(15,50);
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
  tft.print(ScanMode);tft.print("");tft.print(OldScanMode);tft.print(AltMode);
  }

  DrawDate(); //draw time to UI frame
  
  tcaselect(1);DateTime now = rtc.now(); //Save timestamp to SD buffer
    
  DrawBatt();
  
  uint16_t clear, red, green, blue; //color
  
  uint8_t touched = cap.touched();
  for (uint8_t i=0; i<8; i++) {
    if (touched & (1 << i)) {
      ButtonAct = i;
      if (AltMode == 0){
      ScanMode = i;
      EnterCount = 1;
      } else if (i == 4){
        if (AltMode == 1){
        AltMode = 0;/*dataString += "-ALT unPRESSED-";*/tft.setTextSize(2);tft.setCursor(268, 63);tft.setTextColor(ILI9341_BLACK, ILI9341_WHITE);tft.print("NORM");
      }
    }
    }
  }
  if (ClearData == 1){BlankDataReadout();}
  
//--------------------------------------------LUM MODE----------------------------DONE---------------------------------------------------------------------------
  if (ScanMode == 5){
    //dataString += "LUM: ";
    sensors_event_t event;
    tsl.getEvent(&event);
    
    digitalWrite(19, HIGH);//Color Sensor LED Control (low=off)

    tcs.getRawData(&red, &green, &blue, &clear);

    float UVindex = uv.readUV();
    UVindex /= 100.0;  
      
    tft.setCursor(40,170);
    tft.setTextSize(1);

    tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
    tft.setTextSize(2);
    tft.setCursor(130,95);tft.print(round(event.light));tft.print("  ");
    tft.setCursor(130,115);tft.print(uv.readVisible());tft.print("  ");
    tft.setCursor(130,135);tft.print(uv.readIR());tft.print("  ");
    tft.setCursor(130,155);tft.print(uv2.readUV());tft.print("  ");
    tft.setCursor(130,175);tft.print(UVindex);tft.print("  ");
      
    //------------------color sense
    uint32_t sum = clear;
    sum += green;
    sum += blue;
    sum += clear;
    float r, g, b;
    r = red; r /= sum;
    g = green; g /= sum;
    b = blue; b /= sum;
    r *= 256; g *= 256; b *= 256;
    if(r==255){r=0;}
    if(b==255){b=0;}
    if(g==255){g=0;}

    int RedSense = (255-(gammatable[(int)r*5]));
    int GreenSense = (255-(gammatable[(int)g*5]));
    int BlueSense = (255-(gammatable[(int)b*5]));


    leds[0] = CRGB(gammatable[(int)r*2], gammatable[(int)g*2], gammatable[(int)b*2]); FastLED.show(); //COLOR pixel

    byte R,G,B; R = r; G = g; B = b;
    int RGBval = (((R*3) & 0xF8) << 8) | (((G*4) & 0xFC) << 3) | (((B*4) & 0xF8) >> 3);
 
    //tft.fillRect((gridStartX), (gridStartY), 63, 65, RGBval);
    tft.fillRect(gridStartX, gridStartY, (pixelWidth*8), (pixelHeight*8), RGBval); //grideye display frame
    tft.setTextSize(1);
    tft.setCursor(225,210);tft.print(255-(gammatable[(int)r*5]));tft.print(", ");tft.print(255-(gammatable[(int)g*5]));tft.print(", ");tft.print(255-(gammatable[(int)b*5]));tft.print("  ");

    
    
    if(UVindex <= 2.999){tft.setTextSize(1);tft.setCursor (60, 195);tft.setTextColor(ILI9341_GREEN, ILI9341_BLACK);                     tft.print("UV Index Low       ");}
    if((UVindex >= 3) && (UVindex <= 5.999)){tft.setTextSize(1);tft.setCursor (60, 195);tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK);tft.print("UV Index Moderate  ");}
    if((UVindex >= 6) && (UVindex <= 7.999)){tft.setTextSize(1);tft.setCursor (60, 195);tft.setTextColor(ILI9341_ORANGE, ILI9341_BLACK);tft.print("UV Index HIGH!     ");}
    if((UVindex >= 8) && (UVindex <= 10.999)){tft.setTextSize(1);tft.setCursor (60, 195);tft.setTextColor(ILI9341_RED, ILI9341_BLACK);  tft.print("UV Index VERY HIGH!");}
    if(UVindex >= 11){tft.setTextSize(1);tft.setCursor (60, 195);tft.setTextColor(ILI9341_PURPLE, ILI9341_WHITE);                       tft.print("UV Index EXTREME!!!");}
    }
      
//--------------------------------------------ATM MODE--------------------------------------DONE-----------------------------------------------------------------    
    else if (ScanMode == 1)
    {
      tft.setTextSize(2);tft.setCursor(45,195);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      //myStats.clear();
      //---------------------------AMT BMP280---------------------------------
      tcaselect(3); //i2c multiplexer set to ATM BMP280 & MPU9250
      float   temp = bmp.getTemperature();
      uint32_t    press = bmp.getPressure();
      float   alti = bmp.calAltitude(SEA_LEVEL_PRESSURE, press);

      int tempavg; tempavg = (temp); //Temp Average Start
      
      tft.setCursor(138,155); tft.setTextSize(2); tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.print(press/1000); tft.setTextSize(1); tft.setTextColor(ILI9341_CYAN, ILI9341_BLACK); tft.print(" kPa");
      tft.setCursor(138,175);tft.setTextSize(2); tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK); tft.print(alti);
      tft.setTextSize(1); tft.setTextColor(ILI9341_CYAN, ILI9341_BLACK);  tft.print("m");

      //---------------------------GRIDEye Adafruit---------------------------------
      //center pixels: 28, 29, 36, 37
      tcaselect(0); //i2c multiplexer set to GRIDEYE
      
      amg.readPixels(pixels);
//delay(200);
      for(int i=0; i<AMG88xx_PIXEL_ARRAY_SIZE; i++){
        
        GridCen = (GridCen + 1);
        if (GridCen == 28){GridCenA = (int) pixels[i-1];} //checks center 4 pixels, converts to INT and averages
        if (GridCen == 29){GridCenB = (int) pixels[i-1];}
        if (GridCen == 36){GridCenC = (int) pixels[i-1];}
        if (GridCen == 37){GridCenD = (int) pixels[i-1];}
        
        uint8_t colorIndex = map(pixels[i], MINTEMP, MAXTEMP, 0, 255);
        colorIndex = constrain(colorIndex, 0, 255);

        //draw the pixel
        tft.fillRect((gridStartX+(pixelWidth*(pixelX/8))),(gridStartY+(pixelHeight*(pixelY/8))), pixelWidth, pixelHeight, camColors[colorIndex]);
        //highlight center 4 pixels
        tft.drawRect( (gridStartX+(pixelWidth*3)-1), ((gridStartY+(pixelHeight*3))-1), ((pixelWidth*2)+2), ((pixelHeight*2)+2), ILI9341_WHITE);
       
        pixelX = (pixelX+8);
        if (pixelX >= 64){pixelX = 0; pixelY = pixelY+8;}
        if (pixelY >= 64){pixelX = 0; pixelY = 0;}
        //myStats.add(i);
      }
      GridAve = ((GridCenA + GridCenB + GridCenC + GridCenD)/4);
      GridCen = 0;

      //tft.setTextSize(1); 
      //tft.setCursor(220,50);tft.print(myStats.average());tft.print(" avg");
      //tft.setCursor(220,60);tft.print(myStats.minimum());tft.print(" min");
      //tft.setCursor(220,70);tft.print(myStats.maximum());tft.print(" max");

      //GrideyeAvg = 0;
      
      //myStats.clear();

      tempavg = (tempavg + amg.readThermistor()); //Temp Average Step 2
      
      GridAve = (GridAve * 9.0)/ 5.0 + 32.0;
      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setCursor(138,95);tft.setTextSize(2); tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);tft.print((GridAve + mlx.readObjectTempF())/2);
      tft.setTextSize(1); tft.setTextColor(ILI9341_CYAN, ILI9341_BLACK); tft.print(" *F "); //averages the center of the GRIDEYE readings with the target ir sensor

      //-------------------------------MLX90614------------------------------
      tempavg = (tempavg + mlx.readAmbientTempC());//Temp Average Step 3  
      tft.setCursor(138,115);tft.setTextSize(2); tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);tft.print(mlx.readObjectTempF())
      ;tft.setTextSize(1); tft.setTextColor(ILI9341_CYAN, ILI9341_BLACK); tft.print(" *F"); //MLX Directed target temp
      
      //--------------------------------add 9-dof temp to average------------------------------------      
      //tempavg = (tempavg + mpu.getTemperature());

      
      int TempRead = ((tempavg/3) * 9.0)/ 5.0 + 32.0; // Convert Celcius to Fahrenheit
      tft.setCursor(138,135);tft.setTextSize(2); tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);tft.print(TempRead);
      tft.setTextSize(1); tft.setTextColor(ILI9341_CYAN, ILI9341_BLACK); tft.print(" *F "); //average of all inernal thermometers
      
      //tft.setCursor(30,210);tft.setTextSize(1);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);

      }
//---------------------------------------------MAG MODE--------------DONE----------------------------------------------------------------------------------------
    else if (ScanMode == 3)
    {
      Serial.println("MAG");
      tcaselect(3); //i2c multiplexer set to ATM BMP280 & MPU9250

      tft.setCursor(40,170);
      tft.setTextSize(1);
      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);


    //------------------------------------------MAG--------------------------------------------------
      mpu.update_mag();

      tft.setTextColor(ILI9341_PINK, ILI9341_BLACK); tft.setTextSize(2);
      tft.setCursor(25,75); tft.print("X-MAG:");tft.print(mpu.getMagBiasX()); tft.print("uT ^/v ");
      tft.setCursor(25,95); tft.print("Y-MAG:");tft.print(mpu.getMagY()); tft.print("uT O  ");
      tft.setCursor(25,115); tft.print("Z-MAG:");tft.print(mpu.getMagZ()); tft.print("uT <-> ");
    }

//---------------------------------------------MEC MODE------------------------------------------------------------------------------------------------------
    else if (ScanMode == 0){
      tcaselect(3); //i2c multiplexer set to ATM BMP280 & MPU9250
      //if ( imu.dataReady() ){imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);}
    mpu.update();
    
    tft.setTextSize(2);
    //------------------------------------------ACCEL------------------------------------------------
      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      
      
      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setCursor(55,95); tft.print(mpu.getAccX()); tft.setTextSize(2);tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK); tft.print("g ");
      myStats1.add(mpu.getAccX());
      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setCursor(55,115); tft.print(mpu.getAccZ()); tft.setTextSize(2);tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK); tft.print("g ");
      myStats1.add(mpu.getAccZ());
      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setCursor(55,135); tft.print(mpu.getAccY()); tft.setTextSize(2);tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK); tft.print("g ");
      myStats1.add(mpu.getAccY());

      tft.setTextSize(2); tft.setCursor(75,175); tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK); tft.print(myStats1.maximum()); 
      tft.setTextSize(2);tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK); tft.print("g");
        
    //------------------------------------------GYRO-------------------------------------------------
      
      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setCursor(220,95); tft.print(mpu.getGyroX()); tft.setTextSize(2);tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK); tft.print("* ");
      myStats2.add(mpu.getGyroX());
      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setCursor(220,115); tft.print(mpu.getGyroZ()); tft.setTextSize(2);tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK); tft.print("* ");
      myStats2.add(mpu.getGyroZ());
      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setCursor(220,135); tft.setTextSize(2);tft.print(mpu.getGyroY()); tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK); tft.print("* ");
      myStats2.add(mpu.getGyroY());

      tft.setTextSize(2); tft.setCursor(190,175); tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK); tft.print(myStats2.maximum()); 
      tft.setTextSize(2);tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK); tft.print("* ");

      //-------------------------------------------SD Logging ATM---------------------------------------
    }

//---------------------------------------------DIAG MODE-----------------------------------------------------------------------------------------------------
      else if (ScanMode == 7)
      {
        if (digitalRead(A1) == LOW) { //Flashes the Color Sensor LED on and off opposing the HB LED
        digitalWrite(19, HIGH);digitalWrite(8, LOW);
        } else {
        digitalWrite(19, LOW);digitalWrite(8, HIGH);
        }
      //dataString += "DIAG: ";
      Diagscan();
      tft.setCursor(175,75); tft.print(TricVersion);
      delay(500);
      playTone(1600, 10);
      }
//---------------------------------------------MODE5 RAD-----------------------------------------------------------------------------------------------------
      else if (ScanMode == 6)//cap pin 3
      {
                  //tft.setCursor(45,125); tft.print("-no sensor-");
         
        if (RadStart == 0){
          tcaselect(1); //i2c multiplexer set to RTC
          DateTime now = rtc.now();
          RadStartSec = now.unixtime();
          RadStart = 1;
          RadPulseCount = 0;
          RadTick = 0;
          RadPulseAvg = 0;
        }
  
        byte error, address;
        //Serial.println("RAD");
        //dataString += "RAD: ";
        int nDevices;
        //tft.setTextSize(3);
        //tft.setTextColor(ILI9341_GREEN, ILI9341_BLACK);
        //tft.setCursor(25,115);tft.print("Scanning...");
        radiationWatch.loop();
        
        if (RadPulse == 1) {
          int sensor1 = (RadStr); int sensor2 = (RadError); 
          //dataString += " Pulse Detected: "; dataString += String(sensor1);
          //dataString += " uSv/h*100 +/- "; dataString += String(RadError); 
          RadPulse = 0; RadStr = 0; RadError = 0;
          RadPulseCount = RadPulseCount+1;
          RadPulseAvg = (RadPulseAvg + radiationWatch.uSvh());}
        //else {dataString += " No RAD Detection ";}
        

        tft.setTextSize(2);
        tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
        //tft.setCursor(25,85);tft.print("Pulses/Sec:");
        
        
        tcaselect(1); //i2c multiplexer set to RTC
        DateTime now = rtc.now();
        RadPulseTime = (now.unixtime()-RadStartSec); //finds seconds from mode start
        RadTick = ((float)RadPulseCount/(float)RadPulseTime);
        tft.setCursor(160,85);tft.print(RadTick,3);tft.print("  ");

        
        RadPulseAvg2 = ((float)RadPulseAvg/(float)RadPulseCount);
        //tft.setCursor(25,95);tft.print("Avg Pulse: ");
        tft.setCursor(160,105);tft.print(RadPulseAvg2,2);//tft.print(" uSv/h  ");

        if (debug == 1){
        tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);tft.setTextSize(1);
        tft.setCursor(40,220);tft.print(RadPulseTime);
        tft.setCursor(25,220);tft.print(RadPulseCount);
        tft.setCursor(90,220);tft.print(RadPulseAvg);
        tft.setCursor(130,220);tft.print(RadPulseAvg2);
        }
        
        //-------------------------------------------SD Logging ATM---------------------------------------
        //int sensor1 = (RadTick); dataString += "Pulses/Second: ";dataString += String(sensor1);dataString += ", ";
        //int sensor2 = (RadPulseAvg2); dataString += " Average Pulse ";dataString += String(sensor2); dataString += " uSv/h, ";
        //int sensor3 = (radiationWatch.uSvh()); dataString += " Last Pulse: ";dataString += String(sensor3); dataString += " uSv/h, ";
        
        
}

//--------------------------------------------GAS MODE---------------------------DONE----------------------------------------------------------------------------
  else if (ScanMode == 2){
    //Serial.println("GAS");
    //leds[0] = CRGB(20,0,0); FastLED.show(); //COLOR pixel RED
    //if (startupindicator == 1) {tft.setCursor(45,64); tft.setTextSize(2); tft.print("Starting...");}

    //dataString += "GAS: ";

    float ratio0, ratio1, ratio2; // THIS IS ALL EVIL MAGIC, DON'T MESS WITH IT CAUSE IT'LL UNLEASH STUFF WE DON'T UNDERSTAND (don't know how this code really works lol)
    ledOn();

        //leds[0] = CRGB(0,20,0); FastLED.show(); //COLOR pixel GREEN
        delay(100);
    int A0_0 = get_addr_dta(6, 8);int A0_1 = get_addr_dta(6, 4);int A0_2 = get_addr_dta(6, 12);
            //leds[0] = CRGB(0,0,20); FastLED.show(); //COLOR pixel BLUE
    int An_0 = get_addr_dta2(1);int An_1 = get_addr_dta2(2);int An_2 = get_addr_dta2(3);
            //leds[0] = CRGB(20,0,20); FastLED.show(); //COLOR pixel PURPLE
    ratio0 = (float)An_0/(float)A0_0*(1023.0-A0_0)/(1023.0-An_0);ratio1 = (float)An_1/(float)A0_1*(1023.0-A0_1)/(1023.0-An_1);ratio2 = (float)An_2/(float)A0_2*(1023.0-A0_2)/(1023.0-An_2);
    float c = 0;

    ledOff();
            //leds[0] = CRGB(20,20,0); FastLED.show(); //COLOR pixel YELLOW
    //if (startupindicator == 1) {tft.setCursor(45,64); tft.setTextSize(2); tft.print("           ");startupindicator = 0;}
    tft.setTextSize(1);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
    c = pow(ratio0, -1.67)/1.47;tft.setCursor(170,115);tft.print(c); int sensor1 = c;// NH3 ppm
    c = pow(ratio1, -1.179)*4.385;tft.setCursor(170,125);tft.print(c); int sensor2 = c;//CO ppm
    c = pow(ratio2, 1.007)/6.855;tft.setCursor(170,135);tft.print(c); int sensor3 = c;//NO2 ppm
    c = pow(ratio0, -2.518)*570.164;tft.setCursor(170,145);tft.print(c); int sensor4 = c;//C3H8 ppm
    c = pow(ratio0, -2.138)*398.107;tft.setCursor(170,155);tft.print(c); int sensor5 = c;//C4H10 ppm
    c = pow(ratio1, -4.363)*630.957;tft.setCursor(170,165);tft.print(c); int sensor6 = c;//CH4 ppm
    c = pow(ratio1, -1.8)*0.73;tft.setCursor(170,175);tft.print(c); int sensor7 = c;//H2 ppm
    c = pow(ratio1, -1.552)*1.622;tft.setCursor(170,185);tft.print(c); int sensor8 = c;//C2H5OH ppm
                //leds[0] = CRGB(0,0,0); FastLED.show(); //COLOR pixel WHITE
    
    

    delay(100);
/*
    //-------------------------------------------SD Logging GAS---------------------------------------
    dataString += " NH3 (ppm): ";dataString += String(sensor1, 3);dataString += ", ";
    dataString += " CO (ppm): ";dataString += String(sensor2, 3);dataString += ", ";
    dataString += " NO2 (ppm): ";dataString += String(sensor3, 3);dataString += ", ";
    dataString += " C3H8 (ppm): ";dataString += String(sensor4, 3);dataString += ", ";
    dataString += " C4H10 (ppm): ";dataString += String(sensor5, 3);dataString += ", ";
    dataString += " CH4 (ppm): ";dataString += String(sensor6, 3);dataString += ", ";
    dataString += " H2 (ppm): ";dataString += String(sensor7, 3);dataString += ", ";
    dataString += " C2H5OH (ppm): ";dataString += String(sensor8, 3);dataString += ", ";
  */  
  }
//--------------------------------------------ALT MODE-------------------------------------------------------------------------------------------------------    
  else if (ScanMode == 4)
    {
      ScanMode = OldScanMode;
      //dataString += "-ALT PRESSED-";
      AltMode = 1;
      tft.setTextSize(2);
      tft.setCursor(268, 63);
      if (AltMode == 0){tft.setTextColor(ILI9341_BLACK, ILI9341_WHITE);tft.print("ALTM");AltMode=1;}
      //else if (AltMode == 1){tft.setTextColor(ILI9341_BLACK, ILI9341_WHITE);tft.print("NORM");AltMode=0;}
      playTone(1850, 40);
      leds[0] = CRGB(30,30,30); FastLED.show();//colorLED
      delay(400);
      leds[0] = CRGB(0,0,0); FastLED.show();//colorLED
    }
      
//---------------------------------------------LOOP end------------------------------------------------------------------------------------------------------  
  else { }
 //ButtonAction ();
 
 int tmpCounter = 0;

  //-------------------------SD Write------------------------------------
  //SD Logfile check and dump
  // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  
  /*
  File dataFile = SD.open("datalog.txt", FILE_WRITE);
  digitalWrite(8, HIGH); //SD LED on
  //Serial.println("SD File Opened");

  // if the file is available, write to it:
  if (dataFile) {
    dataFile.println(dataString);
    dataFile.close();
    // print to the serial port too:
    Serial.println(dataString);
  }
  // if the file isn't open, pop up an error:
  else {
    Serial.println("error opening datalog.txt");
  }
  digitalWrite(8, LOW); //SD LED off
  */
  //----------------------------------------------------------------------

 while (digitalRead(5) == FALLING){ //Cap Interrupt
   if( (millis() - (digitalRead(5) == FALLING)) > 6000 || inReset){
     if( inReset )
       inReset = false;
       leds[0] = CRGB(30,30,30); FastLED.show();
       if (ScanMode != 4){BlankDataReadout();}
     break;
   } else {
   }
  }

  if (digitalRead(A1) == HIGH) { //HB LED On/off
  digitalWrite(A1, LOW);
  } else {
  digitalWrite(A1, HIGH);
  }
  //---------------------------------------------DEBUG---------------------------------------------------------------------------------------------------------

  //Showing the ButtonAct variable
  if (debug == 1){
      tft.setTextSize(2);
      tft.setCursor(232, 63);
      if (AltMode == 1){
      if (ButtonAct == 1){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("-O-");}      //atm
      else if (ButtonAct == 2){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("-UP");} //gas
      else if (ButtonAct == 3){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("DWN");} //mag
      else if (ButtonAct == 4){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("ALT");} //ALT
      else if (ButtonAct == 5){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("-X-");} //lun
      else if (ButtonAct == 6){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("RGT");} //rad
      else if (ButtonAct == 7){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("BCK");} //diag
      else if (ButtonAct == 0){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("LFT");} //mec
      }
  }



}
