/* Sample code for the TI-Basispraktium final line-following and barcode-reading parcours 
 *   
 * author: Lukas Kaul 
 * date: June 2016
 * 
 * This code relies on the Asurino library with modified methods
 * setMotorSpeed()
 * setBackLED()
 * 
 * https://github.com/LuSeKa/Asurino-Library.git
 * 
 */

#include <Asuro.h>

// Instantiate Asuro
Asuro asuro = Asuro();

// type defs
enum MAIN_STATE {followLine, searchLine, scanBarcode, findLine, blinkNTimes, stop};
enum ENCODER_STATE {black, white};
enum SEARCH_LINE_STATE {left, backFromLeft, right, backFromRight, failure};
enum BARCODE_STATE {bright, dark, end};
enum BLINK_STATE {on, off, done};

// globals

// states
enum MAIN_STATE mainState;
enum ENCODER_STATE encoderState[2] {black, black};
enum BARCODE_STATE barcodeState;
enum SEARCH_LINE_STATE searchLineState;
enum BLINK_STATE blinkState;

// intertask variables
int lineSum = 0; /* WRITE: taskReadLineSensor, READ: others */
int lineDiff = 0; /* WRITE: taskReadLineSensor, READ: others */
int targetBlinkCount = 0; /* WRITE: taskScanBarcode, READ: taskBlinkNTimes */
int statusLedColor = GREEN; /* WRITE: many, READ: taskStatusLED */

// task-local variables
int currentBlinkCount = 0; /* taskBlinkNTimes */ 
int ticksSinceLastDark = 0; /* taskScanBarcode */
int barcodeCount = 0; /* taskScanBarcode */
unsigned long encoderValues[2] {0, 0};

// timeouts
unsigned long taskStatusLED_timeout;
unsigned long taskScanBarcode_timeout;
unsigned long taskEncoder_timeout;
unsigned long taskFollowLine_timeout;
unsigned long taskSearchLine_timeout;
unsigned long taskReadLineSensor_timeout;
unsigned long taskFindLine_timeout;
unsigned long taskBlinkNTimes_timeout;
unsigned long taskStop_timeout;

// periods
unsigned long taskStatusLED_period = 250;
unsigned long taskEncoder_period = 3;
unsigned long taskFollowLine_period = 20;
unsigned long taskSearchLine_period = 20;
unsigned long taskScanBarcode_period = 50;
unsigned long taskReadLineSensor_period = 10;
unsigned long taskFindLine_period = 10;
unsigned long taskBlinkNTimes_period = 500;
unsigned long taskStop_period = 500;

// controller parameters

// distances
int turnCount = 100;
int maxBarDistance = 70;

// brightness values
int whiteThreshold = 750;
int minLineContrast = 110;
int odoThrehshold = 120;

// PWMs
int lineBasePWM = 60;
int findLinePWM = 65;
int turnPWM = 70;
int barcodePWM = 70;
int kp_line = 1;





/************************************************/

/*********** taskReadLineSensor ***********/
void taskReadLineSensor() {
  if (taskReadLineSensor_timeout > millis()) return;
  taskReadLineSensor_timeout += taskReadLineSensor_period;
  
  int lineData[2];
  asuro.readLinesensor(lineData);
  lineDiff = lineData[RIGHT] - lineData[LEFT];
  lineSum = lineData[RIGHT] + lineData[LEFT];
}
/************************************************/

/*********** taskEncoder ***********/
void taskEncoder() {
 
  if (taskEncoder_timeout > millis()) return;
  taskEncoder_timeout += taskEncoder_period;
  int odoData[2];
  asuro.readOdometry(odoData);

  int foundTick = 0;
  foundTick += findTick(RIGHT, odoData);
  foundTick +=  findTick(LEFT, odoData);

  if (foundTick != 0) {
    Serial.print(encoderValues[LEFT]);
    Serial.print('\t');
    Serial.println(encoderValues[RIGHT]);
  }
}

int findTick(int side, int* odoData) {
  // high raw values mean dark, low raw values mean bright
    static int lastVal[2] {512, 512}; 
  switch (encoderState[side]) {
    case white: // values are low
      if (odoData[side] < lastVal[side]) {
        lastVal[side] = odoData[side]; // new lowest value
      }
      else if (odoData[side] > (lastVal[side] + odoThrehshold)) { // transition from bright to dark
        lastVal[side] = odoData[side];
        encoderState[side] = black;
        // tick
        encoderValues[side] ++;
        return 1;
      }
      break;
    case black: // values are high
      if (odoData[side] > lastVal[side]) {
        lastVal[side] = odoData[side]; // new highest value
      }
      else if (odoData[side] < (lastVal[side] - odoThrehshold)) { // transition from dark to bright
        lastVal[side] = odoData[side];
        encoderState[side] = white;
        // tick
        encoderValues[side] ++;
        return 1;
      }
      break;
  }
  return 0;
}
/************************************************/

int phototransistorMWOffset(int dez, int n) {
  int sum = 0;
  /*Berechne den Mittelwert des Grundoffsets*/
  for (int i = 0; i < dez; i++) {
    asuro.readLinesensor(lineData);
    sum += (lineData[0] - lineData[1]);   //  Summe der Offsets
  }
  return sum >> n;   /* Mittelwert des Offsets*/

}

/*********** taskFollowLine ***********/
void taskFollowLine() {
  if (taskFollowLine_timeout > millis()) return;
  taskFollowLine_timeout += taskFollowLine_period;
  asuro.setMotorDirection(FWD,FWD);
  
  don = round(phototransistorMWOffset(2, 1));
  x = don - doff;                  // Regelabweichung
  isum += x;
  if (isum > 16000) isum = 16000;        //Begrenzung um Überlauf zu vermeiden
  if (isum < -16000) isum = -16000;
  yi = isum / 625 * ki;             //I-Anteil berechnen
  // yi= ki * isum;
  yd = (x - xalt) * kd;             // D-Anteil berechnen und mit
  yd += drest;                     // nicht berücksichtigtem Rest addieren
  if (yd > 255) drest = yd - 255;      // merke Rest
  else if (yd < -255) drest = yd + 255;
  else drest = 0;
  yp = x * kp;                      // P-Anteil berechnen
  //yp = kp * x;
  y = yp + yi + yd;                  // Gesamtkorrektur
  y2 = y >> 1;                     // Aufteilung auf beide Motoren
  xalt = x;                        // x merken
  speedLeft = speedRight = speed1;
  asuro.setMotorDirection(FWD, FWD);
  if ( y > 5) {                     // nach rechts
    speedLeft = speed1 + y2;         // links beschleunigen
    if (speedLeft > 255) {
      speedLeft = 255;            // falls Begrenzung
      y2 = speedLeft - speed1;      // dann Rest rechts berücksichtigen
    }
    y = y - y2;
    speedRight = speed1 - y;         // rechts abbremsen
    if (speedRight < 0) {
      speedRight = 0;
    }
  }
  if ( y < -5) {                     // nach links
    speedRight = speed1 - y2;         // rechts beschleunigen !!!was speed - y2!!
    if (speedRight > 255) {
      speedRight = 255;            // falls Begrenzung
      y2 = speed1 - speedRight;      // dann Rest links berücksichtigen !!was speed - speedRight!!
    }
    y = y - y2;
    speedLeft = speed1 + y;            // links abbremsen  !! was speed + y!!!
    if (speedLeft < 0) {
      speedLeft = 0;
    }
  }
  leftDir = rightDir = FWD;


  asuro.setMotorDirection(leftDir, rightDir);
  asuro.setMotorSpeed(abs(speedLeft), abs(speedRight));
}
}
void transitionToFollowLine() {
  Serial.println("Transitioning to FollowLine");
  
  /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */
}
/************************************************/




/*********** taskSearchLine ***********/
void taskSearchLine() {
  // make sure encoders are reset before entering this task
	if (taskSearchLine_timeout > millis()) return;
	taskSearchLine_timeout += taskSearchLine_period;
	
	switch(searchLineState) {
	case left:
		panAndSearch(RIGHT, backFromLeft); // turn right motor to search left
		break;
	case right:
		panAndSearch(LEFT, backFromRight); // turn left motor to search right
		break;
	case backFromLeft:
		backToCenter(RIGHT, right); // go back to center position
		break;
	case backFromRight:
		backToCenter(LEFT, failure); // go back to center position
		break;
	case failure: // the line does not continue
		asuro.setMotorSpeed(0, 0);
		Serial.println("Reached end of line");
		transitionToScanBarcode();
		break;
	}
  /* @@@ TODO: implement sub-statemachine on searchLineState, call panAndSearch, backToCenter or transition */
}

void panAndSearch(int side, SEARCH_LINE_STATE nextState) {
  /* @@@ TODO: control motors; if line is found transition main-statemachine, if encoderValues[side] > turnCount transition sub-statemachine(searchLineState) */
  if
}

void backToCenter(int side, SEARCH_LINE_STATE nextState) {
  /* @@@ TODO: control motors; if encoderValues[side] > turnCount transition sub-statemachine(searchLineState) */
}

void transitionToSearchLine() {
  Serial.println("Transitioning to SearchLine");
  /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */

}
/************************************************/

/*********** taskScanBarcode ***********/
void taskScanBarcode() {
  if (taskScanBarcode_timeout > millis()) return;
  taskScanBarcode_timeout += taskScanBarcode_period;
  
  void taskScanBarcode() {
/*
insert the usual pattern here
*/
	switch (barcodeState) { //bright, dark or end
		case bright:
			ticksSinceLastDark = encoderValues[RIGHT] + encoderValues[LEFT]; // keep track of distance
			if (ticksSinceLastDark > maxBarDistance) { // reached the end of the barcode
				barcodeState = end;
			break;
			}
			if (!noLine()) { // bar detected
				barcodeState = dark;
			break;
			}
			break;
		case dark:
			if (noLine()) { // transition from black to white
				barcodeCount++; // this transition counts as bar
				barcodeState = bright;
			}
			resetEncoders(); // start measuring distance from here
			break;
		case end:
			asuro.setMotorSpeed(0, 0);
			if (barcodeCount == 1) {
				transitionToStop();
			}
			else {
				statusBlinkCount = barcodeCount;
				transitionToBlinkNTimes(); // go to blink task
			}
			break;
			}

}

void transitionToScanBarcode() {
  Serial.println("Transitioning to ScanBarcode");
  /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */

}
/************************************************/

/*********** taskBlinkNTimes ***********/
void taskBlinkNTimes() {
  if (taskBlinkNTimes_timeout > millis()) return;
  taskBlinkNTimes_timeout += taskBlinkNTimes_period;
  
  /* @@@ TODO: implement sub-statemachine on blinkState */
}

void transitionToBlinkNTimes() {
  Serial.println("Transitioning to signal blinking");
  /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */

}
/************************************************/

/*********** taskFindLine ***********/
void taskFindLine() {
  if (taskFindLine_timeout > millis()) return;
  taskFindLine_timeout += taskFindLine_period;

  /* @@@ TODO: transition when line is found */
}

void transitionToFindLine() {
  Serial.println("Transitioning to findLine");
  /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led; set motor speeds? */

}
/************************************************/

/*********** taskStop ***********/
void taskStop() {
  if (taskStop_timeout > millis()) return;
  taskStop_timeout += taskStop_period;

  Serial.println("Parcours finished. Hurray :)");
}

void transitionToStop() {
  Serial.println("Transitioning to Stop");
  /* @@@ TODO: set mainState; reset sub-state; reset task timeouts; reset task variables; set status led */

}
/************************************************/


/*********** helper functions ***********/
void resetEncoders() {
  encoderValues[RIGHT] = 0;
  encoderValues[LEFT] = 0;
}

bool noLine() {
  return (abs(lineDiff) < minLineContrast) && (lineSum > whiteThreshold);
}
/************************************************/
/************* findTickLine ****************/
int findTickLine(int side, int* lineData) {
  // high raw values mean dark, low raw values mean bright
  static int lastVal[2] {512, 512}; 
  switch (line_state[side]) {
    case white: // values are low
      if (lineData[side] < lastVal[side]) {
        lastVal[side] = lineData[side]; // new lowest value
      }
      else if (lineData[side] > (lastVal[side] + odoThrehshold)) { // transition from bright to dark
        lastVal[side] = lineData[side];
        line_state[side] = black;
        // tick
        lineValues[side] ++;
        return 1;
      }
      break;
    case black: // values are high
      if (lineData[side] > lastVal[side]) {
        lastVal[side] = lineData[side]; // new highest value
      }
      else if (lineData[side] < (lastVal[side] - odoThrehshold)) { // transition from dark to bright
        lastVal[side] = lineData[side];
        line_state[side] = white;
        // tick
        lineValues[side] ++;
        return 1;
      }
      break;
  }
  return 0;
}

/********* taskLine ***********/
void taskLine() {
  if (taskEncoder_timeout > millis()) return;
  taskEncoder_timeout += taskEncoder_period;
  int lineData[2];
  asuro.readLinesensor(lineData);

  int foundTick = 0;
  foundTick += findTickLine(RIGHT, lineData);
  foundTick +=  findTickLine(LEFT, lineData);

  }
}
/*********** taskStatusLED ***********/
void taskStatusLED() {
  // blink status led green
  if (taskStatusLED_timeout > millis()) return;
  taskStatusLED_timeout += taskStatusLED_period;
  
  taskLine();
  if(line_state[0] == white && line_state[1] == white)  asuro.setStatusLED(GREEN);
  else if ((line_state[0] == white && line_state[1] == black) || ((line_state[0] == black && line_state[1] == white))) asuro.setStatusLED(RED);
  else asuro.setStatusLED(OFF);
}

/*********** Init ***********/
void hardwareInit() {
  // delays for robustness
  // using delays *before* the main loop is OK
  asuro.Init();
  delay(100);
  Serial.begin(2400);
  delay(100);
  asuro.setFrontLED(ON);
  delay(100);
  Serial.println("Hardware ready");
  delay(100);
}
/************************************************/

/*********** main ***********/
void setup() {
  hardwareInit();
  transitionToFollowLine();
}
void loop() {
  // main state machine
  switch (mainState) {
    
    case followLine:
      taskStatusLED();
      taskReadLineSensor();
      taskFollowLine();
      break;
      
    case searchLine:
      taskStatusLED();
      taskReadLineSensor();
      taskEncoder();
      taskSearchLine();
      break;
      
    case scanBarcode:
      taskStatusLED();
      taskReadLineSensor();
      taskEncoder();
      taskScanBarcode();
      break;
      
    case blinkNTimes:
      taskBlinkNTimes();
      break;
      
    case findLine:
      taskStatusLED();
      taskReadLineSensor();
      taskFindLine();
      break;
      
    case stop:
      taskStatusLED();
      taskStop();
    }
}
/************************************************/