////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////TRUEHEME CODE V0.0.3///////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "ADS1X15.h"
#include <SPI.h>
#include <Wire.h>

int LED_sequence[] = {1,2,3,4};
int LED_dac[] = {1640,1680,2104,2088};
String Device_ID = "SNS HCV-000-3010 SNE";                     //Device ID
String Firmware_version_Auto=__FILE__;
String Firmware_version = "TruehemeV0.0.3";
int LED_unstable_samples[] = {20,20,20,20};
int LED_sample_size[] = {50,50,50,50};
int LED_DS_threshold[] = {22000,22000,22000,22000};
int LED_settling_time[] = {10000,10000,10000,10000};
int extra_delay_per_LED =10000;
int LED_reading_delay[] = {0,0,0,0};
int DAC_Sweep_start_limit[] = {1000,1000,1000,1000};
int DAC_Sweep_end_limit[] = {4000,4000,4000,4000};
int DacSweep_gain_combinations[] = {0,0,0,0};

int incr=8; //DAC sweep increment
int del=500; //DAC delay
int appmode=1;
int moving_average_activation=0;
int raw_ADC_print=1;
int limit_printer=1;
long limit_average_storage=0;

int Blank_gain_combinations[] = {0,0,0,0};
int Sample_gain_combinationsG1[] = {0,0,0,0};
int Sample_gain_combinationsG2[] = {1,1,1,1};
int Sample_gain_combinationsG3[] = {2,2,2,2};
int Sample_gain_combinationsG4[] = {4,4,4,4};
int Sample_gain_combinationsG5[] = {8,8,8,8};

///////////////////////////////////////////DO NOT CHANGE ANY VALUES BELOW //////////////////////////////////////////////////////////////////////////////////////////////
const int PIN_CS_DAC1 = 10;
const int PIN_CS_DAC2 = 9;
const int GAIN_1 = 0x1;
const int GAIN_2 = 0x0;

const int PD1G1 = 6;
const int PD1G2 = 8;
const int PD2G1 = 9;
const int PD2G2 = 7;

const int LEDR = A0;
const int LEDB = A2;
const int LEDG = A1;

ADS1115 ADS(0x48);
///////////////////////////////////////////DO NOT CHANGE ANY VALUES ABOVE //////////////////////////////////////////////////////////////////////////////////////////////
const char compile_date[] = __DATE__ " " __TIME__;
int loopcounter = 0;
unsigned long long intensity_storage = 0;
int temp_counter = 0;

float avg_storage = 0;
float avg_green = 0;
float avg_blue = 0;
float old_green = 0;
float old_blue = 0;

int split_counter = 0;
int intensity_display = 1;
int plotter = 0;

int moving_average_array[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int sorting_array[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int moving_average = 0;
int j = 0;
int temp_sort = 0;
int skip = 0;

float average_readings = 0;
float avg_blank[]={0,0,0,0};
float avg_sample[]={0,0,0,0};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup()
{
  Serial.begin(9600);
  pinMode(PIN_CS_DAC1, OUTPUT);
  pinMode(PIN_CS_DAC2, OUTPUT);
  pinMode(PD1G1, OUTPUT);
  pinMode(PD1G2, OUTPUT);
  pinMode(PD2G1, OUTPUT);
  pinMode(PD2G2, OUTPUT);
  pinMode(LEDR, OUTPUT);
  pinMode(LEDG, OUTPUT);
  pinMode(LEDB, OUTPUT);
  digitalWrite(LEDR, LOW);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, HIGH);
  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV2);
  ADS.begin();
  if(appmode==0)
  {
    Serial.print("Initial DAC Settings:  ");
    for (int i =0; i<4;i++)
    {
      Serial.print("\t");Serial.print(LED_dac[i]);
    }
    Serial.println();
  }  
  if(appmode==0)
  {
    Serial.print("Process: Setup End; Timestamp: ");Serial.println(millis());
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void all_LEDs_turnoff()
{  
  setOutput(GAIN_1, 0, 1);  //LED 1
  setOutput(GAIN_1, 0, 2);  //LED 2
  setOutput(GAIN_1, 0, 3);  //LED 3
  setOutput(GAIN_1, 0, 4);  //LED 4
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void secondary_gain_select(int gain_combo)
{
  bool gain_choice1=bitRead(gain_combo, 0);
  bool gain_choice2=bitRead(gain_combo, 1);
  bool gain_choice3=bitRead(gain_combo, 2);
  bool gain_choice4=bitRead(gain_combo, 3);
  digitalWrite(PD1G1, gain_choice1);
  digitalWrite(PD1G2, gain_choice2);
  digitalWrite(PD2G1, gain_choice3);
  digitalWrite(PD2G2, gain_choice4);
  if(appmode==0)
  {
    Serial.print("Gains Used\t G1:");Serial.print(gain_choice1);Serial.print("\t G2:");Serial.print(gain_choice2);Serial.print("\t G3:");Serial.print(gain_choice3);Serial.print("\t G4:");Serial.println(gain_choice4);
  }  
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//void setOutput(byte channel, byte gain, byte shutdown, unsigned int val, int DACselect)
void setOutput(byte gain, unsigned int val, int LEDselect)
{
  byte channel=0;
  byte shutdown=1;
  if(LEDselect%2==0)
  {
    channel=1;
  }
  else
  {
    channel=0;
  }
  byte lowByte = val & 0xff;
  byte highByte = ((val >> 8) & 0xff) | channel << 7 | gain << 5 | shutdown << 4;
  if (LEDselect <= 2)
  {
    PORTB &= 0xfb;
    SPI.transfer(highByte);
    SPI.transfer(lowByte);
    PORTB |= 0x4;
  }
  else
  {
    PORTB &= 0xfd;
    SPI.transfer(highByte);
    SPI.transfer(lowByte);
    PORTB |= 0x2;
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int setDAC_LED()
{  
  int LED_flags[]={0,0,0,0};
  int flags_combined=0;
  if(appmode==0)
  {
    Serial.println("#DAC Sweep Start");  
  }  
  all_LEDs_turnoff();
  delay(1000);
  ////////////////////////////////////////FIRST FORLOOP TO DISCARD STARTS/////////////////////////////////////////////////
  for (int i=3000;i<4096;i+=incr)
  {
    setOutput(GAIN_1, i, 1);
    delay(del);
    int16_t val_0 = ADS.readADC(0);    
    if((val_0>24000))
    {
      break;
    }
  }
  all_LEDs_turnoff();
  delay(1000);
  ////////////////////////////////////////FIRST FORLOOP TO DISCARD ENDS/////////////////////////////////////////////////  
  //for (int j=0;j<sizeof(LED_sequence);j++)
  for (int j=0;j<4;j++)
  {
    all_LEDs_turnoff();
    secondary_gain_select(DacSweep_gain_combinations[j]);
    for (int i=DAC_Sweep_start_limit[j];i<DAC_Sweep_end_limit[j];i+=incr)
    {
      setOutput(GAIN_1, i, LED_sequence[j]);
      delay(del);
      int16_t val_0 = ADS.readADC(0);   
      if((val_0>LED_DS_threshold[j])&&(LED_flags[j]==0))
      {
        LED_dac[j]=i;
        LED_flags[j]=1;
        break;
      }
    } 
    all_LEDs_turnoff();
    if (appmode==0)
    {
      Serial.print("LED ");Serial.print(j+1);Serial.print(" DACsweep Completed");Serial.print("\t");Serial.print("NEW DAC VALUE : ");Serial.println(LED_dac[j]);
    }
    delay(1000);   
  }
  all_LEDs_turnoff();
  flags_combined=(LED_flags[3]*1)+(LED_flags[2]*10)+(LED_flags[1]*100)+(LED_flags[0]*1000);
  if(appmode==0)
  {
    Serial.println("#DAC Sweep End");  
  }  
  return flags_combined;    
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float detector(int unstable_samples,  int number_of_readings, int detector_select, int gain_combo, int settling_time, int reading_delay)
{
  if(appmode==0)
  {
    Serial.print("Process: Detector Start for any one LED; Timestamp: ");Serial.println(millis());
  }
  secondary_gain_select(gain_combo);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  skip = 0;
  intensity_storage = 0;
  int16_t detector_val = 0;
  if (moving_average_activation==1)
  {
    for (int i = 0; i < unstable_samples + number_of_readings; i += 1)
    {
      int16_t val_0 = ADS.readADC(0);
      int16_t val_1 = ADS.readADC(1);
      if (detector_select == 1)
      {
        detector_val = val_0;
      }
      else
      {
        detector_val = val_1;
      }
      moving_average_array[i % 10] = detector_val;
      for (int j = 0; j < 10; j++)
      {
        sorting_array[j] = moving_average_array[j];
      }
      if (i >= unstable_samples)
      {
        for (int j = 0; j < 6; j++)
        {
          for (int k = 0; k < (9 - j); k++)
          {
            if (sorting_array[k] > sorting_array[k + 1])
            {
              temp_sort = sorting_array[k];
              sorting_array[k] = sorting_array[k + 1];
              sorting_array[k + 1] = temp_sort;
            }
          }
        }
        moving_average = (sorting_array[4] / 2) + (sorting_array[5] / 2);
        if ((detector_val > (moving_average * 0.9)) and (detector_val < (moving_average * 1.1)))
        {
          intensity_storage += detector_val;
          temp_counter++;
        }
        else
        {
          skip++;
        }
      }
    }
  }
  else
  {
    int start_time=millis();
    int current_time=millis();
    int lower_limit=0;
    int upper_limit=0;
    limit_average_storage=0;
    while((current_time-start_time)<settling_time)
    {
      int16_t val_0 = ADS.readADC(detector_select-1);
      if(raw_ADC_print==1)
      {
        Serial.println(val_0);
      }
      current_time=millis();
    }
    delay(extra_delay_per_LED); 
    for (int i=0;i<number_of_readings;i++)
    {
      delay(reading_delay);
      int16_t val_0 = ADS.readADC(detector_select-1);
      if(raw_ADC_print==1)
      {
        if(limit_printer==1)
        {
          Serial.println(val_0);
        }
        else
        {
          Serial.println(val_0);
        }        
      }
      intensity_storage+=val_0;
      temp_counter++;
    }       
  }  
  all_LEDs_turnoff();
  average_readings = intensity_storage/temp_counter;
  if(appmode==0)
  {
    Serial.print("Skipped samples: ");Serial.println(skip);
    Serial.print("Process: Detector Start for any one LED; Timestamp: ");Serial.println(millis());
  }
  return average_readings;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop()
{
  ADS.setGain(0);
  loopcounter = 0;
  avg_storage = 0;
  split_counter = 0;
  if (Serial.available()) 
  {
    char data = Serial.read();
    if (data == 'C') 
    {
      Serial.println("#CS");
      int flags=setDAC_LED();
      if(flags!=1111)
      {
        if(appmode==0)
        {
          Serial.println("Unable to reach 24000 for atleast one LED");
        }        
      }
      Serial.print("LED1_DAC: ");Serial.println(LED_dac[0]);
      Serial.print("LED2_DAC: ");Serial.println(LED_dac[1]);
      Serial.print("LED3_DAC: ");Serial.println(LED_dac[2]);
      Serial.print("LED4_DAC: ");Serial.println(LED_dac[3]);
      Serial.println("#CC");
    }
    if (data == 'B') 
    {
      startTask1();
    }
    else if ((data == 'S')||(data == 'T')||(data == 'U')||(data == 'V')||(data == 'W')) 
    {
      if(data == 'S')
      {
        startTask2(1);
      }
      else if(data == 'T')
      {
        startTask2(2);
      }
      else if(data == 'U')
      {
        startTask2(3);
      }
      else if(data == 'V')
      {
        startTask2(4);
      }
      else if(data == 'W')
      {
        startTask2(5);
      }
    }
    else if (data == 'D') 
    {
      startTask3();
    }
    else if (data == 'I') 
    {      
      Serial.println(Device_ID);
    }
    else if (data == 'R')
    {
      int dac_print=0;
      for (int i=1;i<5;i++)
      {
        Serial.print("LED");Serial.print(i);Serial.print(" DAC : ");Serial.println
       (LED_dac[i-1]);
      }
    }
    else if (data == 'F')
    {
      Serial.print("Firmware Version : ");Serial.println(Firmware_version);
      Serial.print("Firmware Version Autoloaded: ");Serial.println(Firmware_version_Auto);
      Serial.print("Date and Time of Firmware Upload : ");Serial.println(compile_date);
    }
    else if (data == 'P')   
    { 
      Serial.print("RESULT"); Serial.println(" "); Serial.print("LB1 ");   Serial.println(avg_blank[0]); Serial.print("LB2 ");  Serial.println(avg_blank[1]);
      Serial.print("LB3 "); Serial.println(avg_blank[2]); Serial.print("LB4 "); Serial.println(avg_blank[3]);
      Serial.print("LS1 "); Serial.println(avg_sample[0]); Serial.print("LS2 ");  Serial.println(avg_sample[1]); Serial.print("LS3 ");  Serial.println(avg_sample[2]);
      Serial.print("LS4 "); Serial.println(avg_sample[3]);  Serial.println("REND");
      delay(100);
    }
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void startTask1() 
{
  if(appmode==0)
  {
    Serial.print("Process: Buffer start; Timestamp: ");Serial.println(millis());
  }
  all_LEDs_turnoff();
  delay(100);
  all_LEDs_turnoff();
  Serial.println("#BS");
  for (int i=0;i<4;i++)
  {
    setOutput(GAIN_1, LED_dac[i], LED_sequence[i]);
    delay(100);
    avg_blank[i] = detector(LED_unstable_samples[i], LED_sample_size[i], 1, Blank_gain_combinations[i], LED_settling_time[i], LED_reading_delay[i]);
    all_LEDs_turnoff();
    delay(1000);
  }
  if (appmode==0)
  {
    Serial.print(millis());Serial.print("\t");Serial.print("LED1B : "); Serial.print(avg_blank[0]); Serial.print("\t"); Serial.print("LED2B : "); Serial.print(avg_blank[1]); Serial.print("\t"); Serial.print("LED3B : "); Serial.print(avg_blank[2]); Serial.print("\t"); Serial.print("LED4B : "); Serial.println(avg_blank[3]);
  }
  Serial.println("#BC");
  if(appmode==0)
  {
    Serial.print("Process: Buffer end; Timestamp: ");Serial.println(millis());
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void startTask2(int selector) 
{
  if(appmode==0)
  {
    Serial.print("Process: Sample start; Timestamp: ");Serial.println(millis());
  }
  int Sample_gain_combinations[]={0,0,0,0};
  if(selector==1)
  {
    Sample_gain_combinations[0]=Sample_gain_combinationsG1[0];
    Sample_gain_combinations[1]=Sample_gain_combinationsG1[1];
    Sample_gain_combinations[2]=Sample_gain_combinationsG1[2];
    Sample_gain_combinations[3]=Sample_gain_combinationsG1[3];
  }
  if(selector==2)
  {
    Sample_gain_combinations[0]=Sample_gain_combinationsG2[0];
    Sample_gain_combinations[1]=Sample_gain_combinationsG2[1];
    Sample_gain_combinations[2]=Sample_gain_combinationsG2[2];
    Sample_gain_combinations[3]=Sample_gain_combinationsG2[3];
  }
  if(selector==3)
  {
    Sample_gain_combinations[0]=Sample_gain_combinationsG3[0];
    Sample_gain_combinations[1]=Sample_gain_combinationsG3[1];
    Sample_gain_combinations[2]=Sample_gain_combinationsG3[2];
    Sample_gain_combinations[3]=Sample_gain_combinationsG3[3];
  }
  if(selector==4)
  {
    Sample_gain_combinations[0]=Sample_gain_combinationsG4[0];
    Sample_gain_combinations[1]=Sample_gain_combinationsG4[1];
    Sample_gain_combinations[2]=Sample_gain_combinationsG4[2];
    Sample_gain_combinations[3]=Sample_gain_combinationsG4[3];
  }
  if(selector==5)
  {
    Sample_gain_combinations[0]=Sample_gain_combinationsG5[0];
    Sample_gain_combinations[1]=Sample_gain_combinationsG5[1];
    Sample_gain_combinations[2]=Sample_gain_combinationsG5[2];
    Sample_gain_combinations[3]=Sample_gain_combinationsG5[3];
  }
  all_LEDs_turnoff();
  delay(100);
  all_LEDs_turnoff();
  Serial.print("#SS");Serial.println(selector);
  for (int i=0;i<4;i++)
  {
    setOutput(GAIN_1, LED_dac[i], LED_sequence[i]);
    delay(100);
    avg_sample[i] = detector(LED_unstable_samples[i], LED_sample_size[i], 1, Sample_gain_combinations[i],LED_settling_time[i], LED_reading_delay[i]);
    all_LEDs_turnoff();
    delay(1000);
  }
  if (appmode==0)
  {
    Serial.print(millis());Serial.print("\t");Serial.print("LED1S : "); Serial.print(avg_sample[0]); Serial.print("\t"); Serial.print("LED2S : "); Serial.print(avg_sample[1]); Serial.print("\t"); Serial.print("LED3S : "); Serial.print(avg_sample[2]); Serial.print("\t"); Serial.print("LED4S : "); Serial.println(avg_sample[3]);
  }
  Serial.print("#SC");Serial.println(selector);
  if(appmode==0)
  {
    Serial.print("Process: Sample end; Timestamp: ");Serial.println(millis());
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void startTask3() 
{
  if(appmode==0)
  {
    Serial.print("Process: Diagnostics start; Timestamp: ");Serial.println(millis());
  }
  int reading_delay=500;
  int reading_increment=128;
  all_LEDs_turnoff();
  Serial.println("#DS");
  for (j=0;j<4;j++)
  {
    delay(1000);
    for (int i = 0; i <=4096; i += reading_increment)
    {
      setOutput(GAIN_1, i, LED_sequence[j]);
      delay(reading_delay);
      int16_t val_0 = ADS.readADC(0);      
      Serial.print("LED:");Serial.print(j+1);Serial.print("__DAC:");Serial.print(i);Serial.print("__ADC:");Serial.println(val_0);      
    }    
    delay(1000);
  }
  Serial.println("#DC");
  all_LEDs_turnoff();
  delay(1000);
  if(appmode==0)
  {
    Serial.print("Process: Diagnostics end; Timestamp: ");Serial.println(millis());
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////