// Umrechnung vom Intervall [-1,1] auf [-799,799]
// Die Umrechnung braucht nicht linear zu sein! Die Motoren drehen schliesslich erst ab ca. Power 500
void setFloatPower(float links, float rechts) {
  int l = ((links>=0) ? 1 : -1)*(sqrt(abs(links)))*799;
  int r = ((rechts>=0) ? 1 : -1)*(sqrt(abs(rechts)))*799;
  robot.motors.setPowers(l,r);
}

// Funktion, die den Werte zwischen -1.0 und 1.0 beschränkt
// z.B. ist clip(4.2)=1.0, clip(-0.42)=-0.42 und clip(-42)=-1.0
float clip(float f) {
  if (f>1.0) {
    return 1.0;
  } else if (f<-1.0) {
    return -1.0;
  } 
  return f;
}

// Parameter P,I,D
float pid[] = {0.9, 0.0, 0.03};


/** 
 *  Der Linie folgen (dir=-1 heisst schwarz ist links).
 */
int lineFollow(int dir) {
  // Diese Werte zuerst experimentell bestimmen!
  int weiss = 380;  // Messwert auf dem weissen Klebeband
  int schwarz = 30;  // Messwert auf dem schwarzen Klebeband
  
  // Sollwert für die Motoren (in [-1,1]), hängt auch vom Unterprogram setFloatPower ab!
  // D.h. wie schnell sollen diese drehen, wenn man genau auf der Linie fährt.
  float soll = 0.65;

  // Zeitpunkt letzter Messung
  long lastus = micros(); // Systemzeit in Microsekunden

  // Wert letzter Messung
  float lastv = 0.0;

  // Wahr/falsch Variable für ersten Durchgang
  bool first = true;

  // Summe der Abweichungen
  float summe = 0.0;

  // Wiederholen
  while (true) {
    // Werte messen (liefert Ganzzahl in [0,1023]
    int wert = ir.measure();
    // Auf Intervall [-1,1] umrechnen, v ist damit auch die Abweichung vom Sollwert 0.0
    float v = dir*(2.0*(wert-schwarz)/(weiss-schwarz) - 1.0);
    float zeit = (micros()-lastus)/1000000.0;   // Zeitdifferenz in Sekunden
    lastus = micros(); // Zeit merken
    float diff = (lastv-v)/zeit;  // Änderung pro Zeit
    lastv = v;   // Messwert merken
    summe = summe + v*zeit;     // Summe der Abweichung, gewichtet mit der Zeit
    if (first) {
      diff = 0.0;  // Differenz macht beim ersten Mal keinen Sinn
    }
    float korrektur = pid[0]*v + pid[1]*summe - pid[2]*diff;
    float links=clip(soll+korrektur);
    float rechts=clip(soll-korrektur);
    setFloatPower(links, rechts);
    if (robot.buttons.get()!=0) { // Button gedrückt (nicht Null)?
      robot.motors.setPowers(0,0);
      break;
    }
    
    //
    // TODO: Weitere Abbruch-Bedingungen formulieren
    //
    
    first = false;  // Ist nicht mehr das erste Mal
  }
}

// PID-Konstanten über das Menü anpassen.
// Achtung, die Konstanten werden bei einem Reset gelöscht. Gute Konstanten im Code oben eintragen!
void adjust_pid() {
  // Titel beim Anpassen
  char *msg[] = {"Set proportional", "Set integral", "Set differential"};
  while (true) {
    int choice = robot.menu.choice("1 setP", "2 setI", "3 setD", "4 abort");
    if (choice==4) {
      break;
    }
    // Anpassen mit Menufunktion, choice ist 1, 2 oder 3. Anpassungsschritt proportional zur Grösse des Parameters
    pid[choice-1] = robot.menu.adjustFloat(msg[choice-1], pid[choice-1], abs(pid[choice-1])/20.0+0.001);
  }
}