#ifndef COLORDETECT_H
#define COLORDETECT_H

#include <string>
using namespace std;

// Function Declarations
void translateCodetoThreshold(int code);
void setColorThreshold(String msg, int8_t thres[], int arraySize);

void ColorDetect_Setup(int findcolor){
  translateCodetoThreshold(findcolor);
}

//Interpret a message to change what color the camera is detecting
void setColor(String msg) {
  if (msg.length() < 3){
    int val = msg.toInt();
    translateCodetoThreshold(val);
    Serial.println("code detected");
  } else {
    Serial.println("new threshold detected");
    setColorThreshold(msg, threshold, 6);
  }
  for (int j = 0; j < 6; j++){
    Serial.print(threshold[j]);
    Serial.print(" ");
  }
  Serial.println("");
}

//in case we don't want to maually set each LAB threshold, we can send over an int to
//use preset threshold values instead
void translateCodetoThreshold(int code){
  switch(code){
    case 1: //green old = (30, 100, -68, -13, 30, 127)
      //(30,100,-68,2,6,127) - detect the yellow wall as well
      threshold[0] = 30;
      threshold[1] = 100;
      threshold[2] = -93;
      threshold[3] = -5;
      threshold[4] = 13;
      threshold[5] = 127;
    break;
    case 2: //blue
      threshold[0] = 30;
      threshold[1] = 100;
      threshold[2] = -108;
      threshold[3] = -9;
      threshold[4] = 0;
      threshold[5] = -42;
    break;
    case 5: //red (30, 100, 127, 41, 127, 13)
      threshold[0] = 30;
      threshold[1] = 100;
      threshold[2] = 127;
      threshold[3] = 41;
      threshold[4] = 127;
      threshold[5] = 13;
  }
}


//threshold array must have at least six elements. This function helps 
//translating a message with threshold values to ints. Example msg that would
//be received by this function are "1 2 3 4 5 6" or "-100 70 9 -9 -50 128". 
//NOTE: - the threshold value should be between -128 to 128
//      - the message should not include the command character used by the mesh  
//      - suggested command character: 'C'[olor]
void setColorThreshold(String msg, int8_t thres[], int arraySize){
  int len = msg.length();
  int temp = 0;
  int startpoint = 0;
  for (int i = 0; i < len; i++){
    if (msg[i] == ' '){
      thres[temp] = msg.substring(startpoint,i).toInt();
      startpoint = i + 1;
      temp++;
    }
    if (temp == 5){
      thres[temp] = msg.substring(startpoint).toInt();
      break; 
    }
  }
}

#endif
