#include "ADS1X15.h"
#include <SPI.h>
#include <Wire.h>
//BLOOD
//DEV_7
//int LED1_dac = 406;
//int LED2_dac = 160;
//int LED3_dac = 218;
//int LED4_dac = 248;

//DEV_8
//int LED1_dac = 340;
//int LED2_dac = 144;
//int LED3_dac = 502;
//int LED4_dac = 190;

//DEV_7
int LED1_dac = 3096;
int LED2_dac = 1900;
int LED3_dac = 3532;
int LED4_dac = 3980;

int LED1_unstable_samples = 20;
int LED2_unstable_samples = 20;
int LED3_unstable_samples = 20;
int LED4_unstable_samples = 20;
int LED1_sample_size = 50;
int LED2_sample_size = 50;
int LED3_sample_size = 50;
int LED4_sample_size = 50;

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);

int loopcounter = 0;
unsigned long long intensity_storage = 0;
int temp_counter = 0;

//long long data_storage=0;
float avg_storage = 0;
float avg_green = 0;
float avg_blue = 0;
float old_green = 0;
float old_blue = 0;
int flag = 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 unstable_samples = 20;
int number_of_readings = 50;
int skip = 0;

float average_readings = 0;
float avg_LED1B = 0;
float avg_LED2B = 0;
float avg_LED3B = 0;
float avg_LED4B = 0;
float avg_LED1S = 0;
float avg_LED2S = 0;
float avg_LED3S = 0;
float avg_LED4S = 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();
}

void setOutput(byte channel, byte gain, byte shutdown, unsigned int val, int DACselect)
{
  byte lowByte = val & 0xff;
  byte highByte = ((val >> 8) & 0xff) | channel << 7 | gain << 5 | shutdown << 4;
  if (DACselect == 1)
  {
    PORTB &= 0xfb;
    SPI.transfer(highByte);
    SPI.transfer(lowByte);
    PORTB |= 0x4;
  }
  else
  {
    PORTB &= 0xfd;
    SPI.transfer(highByte);
    SPI.transfer(lowByte);
    PORTB |= 0x2;
  }
}

float detector(int unstable_samples,  int number_of_readings, int detector_select, int PD1G1_gain_set, int PD1G2_gain_set, int PD2G1_gain_set, int PD2G2_gain_set)
{
  if (PD1G1_gain_set == 0)
  {
    digitalWrite(PD1G1, LOW);
  }
  else
  {
    digitalWrite(PD1G1, HIGH);
  }
  if (PD1G2_gain_set == 0)
  {
    digitalWrite(PD1G2, LOW);
  }
  else
  {
    digitalWrite(PD1G2, HIGH);
  }
  if (PD2G1_gain_set == 0)
  {
    digitalWrite(PD2G1, LOW);
  }
  else
  {
    digitalWrite(PD2G1, HIGH);
  }
  if (PD2G2_gain_set == 0)
  {
    digitalWrite(PD2G2, LOW);
  }
  else
  {
    digitalWrite(PD2G2, HIGH);
  }
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  skip = 0;
  intensity_storage = 0;
  int16_t detector_val = 0;
  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);
    int16_t val_2 = ADS.readADC(2);
    int16_t val_3 = ADS.readADC(3);
    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++;
      }
    }
    if (intensity_display == 1)
    {
    }
  }
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  average_readings = intensity_storage / temp_counter;
  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 == 'B') {

      startTask1();
    }

    else if (data == 'S') {
      startTask2();
    }

    else if (data == 'P')   {       //Check if the received character is 1
      Serial.print("RESULT"); Serial.println(" "); Serial.print("LB1 ");   Serial.println(avg_LED1B); Serial.print("LB2 ");  Serial.println(avg_LED2B);
      Serial.print("LB3 "); Serial.println(avg_LED3B); Serial.print("LB4 "); Serial.println(avg_LED4B);
      Serial.print("LS1 "); Serial.println(avg_LED1S); Serial.print("LS2 ");  Serial.println(avg_LED2S); Serial.print("LS3 ");  Serial.println(avg_LED3S);
      Serial.print("LS4 "); Serial.println(avg_LED4S);  Serial.println("REND");

      delay(100);
    }

    else if (data == 'D')   {       //Check if the received character is 1
      startTask3();
    }

    else if (data == 'C')   {       //Check if the received character is 1
      startTask4();
    }

    else if (data == 'I')   {       //Check if the received character is 1
      Serial.println("SNS HCV-000-3008 SNE");
    }
  }
}




void startTask1() {
  Serial.println("#BS");
  digitalWrite(LEDB, HIGH);
  digitalWrite(LEDR, HIGH);
  digitalWrite(LEDG, LOW);

  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  delay(100);
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  //Serial.println("LED1 is ON");
  setOutput(0, GAIN_1, 1, LED1_dac, 1);
  delay(100);
  avg_LED1B = detector(LED1_unstable_samples, LED1_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED1 intensity :"); Serial.println(avg_LED1);
  delay(1000);



  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  setOutput(1, GAIN_1, 1, LED2_dac, 1);
  delay(100);
  avg_LED2B = detector(LED2_unstable_samples, LED2_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED2 intensity :"); Serial.println(avg_LED2);
  delay(1000);



  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);

  setOutput(0, GAIN_1, 1, LED3_dac, 2);
  delay(100);
  avg_LED3B = detector(LED3_unstable_samples, LED3_sample_size, 1, 1, 0, 0, 0);
  //Serial.print("Average LED3 intensity :"); Serial.println(avg_LED3);
  delay(1000);



  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  setOutput(1, GAIN_1, 1, LED4_dac, 2);
  delay(100);
  avg_LED4B = detector(LED4_unstable_samples, LED4_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED4 intensity :"); Serial.println(avg_LED4);

  delay(100);

  // Serial.print("LED1B : "); Serial.print(avg_LED1B); Serial.print("\t"); Serial.print("LED2B : "); Serial.print(avg_LED2B); Serial.print("\t"); Serial.print("LED3B : "); Serial.print(avg_LED3B); Serial.print("\t"); Serial.print("LED4B : "); Serial.println(avg_LED4B);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDB, LOW);
  digitalWrite(LEDR, HIGH);
  Serial.println("#BC");
}



void startTask2() {
  Serial.println("#SS");
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  delay(100);
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  digitalWrite(LEDB, HIGH);
  digitalWrite(LEDG, LOW);
  digitalWrite(LEDR, HIGH);
  //  digitalWrite(LED1, HIGH);
  //  digitalWrite(LED2, LOW);
  //  digitalWrite(LED3, LOW);
  //  digitalWrite(LED4, LOW);
  //Serial.println("LED1 is ON");
  setOutput(0, GAIN_1, 1, LED1_dac, 1);
  delay(100);
  avg_LED1S = detector(LED1_unstable_samples, LED1_sample_size, 1, 0, 0, 0, 0);
  //  Serial.print("Average LED1 intensity :"); Serial.println(avg_LED1);
  delay(1000);



  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  setOutput(1, GAIN_1, 1, LED2_dac, 1);
  delay(100);
  avg_LED2S = detector(LED2_unstable_samples, LED2_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED2 intensity :"); Serial.println(avg_LED2);
  delay(1000);



  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, LED3_dac, 2);
  delay(100);
  avg_LED3S = detector(LED3_unstable_samples, LED3_sample_size, 1, 1, 0, 0, 0);
  // Serial.print("Average LED3 intensity :"); Serial.println(avg_LED3);
  delay(1000);



  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  //  digitalWrite(LED1, LOW);
  //  digitalWrite(LED2, LOW);
  //  digitalWrite(LED3, LOW);
  //  digitalWrite(LED4, HIGH);
  //Serial.println("LED4 is ON");
  setOutput(1, GAIN_1, 1, LED4_dac, 2);
  delay(100);
  avg_LED4S = detector(LED4_unstable_samples, LED4_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED4 intensity :"); Serial.println(avg_LED4);

  delay(100);

  //Serial.print("LED1S : "); Serial.print(avg_LED1S); Serial.print("\t"); Serial.print("LED2S : "); Serial.print(avg_LED2S); Serial.print("\t"); Serial.print("LED3S : "); Serial.print(avg_LED3S); Serial.print("\t"); Serial.print("LED4S : "); Serial.println(avg_LED4S);
  digitalWrite(LEDG, HIGH);
  digitalWrite(LEDR, LOW);
  digitalWrite(LEDB, HIGH);
  Serial.println("#SC");
}

void startTask3() {

  ADS.setGain(0);
  loopcounter = 0;
  avg_storage = 0;
  split_counter = 0;


  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  Serial.println("LED1 is ON");
  delay(1000);
  for (int i = 100; i < 1200; i += 300)
  {
    setOutput(0, GAIN_1, 1, i, 1);
    delay(50);
    int16_t val_0 = ADS.readADC(0);
    Serial.print(val_0); Serial.print("__"); Serial.println(i);

  }
  //setOutput(0, GAIN_1, 1, LED1_dac, 1);
  //delay(100);
  //avg_LED1=detector(LED1_unstable_samples, LED1_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED1 intensity :");Serial.println(avg_LED1);
  delay(1000);


  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  Serial.println("LED2 is ON");
  delay(1000);
  for (int i = 100; i < 1200; i += 300)
  {
    setOutput(1, GAIN_1, 1, i, 1);
    delay(50);
    int16_t val_0 = ADS.readADC(0);
    Serial.print(val_0); Serial.print("__"); Serial.println(i);

  }
  //setOutput(1, GAIN_1, 1, LED2_dac, 1);
  //delay(100);
  //avg_LED2=detector(LED2_unstable_samples, LED2_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED2 intensity :");Serial.println(avg_LED2);
  delay(1000);



  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  Serial.println("LED3 is ON");
  delay(1000);
  for (int i = 100; i < 1200; i += 300)
  {
    setOutput(0, GAIN_1, 1, i, 2);
    delay(50);
    int16_t val_0 = ADS.readADC(0);
    Serial.print(val_0); Serial.print("__"); Serial.println(i);

  }
  //setOutput(0, GAIN_1, 1, LED3_dac, 2);
  //delay(100);
  //avg_LED3=detector(LED3_unstable_samples, LED3_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED3 intensity :");Serial.println(avg_LED3);
  delay(1000);



  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  Serial.println("LED4 is ON");
  delay(1000);
  for (int i = 100; i < 1200; i += 300)
  {
    setOutput(1, GAIN_1, 1, i, 2);
    delay(50);
    int16_t val_0 = ADS.readADC(0);
    Serial.print(val_0); Serial.print("__"); Serial.println(i);

  }
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
}
void startTask4() {

  ADS.setGain(0);
  loopcounter = 0;
  avg_storage = 0;
  split_counter = 0;
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  delay(100);
  flag = 0;
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  delay(1000);
  Serial.println("LED1 is ON");
  for (int i = 0; i < 500; i += 2)
  {
    setOutput(0, GAIN_1, 1, i, 1);
    delay(500);
    int16_t val_0 = ADS.readADC(0);
    Serial.print(val_0); Serial.print("__"); Serial.println(i);
    if ((val_0 > 22000) && (flag == 0))
    {
      LED1_dac = i;
      flag = 1;
    }
  }
  setOutput(0, GAIN_1, 1, LED1_dac, 1);
//  delay(100);
//  avg_LED1=detector(LED1_unstable_samples, LED1_sample_size, 1, 1, 0, 0, 0);
//  Serial.print("Average LED1 intensity :");Serial.println(avg_LED1);
//  delay(1000);


  flag = 0;
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  delay(1000);
  Serial.println("LED2 is ON");
  for (int i = 0; i < 500; i += 2)
  {
    setOutput(1, GAIN_1, 1, i, 1);
    delay(500);
    int16_t val_0 = ADS.readADC(0);
    Serial.print(val_0); Serial.print("__"); Serial.println(i);
    if ((val_0 > 22000) && (flag == 0))
    {
      LED2_dac = i;
      flag = 1;
    }
  }
  //setOutput(1, GAIN_1, 1, LED2_dac, 1);
  //delay(100);
  //avg_LED2=detector(LED2_unstable_samples, LED2_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED2 intensity :");Serial.println(avg_LED2);
  delay(1000);


  flag = 0;
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  delay(1000);
  Serial.println("LED3 is ON");
  digitalWrite(PD1G1, HIGH);
  for (int i = 100; i < 700; i += 2)
  {
    setOutput(0, GAIN_1, 1, i, 2);
    delay(500);
    int16_t val_0 = ADS.readADC(0);
    Serial.print(val_0); Serial.print("__"); Serial.println(i);
    if ((val_0 > 22000) && (flag == 0))
    {
      LED3_dac = i;
      flag = 1;
    }
  }
  setOutput(0, GAIN_1, 1, LED3_dac, 2);
  delay(100);
//  avg_LED3=detector(LED3_unstable_samples, LED3_sample_size, 1, 1, 0, 0, 0);
//  Serial.print("Average LED3 intensity :");Serial.println(avg_LED3);
  delay(1000);
  digitalWrite(PD1G1, LOW);


  flag = 0;
  setOutput(1, GAIN_1, 1, 0, 1);
  setOutput(0, GAIN_1, 1, 0, 1);
  setOutput(1, GAIN_1, 1, 0, 2);
  setOutput(0, GAIN_1, 1, 0, 2);
  delay(1000);
  Serial.println("LED4 is ON");
  for (int i = 0; i < 500; i += 2)
  {
    setOutput(1, GAIN_1, 1, i, 2);
    delay(500);
    int16_t val_0 = ADS.readADC(0);
    Serial.print(val_0); Serial.print("__"); Serial.println(i);
    if ((val_0 > 22000) && (flag == 0))
    {
      LED4_dac = i;
      flag = 1;
    }
  }
  // setOutput(1, GAIN_1, 1, LED4_dac, 2);
  //delay(100);
  //avg_LED4=detector(LED4_unstable_samples, LED4_sample_size, 1, 0, 0, 0, 0);
  //Serial.print("Average LED4 intensity :");Serial.println(avg_LED4);

  delay(1000);
  Serial.print("LED1 DAC :"); Serial.print(LED1_dac); Serial.print("\t"); Serial.print("LED2 DAC :"); Serial.print(LED2_dac); Serial.print("\t"); Serial.print("LED3 DAC :"); Serial.print(LED3_dac); Serial.print("\t"); Serial.print("LED4 DAC :"); Serial.println(LED4_dac);
  delay(5000);
}