enum lineStates {
  zero,   // 000000 <-
  one,    // 100000
  //two,    // 110000
  three,  // 010000
  //four,   // 011000
  five,   // 001000
  six,    // 001100
  seven,  // 000100
  //eight,  // 000110
  nine,   // 000010
  //ten,    // 000011
  eleven, // 000001
  twelve,  // 000000 ->
  thirteen // 000000 -^-

};


//ta bort dubbel och kolla höger/vänster på enkel state


lineStates lineState = thirteen;
void handleSensors(String sensorReading)
{




  switch(lineState) {
    case zero:
      stateZero(sensorReading);
      // Serial.print("Zero");
      break;
    case one:
      stateOne(sensorReading);
      // Serial.print("One");
      break;
    //case two:
      //stateTwo(sensorReading);
      // Serial.print("Two");
      //break;
    case three:
      stateThree(sensorReading);
      // Serial.print("Three");
      break;
    //case four:
      //stateFour(sensorReading);
      // Serial.print("Four");
      //break;
    case five:
      stateFive(sensorReading);
      // Serial.print("Five");
      break;
    case six:
      stateSix(sensorReading);
       //Serial.print("Six");
      break;
    case seven:
      stateSeven(sensorReading);
      // Serial.print("Seven");
      break;
    //case eight:
      //stateEight(sensorReading);
      // Serial.print("Eight");
      //break;
    case nine:
      stateNine(sensorReading);
      // Serial.print("Nine");
      break;
    //case ten:
      //stateTen(sensorReading);
      // Serial.print("Ten");
      //break;
    case eleven:
      stateEleven(sensorReading);
      // Serial.print("Eleven");
      break;
    case twelve:
      stateTwelve(sensorReading);
      // Serial.print("Twelve");
      break;
    case thirteen:
      stateThirteen(sensorReading);
      // Serial.print("Thirteen");
      break;
  }
}
/*
 * Handle sensor inputs when line is to far to the left ( <- 000000 )
 */
void stateZero(String sensorReading)
{
  Serial.print("zero: ");
  Serial.println(sensorReading);
  String importantSensors = sensorReading.substring(0,1);

  if (importantSensors == "1") {
    setSteering(120);
    lineState = one;
  }
   reference_value = 0.4;
}

/*
 * Handle sensor input when last reading was 100000
 */
void stateOne(String sensorReading)
{
  Serial.print("one: ");
  Serial.println(sensorReading);
  String importantSensors = sensorReading.substring(0,2);
  reference_value = 0.4;
  if (importantSensors == "00") {
    setSteering(120);
    lineState = zero;
  } else if(importantSensors == "11" ||importantSensors == "01") {
    setSteering(48);
    lineState = three;
//    reference_value = ref_speed * 0.8;
  }
}

/*
 * Handle sensor input when last reading was 110000
 */
//void stateTwo(String sensorReading)
//{
//  String importantSensors = sensorReading.substring(0,2);
//  if (importantSensors == "10") {
//    setSteering(1500);
//    lineState = one;
//  } else if (importantSensors == "01") {
//    setSteering(1400);
//    lineState = three;
//  }
//}
/*
 * Hande sensor input when last reading was 010000
 */
void stateThree(String sensorReading)
{
  Serial.print("three: ");
  Serial.println(sensorReading);
  reference_value = 0.6;
  String importantSensors = sensorReading.substring(0,3);
  if ((importantSensors == "110") || (importantSensors =="100")) {
    setSteering(108);
    lineState = one;
  } else if ((importantSensors == "011") || (importantSensors == "001")) {
    setSteering(48); // sväng tillbaks
    lineState = five;
  }

}

/*
 * 011000 previous value
 */
//void stateFour(String sensorReading)
//{
//  String importantSensors = sensorReading.substring(1,3);
//  if (importantSensors == "10") {
//    setSteering(1400);
//    lineState = three;
//  } else if (importantSensors == "01") {
//    setSteering(1300);
//    lineState = five;
//  }
//}
/*
 * 001000
 */
void stateFive(String sensorReading)
{
  Serial.print("five: ");
  Serial.println(sensorReading);
  reference_value = 0.6;
  String importantSensors = sensorReading.substring(1,4); // 011100
  if ((importantSensors == "110") || (importantSensors == "100")) {
    setSteering(72);
    lineState = three;
  } else if (importantSensors == "011" || importantSensors == "001") {
    setSteering(48);
    lineState = six;
  }
}
/*
 * 001100
 */
void stateSix(String sensorReading)
{
  Serial.print("six: ");
  Serial.println(sensorReading);
  reference_value = 0.6;
  String importantSensors = sensorReading.substring(2,4);
  if (importantSensors == "10") {
    setSteering(72);
    lineState = five;
  } else if (importantSensors == "01") {
    setSteering(48);
    lineState = seven;
  }
}
/*
 * 000100
 */
void stateSeven(String sensorReading)
{
  Serial.print("seven: ");
  Serial.println(sensorReading);
  reference_value = 0.6;
  String importantSensors = sensorReading.substring(2,5);
  if (importantSensors == "011" || importantSensors == "001") {
    setSteering(48);
    lineState = nine;
  } else if (importantSensors == "110" || importantSensors == "100") {
    setSteering(72);
    lineState = six;
  }
}
/*
 * 000110
 */
//void stateEight(String sensorReading)
//{
//  String importantSensors = sensorReading.substring(3,5);
//  if (importantSensors == "10") {
//    setSteering(1200);
//    lineState = seven;
//  } else if (importantSensors == "01") {
//    setSteering(1100);
//    lineState = nine;
//  }
//}
/*
 * 000010
 */
void stateNine(String sensorReading)
{
  Serial.print("nine: ");
  Serial.println(sensorReading);
  reference_value = 0.6;
  String importantSensors = sensorReading.substring(3,6);
  if ((importantSensors == "011") || (importantSensors == "001")) {
    setSteering(12);
    lineState = eleven;
  } else if ((importantSensors == "110") || (importantSensors == "100")) {
    setSteering(72);
    lineState = seven;
  }
}
/*
 * 000011
 */
//void stateTen(String sensorReading)
//{
//  String importantSensors = sensorReading.substring(4);
//  if (importantSensors == "10") {
//    setSteering(1100);
//    lineState = nine;
//  } else if (importantSensors == "01") {
//    setSteering(1000);
//    lineState = eleven;
//  }
//}
/*
 * 000001
 */
void stateEleven(String sensorReading)
{
  Serial.print("eleven: ");
  Serial.println(sensorReading);
  String importantSensors = sensorReading.substring(4,6);
  reference_value = 0.4;
  if (importantSensors == "00") {
    setSteering(0);
    lineState = twelve;
  } else if ((importantSensors == "11"  || (importantSensors == "10"))) {
    setSteering(72);
    lineState = nine;
  }
}
/*
 * Dissapeared to the right ( 000000 -> )
 */
void stateTwelve(String sensorReading)
{
  Serial.print("twelve: ");
  Serial.println(sensorReading);
  reference_value = 0.4;
  String importantSensors = sensorReading.substring(5,6);
  if (importantSensors == "1") {
    setSteering(0);
    lineState = eleven;
  }
}
/*
 * START STATE
 */
void stateThirteen(String sensorReading)
{
  Serial.print("thirteen: ");
  Serial.println(sensorReading);
  String importantSensors = sensorReading.substring(2,4);
  if (importantSensors == "11") {
    setSteering(60);
    lineState = six;
  }
}