#include "ADS1X15.h"
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <math.h>

const int buttonPin = 12;  // Button pin
const int PIN_CS = 10;
const int GAIN_1 = 0x1;
const int GAIN_2 = 0x0;
const unsigned int steps = 512;
unsigned int sines_of_steps[steps];
char data;   //Variable to store the data<

ADS1115 ADS(0x48);

int blue_dac = 3130; //390 2940
int green_dac = 1060; //2320 2860
int green_sample_size = 100;
int blue_sample_size = 100;

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

float avg_storage = 0;
float avg_blueS = 0;
float avg_blueS1 = 0;
float avg_blueS2 = 0;
float avg_blueS3 = 0;
float avg_greenS = 0;
float avg_greenS1 = 0;
float avg_greenS2 = 0;
float avg_greenS3 = 0;
float avg_blueB = 0;
float avg_blueB1 = 0;
float avg_blueB2 = 0;
float avg_blueB3 = 0;
float avg_greenB = 0;
float avg_greenB1 = 0;
float avg_greenB2 = 0;
float avg_greenB3 = 0;
float old_blue = 0;
float old_green = 0;
float avg_green = 0;
float avg_blue = 0;
//
bool buttonState = false;  // Current button state
bool lastButtonState = false;  // Previous button state
//int taskNumber = 1;  // Current task number
bool taskStarted = false;
//int buttonPressCount = 0;
int bs;
int split_counter = 0;
int intensity_display = 1;
int plotter = 0;
int blue_stable_time = 50;
int green_stable_time = 50;

int blue_samples = 10;
int green_samples = 10;

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;

void setup() {

  Serial.begin(115200);
  pinMode(PIN_CS, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);
  SPI.begin();
  SPI.setClockDivider(SPI_CLOCK_DIV2);
  ADS.begin();
}

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

void loop() {
  loopcounter = 0;
  avg_storage = 0;
  split_counter = 0;

  if (Serial.available()) {
    char data = Serial.read();

    if (data == 'B') {
      taskStarted = false;
      Serial.println("#Place Buffer");
      while (digitalRead(buttonPin) == LOW) {
      }
      startTask1();
    }

    else if (data == 'S') {
      taskStarted = false;
      Serial.println("#Place Sample");
      while (digitalRead(buttonPin) == LOW) {
      }
      startTask2();
    }

    else if (data == 'P')   {       //Check if the received character is 1
      Serial.print("RESULT");
      Serial.print(" "); Serial.print("SN HCV1002"); Serial.print(" "); Serial.print(avg_greenB); Serial.print(" "); Serial.print(avg_blueB); Serial.print(" "); Serial.print(avg_greenS); Serial.print(" "); Serial.print(avg_blueS); Serial.print(" "); Serial.print("REND"); Serial.println(" ");

      delay(100);
    }

    else if (data == 'D')   {       //Check if the received character is 1
      startTask3();
    }
    else if (data == 'I')   {       //Check if the received character is 1
      Serial.println("SN HCV1002");
    }
  }
}
void startTask1() {
  Serial.println("#Buffer Started");
  setOutput(1, GAIN_1, 1, 0);
  setOutput(0, GAIN_1, 1, 0);
  delay(500);
  setOutput(1, GAIN_1, 1, 0);
  setOutput(0, GAIN_1, 1, green_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  float f = ADS.toVoltage(1);  // voltage factor
  for (int i = 0; i < green_samples + green_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    if (i >= green_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;
        temp_counter++;
      }
    }

    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Green_Resistor_drop: "); Serial.print(val_2);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_greenB1 = intensity_storage / temp_counter;
  delay(500);

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, blue_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  for (int i = 0; i < blue_samples + blue_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    //Serial.println(moving_average_array[0]);
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    //Serial.println(sorting_array[0]);
    if (i >= blue_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;

        temp_counter++;
      }
    }
    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Blue_Resistor_drop: "); Serial.print(val_3);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_blueB1 = intensity_storage / temp_counter;

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, 0);
  delay (100);

  setOutput(1, GAIN_1, 1, 0);
  setOutput(0, GAIN_1, 1, green_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  f = ADS.toVoltage(1);  // voltage factor
  for (int i = 0; i < green_samples + green_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    if (i >= green_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;
        temp_counter++;
      }
    }

    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Green_Resistor_drop: "); Serial.print(val_2);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_greenB2 = intensity_storage / temp_counter;
  delay(500);

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, blue_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  for (int i = 0; i < blue_samples + blue_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    //Serial.println(moving_average_array[0]);
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    //Serial.println(sorting_array[0]);
    if (i >= blue_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;

        temp_counter++;
      }
    }
    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Blue_Resistor_drop: "); Serial.print(val_3);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_blueB2 = intensity_storage / temp_counter;

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, 0);
  delay (100);
  setOutput(1, GAIN_1, 1, 0);
  setOutput(0, GAIN_1, 1, green_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  f = ADS.toVoltage(1);  // voltage factor
  for (int i = 0; i < green_samples + green_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    if (i >= green_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;
        temp_counter++;
      }
    }

    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Green_Resistor_drop: "); Serial.print(val_2);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_greenB3 = intensity_storage / temp_counter;
  delay(500);

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, blue_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  for (int i = 0; i < blue_samples + blue_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    //Serial.println(moving_average_array[0]);
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    //Serial.println(sorting_array[0]);
    if (i >= blue_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;

        temp_counter++;
      }
    }
    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Blue_Resistor_drop: "); Serial.print(val_3);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_blueB3 = intensity_storage / temp_counter;
  delay(100);
  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, 0);
  avg_blueB = (avg_blueB1 + avg_blueB2 + avg_blueB3) / 3;
  avg_greenB = (avg_greenB1 + avg_greenB2 + avg_greenB3) / 3;
  //  Serial.print("ok Green B Intensity: "); Serial.println(avg_greenB);
  //  Serial.print("ok Blue B Intensity: "); Serial.println(avg_blueB);
  Serial.println("#Buffer Completed");


}
void startTask2() {
  Serial.println("#Sample started");
  setOutput(1, GAIN_1, 1, 0);
  setOutput(0, GAIN_1, 1, green_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  float f = ADS.toVoltage(1);  // voltage factor
  for (int i = 0; i < green_samples + green_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    if (i >= green_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;
        temp_counter++;
      }
    }

    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Green_Resistor_drop: "); Serial.print(val_2);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_greenS1 = intensity_storage / temp_counter;
  delay(500);

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, blue_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  for (int i = 0; i < blue_samples + blue_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    //Serial.println(moving_average_array[0]);
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    //Serial.println(sorting_array[0]);
    if (i >= blue_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;

        temp_counter++;
      }
    }
    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Blue_Resistor_drop: "); Serial.print(val_3);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_blueS1 = intensity_storage / temp_counter;

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, 0);
  delay(100);
  setOutput(1, GAIN_1, 1, 0);
  setOutput(0, GAIN_1, 1, green_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  f = ADS.toVoltage(1);  // voltage factor
  for (int i = 0; i < green_samples + green_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    if (i >= green_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;
        temp_counter++;
      }
    }

    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Green_Resistor_drop: "); Serial.print(val_2);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_greenS2 = intensity_storage / temp_counter;
  delay(500);

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, blue_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  for (int i = 0; i < blue_samples + blue_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    //Serial.println(moving_average_array[0]);
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    //Serial.println(sorting_array[0]);
    if (i >= blue_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;

        temp_counter++;
      }
    }
    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Blue_Resistor_drop: "); Serial.print(val_3);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_blueS2 = intensity_storage / temp_counter;

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, 0);
  delay(100);
  setOutput(1, GAIN_1, 1, 0);
  setOutput(0, GAIN_1, 1, green_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  f = ADS.toVoltage(1);  // voltage factor
  for (int i = 0; i < green_samples + green_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    if (i >= green_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;
        temp_counter++;
      }
    }

    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Green_Resistor_drop: "); Serial.print(val_2);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_greenS3 = intensity_storage / temp_counter;
  delay(500);

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, blue_dac);
  memset (moving_average_array, 0, 10);
  temp_counter = 0;
  intensity_storage = 0;
  for (int i = 0; i < blue_samples + blue_stable_time; 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);
    moving_average_array[i % 10] = val_1;
    //Serial.println(moving_average_array[0]);
    for (int j = 0; j < 10; j++)
    {
      sorting_array[j] = moving_average_array[j];
    }
    //Serial.println(sorting_array[0]);
    if (i >= blue_stable_time)
    {
      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 ((val_1 > (moving_average * 0.9)) and (val_1 < (moving_average * 1.1)))
      {
        intensity_storage += val_1;

        temp_counter++;
      }
    }
    if (intensity_display == 1)
    {
      //      Serial.print("DAC_value : "); Serial.print(i); Serial.print('\t'); Serial.print('\t'); Serial.print("Blue_Resistor_drop: "); Serial.print(val_3);
      //      Serial.print('\t'); Serial.print('\t'); Serial.print("PD_output: "); Serial.print(val_1); Serial.print('\t'); Serial.print('\t'); Serial.print("PD_2X_output: "); Serial.println(val_0);
    }
    if (plotter == 1)
    {
      Serial.println(val_1);
    }
    //delay(200);
  }
  avg_blueS3 = intensity_storage / temp_counter;

  setOutput(0, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, 0);
  avg_greenS =  (avg_greenS1 + avg_greenS2 + avg_greenS3) / 3;
  avg_blueS = (avg_blueS1 + avg_blueS2 + avg_blueS3) / 3;
  //  Serial.print("ok Green Intensity: "); Serial.println(avg_greenS);
  //  Serial.print("ok Blue Intensity: "); Serial.println(avg_blueS);
  Serial.println("#Sample Completed");
  delay(300);
}

void startTask3() {
  ADS.setGain(0);
  loopcounter = 0;
  avg_storage = 0;
  split_counter = 0;
  setOutput(1, GAIN_1, 1, 0);
  setOutput(1, GAIN_1, 1, 0);
  setOutput(0, GAIN_1, 1, 0);
  float f = ADS.toVoltage(1);  // voltage factor
  for (int i = 400; i < 1400; i += 200)
  {
    setOutput(0, GAIN_1, 1, i);
    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);
    {
      Serial.print("PG "); Serial.println(val_1);
    }
  }
  setOutput(0, GAIN_1, 1, 0);
  delay(1000);
  setOutput(1, GAIN_1, 1, 0);
  f = ADS.toVoltage(1);  // voltage factor
  for (int i = 1200; i < 2200; i += 200)
  {
    setOutput(1, GAIN_1, 1, i);
    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);
    {
      Serial.print("PB "); Serial.println(val_1);
    }

  }
  setOutput(1, GAIN_1, 1, 0);
  Serial.println("END");
}