#define NUM_SAMPLES 10

#define PH_INDICATOR A5
#define SET_POINT A4

#define PH_MULTIPLIER 4
#define MAX_PH 14
#define MIN_PH 0

#define ACID_PIN 5
#define ALKALI_PIN 6

#define HYSTERSIS 1

#define DESIRED_DELAY 1000

#define ADC_RESOLUTION 10

//#define DEBUG_PRINT
#define DEBUG(x) Serial.print(x)
#define DEBUG_LN(x) Serial.println(x)

uint16_t x[NUM_SAMPLES] = {1,   3,   5,   7, 12, 18, 25,  77, 88, 100};
//Ensure that the X list is sorted!! - No sorting done here
float y[NUM_SAMPLES] =    {0, 0.2, 0.3, 0.5,  1,  2, 3, 5.2,  9, 10};
float approx;

uint8_t i = 0;


void setup() {
  Serial.begin(115200);
  pinMode(ACID_PIN,OUTPUT);
  pinMode(ALKALI_PIN,OUTPUT);
  while(!Serial);
  Serial.println("Hello Chem Eng, I got bored...");
  Serial.println("------------------------------");
  Serial.println("RED_LED adds ALKALI");
  Serial.println("YELLOW_LED adds ACID");
  Serial.println("LEFT_KNOB is flash PH");
  Serial.println("RIGHT KNOB is setpoint PH");
  Serial.println("------------------------------");
  delay(10000);
  // put your setup code here, to run once:

}

void loop() {
  uint32_t time1 = millis();
  uint16_t pot = analogRead(PH_INDICATOR);
  uint16_t pot2 = analogRead(SET_POINT);
  
  float desired_ph = map(pot2, 0,1<<ADC_RESOLUTION,MIN_PH,PH_MULTIPLIER*MAX_PH)/PH_MULTIPLIER;
  uint16_t val = map(pot, 0,1<<ADC_RESOLUTION,0,x[NUM_SAMPLES-1]);

  approx = approximate(val);

  Serial.print("adc_val:");
  Serial.print(val);
  Serial.print(",set_p:");
  Serial.print(desired_ph);
  Serial.print(",hyster:");
  Serial.print(HYSTERSIS);
  Serial.print(",approx_ph:");
  Serial.println(approx);

  if(desired_ph > approx+HYSTERSIS) {
    digitalWrite(ACID_PIN,HIGH);
    digitalWrite(ALKALI_PIN,LOW);
  }
  else if(desired_ph < approx-HYSTERSIS) {
    digitalWrite(ACID_PIN,LOW);
    digitalWrite(ALKALI_PIN,HIGH);
  }
  else {
    digitalWrite(ACID_PIN,LOW);
    digitalWrite(ALKALI_PIN,LOW);
  }

  uint32_t time2 = millis();
  uint16_t del = 0;
  if((time2-time1)<DESIRED_DELAY) {
    del = DESIRED_DELAY - (time2-time1);
  }
  delay(del);
  // put your main code here, to run repeatedly:
}

uint8_t find_closest_min(uint16_t value) {
  uint8_t iteration = 0;
  int16_t diff = 32767;
  while(iteration<NUM_SAMPLES-1) {
    diff = value - x[iteration];
    if(diff<0) {
      return iteration-1;
    }
    iteration++;
  }
  return iteration;
}

float approximate(uint16_t x_val) {
  uint8_t idx = find_closest_min(x_val);
  uint8_t idx2 = idx+1;
  if(idx == NUM_SAMPLES-1) {
    idx = NUM_SAMPLES-2;
    idx2 = NUM_SAMPLES-1;
  }
  
  float slope = (y[idx2]-y[idx])/(x[idx2]-x[idx]);
  float c = y[idx2]-slope*x[idx2];
  return slope*x_val+c;
}