import garciadelcastillo.dashedlines.*; //<>//
import processing.serial.*;
import controlP5.*;

import java.util.Queue;
import java.util.LinkedList;
import java.util.concurrent.ConcurrentLinkedQueue;
import java.util.regex.Pattern;


static final Pattern FLOAT_REGEX = Pattern.compile("^-?(?:0|[1-9]\\d*)(?:\\.\\d+)?$");

static final String COM_PORT = "COM7";
static final int speedStart = 0, speedEnd = 240;
static final int minSampleSize = 50, maxSampleSize = 600;
static final float minTimeSpan = 5F, maxTimeSpan = 60F;

static final float[] graphPos = new float[2];

DashedLines dash;
PFont consolas_16, consolas_16_bold;

ControlP5 cp5;
Textfield kpInput, kiInput, kdInput;
Textfield errorRangeMinInput, errorRangeMaxInput, integralRangeMinInput, integralRangeMaxInput, derivativeRangeMinInput, derivativeRangeMaxInput, speedRangeMinInput, speedRangeMaxInput, accelerationRangeMinInput, accelerationRangeMaxInput, servoRangeMinInput, servoRangeMaxInput;
Toggle autoScaleErrorToggle, autoScaleIntegralToggle, autoScaleDerivativeToggle, autoScaleSpeedToggle, autoScaleAccelerationToggle, autoScaleServoToggle;
Slider sampleSizeSlider, timeSpanSlider;
Toggle sampleOrTimeGraphToggle;
Toggle smoothCurvesToggle;

Rectangle kpArduinoRect, kiArduinoRect, kdArduinoRect;
Rectangle errorGraphLabelRect, integralGraphLabelRect, derivativeGraphLabelRect, speedGraphLabelRect, accelerationGraphLabelRect, servoGraphLabelRect;
Rectangle speedGaugeRect, graphRect, statusRect;
Rectangle stateRect, switchFlagRect, speedDiffFlagRect, minSpeedFlagRect, maxSpeedFlagRect, brakeFlagRect, relayRect, satellitesRect;

Serial serial;
boolean hasIgnoredFirstPacket, hasReadSecondPacket;
int timeZero;

final Buffer<Sample> samples = Buffer.boundedList(maxSampleSize);
final Queue<Sample> sampleQueue = new LinkedList<>();
int sampleSize;
float timeSpan;

boolean showError = true, showIntegral = true, showDerivative = true, showSpeed, showAcceleration, showServo;
boolean autoScaleError, autoScaleIntegral = true, autoScaleDerivative = true, autoScaleSpeed, autoScaleAcceleration = true, autoScaleServo = true;
float errorRangeMin = -3F, errorRangeMax = 3F, integralRangeMin = Float.NaN, integralRangeMax = Float.NaN, derivativeRangeMin = Float.NaN, derivativeRangeMax = Float.NaN, speedRangeMin = 0F, speedRangeMax = 150F, accelerationRangeMin = Float.NaN, accelerationRangeMax = Float.NaN;
int servoRangeMin = 1170, servoRangeMax = 1970;
boolean sampleOrTimeGraph = false;
boolean drawSmoothCurves;


void setup() {
  size(1600, 512);
  pixelDensity(1);

  windowTitle("Cruise Controller MX5");
  
  speedGaugeRect = Rectangle.xywh(32, 32, 300, 300);
  graphRect = Rectangle.tlbr(speedGaugeRect.top, speedGaugeRect.right + 32, speedGaugeRect.top + 260, width - 32);
  statusRect = Rectangle.xywh(-1, height - 32, width + 1, 32);

  var splitRect = statusRect.splitVerticallyInto(8);
  stateRect = splitRect[0];
  switchFlagRect = splitRect[1];
  speedDiffFlagRect = splitRect[2];
  minSpeedFlagRect = splitRect[3];
  maxSpeedFlagRect = splitRect[4];
  brakeFlagRect = splitRect[5];
  relayRect = splitRect[6];
  satellitesRect = splitRect[7];

  consolas_16 = createFont("Consolas", 16, true);
  consolas_16_bold = createFont("Consolas Bold", 16, true);
  textFont(consolas_16);

  dash = new DashedLines(this);

  errorGraphLabelRect = Rectangle.xywh(graphRect.left, graphRect.top - 23, floor(textWidth("[ERREUR]")), 18);
  integralGraphLabelRect = Rectangle.xywh(errorGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[INTÉGRAL]")), errorGraphLabelRect.height);
  derivativeGraphLabelRect = Rectangle.xywh(integralGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[DÉRIVATIF]")), errorGraphLabelRect.height);
  speedGraphLabelRect = Rectangle.xywh(derivativeGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[VITESSE]")), errorGraphLabelRect.height);
  accelerationGraphLabelRect = Rectangle.xywh(speedGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[ACCÉLÉRATION]")), errorGraphLabelRect.height);
  servoGraphLabelRect = Rectangle.xywh(accelerationGraphLabelRect.right + 16, errorGraphLabelRect.top, floor(textWidth("[SERVO]")), errorGraphLabelRect.height);

  cp5 = new ControlP5(this);
  ControlFont font = new ControlFont(consolas_16);

  kpInput = cp5.addTextfield("_kpInput")
    .setPosition(87, 344)
    .setSize(91, 24)
    .setFont(font)
    .setCaptionLabel("")
    .setId(1);

  kiInput = cp5.addTextfield("_kiInput")
    .setPosition(87, 374)
    .setSize(91, 24)
    .setFont(font)
    .setCaptionLabel("")
    .setId(2);

  kdInput = cp5.addTextfield("_kdInput")
    .setPosition(87, 404)
    .setSize(91, 24)
    .setFont(font)
    .setCaptionLabel("")
    .setId(3);

  sampleSizeSlider = cp5.addSlider("sampleSize")
    .setPosition(graphRect.left + floor(textWidth("Nombre d'échantillons")) + 8, graphRect.bottom + 9)
    .setSize((maxSampleSize - minSampleSize) / 3, 20)
    .setSliderMode(Slider.FLEXIBLE)
    .setRange(minSampleSize, maxSampleSize)
    .setValue(100)
    .setNumberOfTickMarks((maxSampleSize - minSampleSize) / 10 + 1)
    .showTickMarks(false)
    .snapToTickMarks(true)
    .setFont(font)
    .setCaptionLabel("");
  var pos = sampleSizeSlider.getPosition();

  sampleOrTimeGraphToggle = cp5.addToggle("sampleOrTimeGraph")
    .setPosition(pos[0] + sampleSizeSlider.getWidth() + 16F, pos[1])
    .setSize(50, 20)
    .setMode(ControlP5.SWITCH)
    .setCaptionLabel("");
  pos = sampleOrTimeGraphToggle.getPosition();

  timeSpanSlider = cp5.addSlider("timeSpan")
    .setPosition(pos[0] + sampleOrTimeGraphToggle.getWidth() + 16F + floor(textWidth("Intervalle de temps")) + 8F, pos[1])
    .setSize(int(maxTimeSpan - minTimeSpan) * 3, 20)
    .setSliderMode(Slider.FLEXIBLE)
    .setRange(minTimeSpan, maxTimeSpan)
    .setValue(minTimeSpan * 2)
    .setNumberOfTickMarks(int(maxTimeSpan - minTimeSpan) + 1)
    .showTickMarks(false)
    .snapToTickMarks(true)
    .setFont(font)
    .setCaptionLabel("");

  //var saveSamplesButton = cp5.addButton("saveSamples")
  //  .setPosition(width - 32 - 80, 8)
  //  .setSize(80, 20)
  //  .setFont(font)
  //  .setCaptionLabel("Sauver");
  //pos = saveSamplesButton.getPosition();

  smoothCurvesToggle = cp5.addToggle("drawSmoothCurves")
    .setPosition(width - 32 - 150, 8)
    .setSize(150, 20)
    .setFont(font)
    .setCaptionLabel("Courbes lisses");
  smoothCurvesToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);

  errorRangeMinInput = cp5.addTextfield("setErrorRangeMin")
    .setPosition(1323, 323)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(errorRangeMin)) {
    errorRangeMinInput.setText(str(errorRangeMin));
  }

  errorRangeMaxInput = cp5.addTextfield("setErrorRangeMax")
    .setPosition(1418, 323)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(errorRangeMax)) {
    errorRangeMaxInput.setText(str(errorRangeMax));
  }

  integralRangeMinInput = cp5.addTextfield("setIntegralRangeMin")
    .setPosition(1323, 347)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(integralRangeMin)) {
    integralRangeMinInput.setText(str(integralRangeMin));
  }

  integralRangeMaxInput = cp5.addTextfield("setIntegralRangeMax")
    .setPosition(1418, 347)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(integralRangeMax)) {
    integralRangeMaxInput.setText(str(integralRangeMax));
  }

  derivativeRangeMinInput = cp5.addTextfield("setDerivativeRangeMin")
    .setPosition(1323, 371)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(derivativeRangeMin)) {
    derivativeRangeMinInput.setText(str(derivativeRangeMin));
  }

  derivativeRangeMaxInput = cp5.addTextfield("setDerivativeRangeMax")
    .setPosition(1418, 371)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(derivativeRangeMax)) {
    derivativeRangeMaxInput.setText(str(derivativeRangeMax));
  }

  speedRangeMinInput = cp5.addTextfield("setSpeedRangeMin")
    .setPosition(1323, 395)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(speedRangeMin)) {
    speedRangeMinInput.setText(str(speedRangeMin));
  }

  speedRangeMaxInput = cp5.addTextfield("setSpeedRangeMax")
    .setPosition(1418, 395)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(speedRangeMax)) {
    speedRangeMaxInput.setText(str(speedRangeMax));
  }

  accelerationRangeMinInput = cp5.addTextfield("setAccelerationRangeMin")
    .setPosition(1323, 419)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(accelerationRangeMin)) {
    accelerationRangeMinInput.setText(str(accelerationRangeMin));
  }

  accelerationRangeMaxInput = cp5.addTextfield("setAccelerationRangeMax")
    .setPosition(1418, 419)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (!Float.isNaN(accelerationRangeMax)) {
    accelerationRangeMaxInput.setText(str(accelerationRangeMax));
  }

  servoRangeMinInput = cp5.addTextfield("setServoRangeMin")
    .setPosition(1323, 443)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (servoRangeMin != Integer.MIN_VALUE) {
    servoRangeMinInput.setText(str(servoRangeMin));
  }

  servoRangeMaxInput = cp5.addTextfield("setServoRangeMax")
    .setPosition(1418, 443)
    .setSize(91, 22)
    .setAutoClear(false)
    .setFont(font)
    .setCaptionLabel("");
  if (servoRangeMax != Integer.MIN_VALUE) {
    servoRangeMaxInput.setText(str(servoRangeMax));
  }

  autoScaleErrorToggle = cp5.addToggle("autoScaleError")
    .setPosition(width - 32 - 50, 324)
    .setSize(50, 20)
    .setFont(font);
  autoScaleErrorToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
  updateAutoScaleToggleLabel(autoScaleErrorToggle);

  autoScaleIntegralToggle = cp5.addToggle("autoScaleIntegral")
    .setPosition(width - 32 - 50, 348)
    .setSize(50, 20)
    .setFont(font);
  autoScaleIntegralToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
  updateAutoScaleToggleLabel(autoScaleIntegralToggle);

  autoScaleDerivativeToggle = cp5.addToggle("autoScaleDerivative")
    .setPosition(width - 32 - 50, 372)
    .setSize(50, 20)
    .setFont(font);
  autoScaleDerivativeToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
  updateAutoScaleToggleLabel(autoScaleDerivativeToggle);

  autoScaleSpeedToggle = cp5.addToggle("autoScaleSpeed")
    .setPosition(width - 32 - 50, 396)
    .setSize(50, 20)
    .setFont(font);
  autoScaleSpeedToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
  updateAutoScaleToggleLabel(autoScaleSpeedToggle);

  autoScaleAccelerationToggle = cp5.addToggle("autoScaleAcceleration")
    .setPosition(width - 32 - 50, 420)
    .setSize(50, 20)
    .setFont(font);
  autoScaleAccelerationToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
  updateAutoScaleToggleLabel(autoScaleAccelerationToggle);

  autoScaleServoToggle = cp5.addToggle("autoScaleServo")
    .setPosition(width - 32 - 50, 444)
    .setSize(50, 20)
    .setFont(font);
  autoScaleServoToggle.getCaptionLabel().align(ControlP5.CENTER, ControlP5.CENTER);
  updateAutoScaleToggleLabel(autoScaleServoToggle);

  kpArduinoRect = Rectangle.xywh(186, 344, 91, 23);
  kiArduinoRect = Rectangle.xywh(kpArduinoRect.left, kpArduinoRect.top + 30, kpArduinoRect.width, kpArduinoRect.height);
  kdArduinoRect = Rectangle.xywh(kpArduinoRect.left, kiArduinoRect.top + 30, kpArduinoRect.width, kpArduinoRect.height);

  println("Available serial ports");
  printArray(Serial.list());
  println("Trying %s".formatted(COM_PORT));

  try {
    serial = new Serial(this, COM_PORT, 115200);
    serial.bufferUntil('\n');
  }
  catch (Exception e) {
    println(e.getMessage());
  }
}

void draw() {
  var frameTimeStart = millis();

  background(0);
  textFont(consolas_16);

  while (!sampleQueue.isEmpty()) {
    samples.add(sampleQueue.poll());
  }

  var latestSample = samples.get();

  drawSpeedGauge(latestSample);
  drawKPID(latestSample);
  drawGraph(frameTimeStart);
  drawValues(latestSample);
  drawStatusBar(latestSample);

  pushStyle();

  fill(200);
  textAlign(LEFT, TOP);
  text("%.1f fps / %d mspf".formatted(frameRate, millis() - frameTimeStart), 4, 4);

  popStyle();
}

void drawSpeedGauge(Sample sample) {
  push();

  var speedGaugeDiameter = speedGaugeRect.width;
  var speedGaugeRadius = speedGaugeDiameter / 2F;
  var speedGaugeX = speedGaugeRect.centerX();
  var speedGaugeY = speedGaugeRect.centerY();
  var speedGaugeStart = PI - QUARTER_PI;
  var speedGaugeEnd = TWO_PI + QUARTER_PI;
  float speed = speedStart;

  if (sample != null) {
    speed = constrain(sample.speed, speedStart, speedEnd);

    pushStyle();

    stroke(159, 0, 0);
    strokeWeight(8);
    strokeCap(SQUARE);
    noFill();

    if (sample.speedMin > speedStart) {
      arc(speedGaugeX, speedGaugeY, speedGaugeDiameter - 8F, speedGaugeDiameter - 8F, speedGaugeStart, map(sample.speedMin, speedStart, speedEnd, speedGaugeStart, speedGaugeEnd));
    }

    if (sample.speedMax < speedEnd) {
      arc(speedGaugeX, speedGaugeY, speedGaugeDiameter - 8F, speedGaugeDiameter - 8F, map(sample.speedMax, speedStart, speedEnd, speedGaugeStart, speedGaugeEnd), speedGaugeEnd);
    }

    popStyle();
  }

  stroke(127, 180, 127);
  strokeWeight(1);
  noFill();
  arc(speedGaugeX, speedGaugeY, speedGaugeDiameter - 16F, speedGaugeDiameter - 16F, speedGaugeStart, speedGaugeEnd);

  textAlign(CENTER, CENTER);
  textSize(18);

  for (int i = speedStart, j = 0; i <= speedEnd; i += 10, j++) {
    var a = map(i, speedStart, speedEnd, speedGaugeStart, speedGaugeEnd);
    var bigTick = j % 3 == 0;
    var cosA = cos(a);
    var sinA = sin(a);

    var tickX = speedGaugeX + speedGaugeRadius * cosA;
    var tickY = speedGaugeY + speedGaugeRadius * sinA;
    var tickX2 = speedGaugeX + (speedGaugeRadius - (bigTick ? 15 : 8)) * cosA;
    var tickY2 = speedGaugeY + (speedGaugeRadius - (bigTick ? 15 : 8)) * sinA;

    if (bigTick) {
      stroke(255, 127, 0);
    } else {
      stroke(127, 180, 127);
    }
    strokeWeight(bigTick ? 3 : 1);
    line(tickX, tickY, tickX2, tickY2);

    if (bigTick) {
      noStroke();
      fill(255);
      text(i, speedGaugeX + (speedGaugeRadius - 34) * cosA, speedGaugeY + (speedGaugeRadius - 34) * sinA);
    }
  }

  stroke(127, 180, 127);
  strokeWeight(3);
  noFill();
  arc(speedGaugeX, speedGaugeY, speedGaugeDiameter + 1, speedGaugeDiameter + 1, speedGaugeStart, speedGaugeEnd);

  noStroke();
  fill(255);
  textSize(22);
  text("KPH", speedGaugeX, speedGaugeY - 40);
  textSize(16);

  stroke(200);
  strokeWeight(1);
  fill(31);
  rectMode(CENTER);
  rect(speedGaugeX, speedGaugeY + 60, 66, 26);

  rectMode(CORNER);

  noStroke();
  fill(255);
  text(nf(speed, 0, 2), speedGaugeX, speedGaugeY + 60);

  var speedAngle = map(speed, speedStart, speedEnd, speedGaugeStart, speedGaugeEnd);
  stroke(255, 127, 0);
  strokeWeight(4);
  line(speedGaugeX - 20 * cos(speedAngle), speedGaugeY - 20 * sin(speedAngle), speedGaugeX + (speedGaugeRadius - 20) * cos(speedAngle), speedGaugeY + (speedGaugeRadius - 20) * sin(speedAngle));

  stroke(0);
  strokeWeight(4);
  fill(127);
  circle(speedGaugeX, speedGaugeY, 20);

  if (sample != null) {
    var consigne = sample.consigne;

    if (consigne > 0) {
      var consigneAngle = map(constrain(consigne, speedStart, speedEnd), speedStart, speedEnd, speedGaugeStart, speedGaugeEnd);

      noStroke();
      fill(60, 255, 60);
      pushMatrix();
      translate(speedGaugeX, speedGaugeY);
      rotate(consigneAngle);
      translate(speedGaugeRadius, 0);
      triangle(4, 0, 12, 8, 12, -8);
      stroke(60, 255, 60);
      strokeWeight(3);
      strokeCap(SQUARE);
      line(8, 0, -8, 0);
      strokeCap(ROUND);
      popMatrix();

      stroke(60, 255, 60);
      strokeWeight(1);
      fill(31);
      rectMode(CENTER);
      rect(speedGaugeX, speedGaugeY + 95, 66, 26);
      rectMode(CORNER);

      noStroke();
      fill(60, 255, 60);
      text(nf(consigne, 0, 2), speedGaugeX, speedGaugeY + 95);
    }
  }

  pop();
}

void drawKPID(Sample sample) {
  push();

  noStroke();
  fill(255);
  textAlign(RIGHT, TOP);
  text("kp", 81, 350);
  text("ki", 81, 380);
  text("kd", 81, 410);
  textAlign(CENTER, BOTTOM);
  text("Réglage", 133, 338);
  text("Actuel", 232, 338);

  stroke(127);
  strokeWeight(1);
  fill(63);
  rect(kpArduinoRect.left, kpArduinoRect.top, kpArduinoRect.width, kpArduinoRect.height);
  rect(kiArduinoRect.left, kiArduinoRect.top, kiArduinoRect.width, kiArduinoRect.height);
  rect(kdArduinoRect.left, kdArduinoRect.top, kdArduinoRect.width, kdArduinoRect.height);

  if (sample != null) {
    fill(255);
    textAlign(LEFT, CENTER);
    clip(kpArduinoRect.left, kpArduinoRect.top, kpArduinoRect.width, kpArduinoRect.height);
    text(str(sample.kp), kpArduinoRect.left + 4, kpArduinoRect.centerY());
    clip(kiArduinoRect.left, kiArduinoRect.top, kiArduinoRect.width, kiArduinoRect.height);
    text(str(sample.ki), kiArduinoRect.left + 4, kiArduinoRect.centerY());
    clip(kdArduinoRect.left, kdArduinoRect.top, kdArduinoRect.width, kdArduinoRect.height);
    text(str(sample.kd), kdArduinoRect.left + 4, kdArduinoRect.centerY());
    noClip();
  }

  pop();
}

void drawGraph(int frameTimeStart) {
  pushStyle();

  textAlign(LEFT, BOTTOM);
  fill(127, 127, 255);
  text(showError ? "[ERREUR]" : " ERREUR", errorGraphLabelRect.left, errorGraphLabelRect.bottom);
  fill(255, 255, 0);
  text(showIntegral ? "[INTÉGRAL]" : " INTÉGRAL", integralGraphLabelRect.left, integralGraphLabelRect.bottom);
  fill(255, 63, 63);
  text(showDerivative ? "[DÉRIVATIF]" : " DÉRIVATIF", derivativeGraphLabelRect.left, derivativeGraphLabelRect.bottom);
  fill(127, 255, 255);
  text(showSpeed ? "[VITESSE]" : " VITESSE", speedGraphLabelRect.left, speedGraphLabelRect.bottom);
  fill(127, 255, 127);
  text(showAcceleration ? "[ACCÉLÉRATION]" : " ACCÉLÉRATION", accelerationGraphLabelRect.left, accelerationGraphLabelRect.bottom);
  fill(255, 127, 255);
  text(showServo ? "[SERVO]" : " SERVO", servoGraphLabelRect.left, servoGraphLabelRect.bottom);

  popStyle();

  push();

  var maxTimestamp = frameTimeStart - timeZero;
  var minTimestamp = maxTimestamp - timeSpan * 1000F;

  int sampleSize;
  if (sampleOrTimeGraph) {
    sampleSize = this.sampleSize;
  } else {
    var i = 0;

    for (var sample : samples) {
      i++;

      if (sample.timestamp < minTimestamp) {
        break;
      }
    }

    sampleSize = i;
  }

  translate(graphRect.left, graphRect.top);

  final var graphMargin = 24;

  stroke(100);
  strokeWeight(1);
  dash.pattern(5, 5);
  dash.line(0, graphMargin - 1, graphRect.width, graphMargin - 1);
  dash.line(0, graphRect.height - graphMargin + 1, graphRect.width, graphRect.height - graphMargin + 1);

  dash.pattern(3, 7);

  if (sampleOrTimeGraph) {
    for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
      var i = iterator.count();
      var sample = iterator.next();

      if (sample.drawTimeTick) {
        var tickX = map(i, 0, sampleSize, graphRect.width, 0);

        dash.line(tickX, graphRect.height - graphMargin, tickX, graphMargin);
      }
    }
  } else {
    var frameTimePast = frameTimeStart - timeSpan * 1000F;
    var prevT = floor(map(-1, 0, graphRect.width - 1, frameTimePast, frameTimeStart) / 1000F);

    for (var i = 0; i < graphRect.width; i++) {
      var t = floor(map(i, 0, graphRect.width - 1, frameTimePast, frameTimeStart) / 1000F);

      if (t != prevT) {
        dash.line(i, graphRect.height - graphMargin, i, graphMargin);
      }

      prevT = t;
    }
  }

  // ERREUR

  if (showError) {
    errorRangeMin = float(errorRangeMinInput.getText());
    errorRangeMax = float(errorRangeMaxInput.getText());

    var autoScale = autoScaleError || errorRangeMin == errorRangeMax;
    var autoScaleMin = autoScale || Float.isNaN(errorRangeMin);
    var autoScaleMax = autoScale || Float.isNaN(errorRangeMax);

    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : errorRangeMin;
    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : errorRangeMax;
    var sampleCount = 0;

    for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
      var sample = iterator.next();

      if (sample.state != 2) {
        continue;
      }

      sampleCount++;

      var value = sample.error;

      if (autoScaleMin && value < minValue) {
        minValue = value;
      }

      if (autoScaleMax && value > maxValue) {
        maxValue = value;
      }
    }

    if (sampleCount > 0) {
      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);

      if (minValue <= 0 && maxValue >= 0) {
        var zero = minValue == maxValue ? graphRect.height / 2 : map(0, minValue, maxValue, graphRect.height - graphMargin, graphMargin);

        stroke(63, 63, 127);
        strokeWeight(1);
        dash.pattern(3, 3);
        dash.line(0, zero, graphRect.width, zero);
      }

      stroke(127, 127, 255);
      strokeWeight(1);

      var pointsDrawn = 0;

      var i = 0;

      for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
        i = iterator.count();
        var sample = iterator.next();

        if (sample.state != 2) {
          if (pointsDrawn > 0) {
            if (drawSmoothCurves && pointsDrawn > 1) {
              var nextSample1 = samples.get(i - 1);
              var nextSample2 = samples.get(i - 2);

              if (nextSample1 != null && nextSample2 != null) {
                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.error, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
                var nextX1 = graphPos[0];
                var nextY1 = graphPos[1];

                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.error, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
                var nextX2 = graphPos[0];
                var nextY2 = graphPos[1];

                curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
              }
            }

            endShape();

            pointsDrawn = 0;
          }

          continue;
        }

        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.error, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
        var x = graphPos[0];
        var y = graphPos[1];

        if (pointsDrawn == 0) {
          beginShape();
        }

        noFill();

        if (drawSmoothCurves) {
          if (pointsDrawn == 0) {
            var previousSample = samples.get(i + 1);

            if (previousSample != null) {
              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.error, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
              var prevX = graphPos[0];
              var prevY = graphPos[1];

              curveVertex(x - prevX + x, y - prevY + y);
            }
          }

          curveVertex(x, y);
        } else {
          vertex(x, y);
        }

        pointsDrawn++;
      }

      if (pointsDrawn > 0) {
        if (drawSmoothCurves && pointsDrawn > 1) {
          var nextSample1 = samples.get(i - 1);
          var nextSample2 = samples.get(i - 2);

          if (nextSample1 != null && nextSample2 != null) {
            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.error, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
            var nextX1 = graphPos[0];
            var nextY1 = graphPos[1];

            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.error, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
            var nextX2 = graphPos[0];
            var nextY2 = graphPos[1];

            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
          }
        }

        endShape();
      }

      noClip();

      fill(127, 127, 255);
      textAlign(LEFT, TOP);
      text(nfs(maxValue, 0, 3), 4, 6);
      textAlign(LEFT, BOTTOM);
      text(nfs(minValue, 0, 3), 4, graphRect.height - 3);
      textAlign(LEFT);
    }
  }

  // INTEGRAL

  if (showIntegral) {
    integralRangeMin = float(integralRangeMinInput.getText());
    integralRangeMax = float(integralRangeMaxInput.getText());

    var autoScale = autoScaleIntegral || integralRangeMin == integralRangeMax;
    var autoScaleMin = autoScale || Float.isNaN(integralRangeMin);
    var autoScaleMax = autoScale || Float.isNaN(integralRangeMax);

    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : integralRangeMin;
    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : integralRangeMax;
    var sampleCount = 0;

    for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
      var sample = iterator.next();

      if (sample.state != 2) {
        continue;
      }

      sampleCount++;

      var value = sample.integral;

      if (autoScaleMin && value < minValue) {
        minValue = value;
      }

      if (autoScaleMax && value > maxValue) {
        maxValue = value;
      }
    }

    if (sampleCount > 0) {
      pushStyle();

      stroke(255, 255, 0);
      strokeWeight(1);

      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);

      var pointsDrawn = 0;

      var i = 0;

      for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
        i = iterator.count();
        var sample = iterator.next();

        if (sample.state != 2) {
          if (pointsDrawn > 0) {
            if (drawSmoothCurves && pointsDrawn > 1) {
              var nextSample1 = samples.get(i - 1);
              var nextSample2 = samples.get(i - 2);

              if (nextSample1 != null && nextSample2 != null) {
                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.integral, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
                var nextX1 = graphPos[0];
                var nextY1 = graphPos[1];

                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.integral, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
                var nextX2 = graphPos[0];
                var nextY2 = graphPos[1];

                curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
              }
            }

            endShape();

            pointsDrawn = 0;
          }

          continue;
        }

        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.integral, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
        var x = graphPos[0];
        var y = graphPos[1];

        if (pointsDrawn == 0) {
          beginShape();
        }

        noFill();

        if (drawSmoothCurves) {
          if (pointsDrawn == 0) {
            var previousSample = samples.get(i + 1);

            if (previousSample != null) {
              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.integral, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
              var prevX = graphPos[0];
              var prevY = graphPos[1];

              curveVertex(x - prevX + x, y - prevY + y);
            }
          }

          curveVertex(x, y);
        } else {
          vertex(x, y);
        }

        pointsDrawn++;
      }

      if (pointsDrawn > 0) {
        if (drawSmoothCurves && pointsDrawn > 1) {
          var nextSample1 = samples.get(i - 1);
          var nextSample2 = samples.get(i - 2);

          if (nextSample1 != null && nextSample2 != null) {
            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.integral, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
            var nextX1 = graphPos[0];
            var nextY1 = graphPos[1];

            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.integral, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
            var nextX2 = graphPos[0];
            var nextY2 = graphPos[1];

            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
          }
        }

        endShape();
      }

      noClip();

      fill(255, 255, 0);
      textAlign(LEFT, TOP);
      text(nfs(maxValue, 0, 3), 90, 6);
      textAlign(LEFT, BOTTOM);
      text(nfs(minValue, 0, 3), 90, graphRect.height - 3);

      popStyle();
    }
  }

  // DERIVATIF

  if (showDerivative) {
    derivativeRangeMin = float(derivativeRangeMinInput.getText());
    derivativeRangeMax = float(derivativeRangeMaxInput.getText());

    var autoScale = autoScaleDerivative || derivativeRangeMin == derivativeRangeMax;
    var autoScaleMin = autoScale || Float.isNaN(derivativeRangeMin);
    var autoScaleMax = autoScale || Float.isNaN(derivativeRangeMax);

    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : derivativeRangeMin;
    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : derivativeRangeMax;
    var sampleCount = 0;

    for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
      var sample = iterator.next();

      if (sample.state != 2) {
        continue;
      }

      sampleCount++;

      var value = sample.derivative;

      if (autoScaleMin && value < minValue) {
        minValue = value;
      }

      if (autoScaleMax && value > maxValue) {
        maxValue = value;
      }
    }

    if (sampleCount > 0) {
      pushStyle();

      stroke(255, 63, 63);
      strokeWeight(1);

      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);

      var pointsDrawn = 0;

      var i = 0;

      for (var iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
        i = iterator.count();
        var sample = iterator.next();

        if (sample.state != 2) {
          if (pointsDrawn > 0) {
            if (drawSmoothCurves && pointsDrawn > 1) {
              var nextSample1 = samples.get(i - 1);
              var nextSample2 = samples.get(i - 2);

              if (nextSample1 != null && nextSample2 != null) {
                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.derivative, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
                var nextX1 = graphPos[0];
                var nextY1 = graphPos[1];

                calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.derivative, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
                var nextX2 = graphPos[0];
                var nextY2 = graphPos[1];

                curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
              }
            }

            endShape();

            pointsDrawn = 0;
          }

          continue;
        }

        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.derivative, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
        var x = graphPos[0];
        var y = graphPos[1];

        if (pointsDrawn == 0) {
          beginShape();
        }

        noFill();

        if (drawSmoothCurves) {
          if (pointsDrawn == 0) {
            var previousSample = samples.get(i + 1);

            if (previousSample != null) {
              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.derivative, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
              var prevX = graphPos[0];
              var prevY = graphPos[1];

              curveVertex(x - prevX + x, y - prevY + y);
            }
          }

          curveVertex(x, y);
        } else {
          vertex(x, y);
        }

        pointsDrawn++;
      }

      if (pointsDrawn > 0) {
        if (drawSmoothCurves && pointsDrawn > 1) {
          var nextSample1 = samples.get(i - 1);
          var nextSample2 = samples.get(i - 2);

          if (nextSample1 != null && nextSample2 != null) {
            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.derivative, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
            var nextX1 = graphPos[0];
            var nextY1 = graphPos[1];

            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.derivative, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
            var nextX2 = graphPos[0];
            var nextY2 = graphPos[1];

            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
          }
        }

        endShape();
      }

      noClip();

      fill(255, 63, 63);
      textAlign(LEFT, TOP);
      text(nfs(maxValue, 0, 3), 176, 6);
      textAlign(LEFT, BOTTOM);
      text(nfs(minValue, 0, 3), 176, graphRect.height - 3);

      popStyle();
    }
  }

  // VITESSE

  if (showSpeed) {
    speedRangeMin = float(speedRangeMinInput.getText());
    speedRangeMax = float(speedRangeMaxInput.getText());

    var autoScale = autoScaleSpeed || speedRangeMin == speedRangeMax;
    var autoScaleMin = autoScale || Float.isNaN(speedRangeMin);
    var autoScaleMax = autoScale || Float.isNaN(speedRangeMax);

    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : speedRangeMin;
    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : speedRangeMax;

    var iterator = samples.cappedIterator(sampleSize);

    while (iterator.hasNext()) {
      var sample = iterator.next();

      var value = sample.speed;

      if (autoScaleMin && value < minValue) {
        minValue = value;
      }

      if (autoScaleMax && value > maxValue) {
        maxValue = value;
      }
    }

    if (iterator.count() > 0) {
      pushStyle();

      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);

      var isDrawingCurve = false;

      // CONSIGNE

      stroke(63, 127, 127);
      strokeWeight(1);
      dash.pattern(3, 3);

      for (iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
        var i = iterator.count();
        var sample = iterator.next();

        if (sample.state == 0) {
          if (isDrawingCurve) {
            dash.endShape();
            isDrawingCurve = false;
          }
          continue;
        }

        if (!isDrawingCurve) {
          dash.beginShape();
          isDrawingCurve = true;
        }

        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.consigne, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
        var x = graphPos[0];
        var y = graphPos[1];

        noFill();

        dash.vertex(x, y);
      }

      if (isDrawingCurve) {
        dash.endShape();
        isDrawingCurve = false;
      }

      // VITESSE

      stroke(127, 255, 255);
      strokeWeight(1);

      var pointsDrawn = 0;

      var i = 0;

      for (iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
        i = iterator.count();
        var sample = iterator.next();

        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.speed, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
        var x = graphPos[0];
        var y = graphPos[1];

        if (pointsDrawn == 0) {
          beginShape();
        }

        noFill();

        if (drawSmoothCurves) {
          if (pointsDrawn == 0) {
            var previousSample = samples.get(i + 1);

            if (previousSample != null) {
              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.speed, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
              var prevX = graphPos[0];
              var prevY = graphPos[1];

              curveVertex(x - prevX + x, y - prevY + y);
            }
          }

          curveVertex(x, y);
        } else {
          vertex(x, y);
        }

        pointsDrawn++;
      }

      if (pointsDrawn > 0) {
        if (drawSmoothCurves && pointsDrawn > 1) {
          var nextSample1 = samples.get(i - 1);
          var nextSample2 = samples.get(i - 2);

          if (nextSample1 != null && nextSample2 != null) {
            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.speed, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
            var nextX1 = graphPos[0];
            var nextY1 = graphPos[1];

            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.speed, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
            var nextX2 = graphPos[0];
            var nextY2 = graphPos[1];

            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
          }
        }

        endShape();
      }

      noClip();

      fill(127, 255, 255);
      textAlign(LEFT, TOP);
      text(nfs(maxValue, 0, 3), 262, 6);
      textAlign(LEFT, BOTTOM);
      text(nfs(minValue, 0, 3), 262, graphRect.height - 3);

      popStyle();
    }
  }

  // ACCELERATION

  if (showAcceleration) {
    accelerationRangeMin = float(accelerationRangeMinInput.getText());
    accelerationRangeMax = float(accelerationRangeMaxInput.getText());

    var autoScale = autoScaleAcceleration || accelerationRangeMin == accelerationRangeMax;
    var autoScaleMin = autoScale || Float.isNaN(accelerationRangeMin);
    var autoScaleMax = autoScale || Float.isNaN(accelerationRangeMax);

    var minValue = autoScaleMin ? Float.POSITIVE_INFINITY : accelerationRangeMin;
    var maxValue = autoScaleMax ? Float.NEGATIVE_INFINITY : accelerationRangeMax;

    var iterator = samples.cappedIterator(sampleSize);

    while (iterator.hasNext()) {
      var sample = iterator.next();

      var value = sample.acceleration;

      if (autoScaleMin && value < minValue) {
        minValue = value;
      }

      if (autoScaleMax && value > maxValue) {
        maxValue = value;
      }
    }

    if (iterator.count() > 0) {
      pushStyle();

      stroke(127, 255, 127);
      strokeWeight(1);

      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);

      var pointsDrawn = 0;

      var i = 0;

      for (iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
        i = iterator.count();
        var sample = iterator.next();

        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.acceleration, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
        var x = graphPos[0];
        var y = graphPos[1];

        if (pointsDrawn == 0) {
          beginShape();
        }

        noFill();

        if (drawSmoothCurves) {
          if (pointsDrawn == 0) {
            var previousSample = samples.get(i + 1);

            if (previousSample != null) {
              calculateGraphPosition(graphPos, graphMargin, i, sampleSize, previousSample.acceleration, minValue, maxValue, previousSample.timestamp, minTimestamp, maxTimestamp);
              var prevX = graphPos[0];
              var prevY = graphPos[1];

              curveVertex(x - prevX + x, y - prevY + y);
            }
          }

          curveVertex(x, y);
        } else {
          vertex(x, y);
        }

        pointsDrawn++;
      }

      if (pointsDrawn > 0) {
        if (drawSmoothCurves && pointsDrawn > 1) {
          var nextSample1 = samples.get(i - 1);
          var nextSample2 = samples.get(i - 2);

          if (nextSample1 != null && nextSample2 != null) {
            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample1.acceleration, minValue, maxValue, nextSample1.timestamp, minTimestamp, maxTimestamp);
            var nextX1 = graphPos[0];
            var nextY1 = graphPos[1];

            calculateGraphPosition(graphPos, graphMargin, i, sampleSize, nextSample2.acceleration, minValue, maxValue, nextSample2.timestamp, minTimestamp, maxTimestamp);
            var nextX2 = graphPos[0];
            var nextY2 = graphPos[1];

            curveVertex(nextX1 - (nextX2 - nextX1), nextY1 - (nextY2 - nextY1));
          }
        }

        endShape();
      }

      noClip();

      fill(127, 255, 127);
      textAlign(LEFT, TOP);
      text(nfs(maxValue, 0, 3), 348, 6);
      textAlign(LEFT, BOTTOM);
      text(nfs(minValue, 0, 3), 348, graphRect.height - 3);

      popStyle();
    }
  }

  // SERVO

  if (showServo) {
    servoRangeMin = toInt(servoRangeMinInput.getText());
    servoRangeMax = toInt(servoRangeMaxInput.getText());

    var autoScale = autoScaleServo || servoRangeMin == servoRangeMax;
    var autoScaleMin = autoScale || servoRangeMin == Integer.MIN_VALUE;
    var autoScaleMax = autoScale || servoRangeMax == Integer.MIN_VALUE;

    var minValue = autoScaleMin ? Integer.MAX_VALUE : servoRangeMin;
    var maxValue = autoScaleMax ? Integer.MIN_VALUE : servoRangeMax;

    var iterator = samples.cappedIterator(sampleSize);

    while (iterator.hasNext()) {
      var sample = iterator.next();

      var value = sample.servo;

      if (autoScaleMin && value < minValue) {
        minValue = value;
      }

      if (autoScaleMax && value > maxValue) {
        maxValue = value;
      }
    }

    if (iterator.count() > 0) {
      pushStyle();

      stroke(255, 127, 255);
      strokeWeight(1);

      clip(0, graphMargin, graphRect.width, graphRect.height - 2 * graphMargin + 1);

      beginShape();

      for (iterator = samples.cappedIterator(sampleSize); iterator.hasNext(); ) {
        var i = iterator.count();
        var sample = iterator.next();

        calculateGraphPosition(graphPos, graphMargin, i, sampleSize, sample.servo, minValue, maxValue, sample.timestamp, minTimestamp, maxTimestamp);
        var x = graphPos[0];
        var y = graphPos[1];

        noFill();

        vertex(x, y);
      }

      endShape();

      noClip();

      fill(255, 127, 255);
      textAlign(LEFT, TOP);
      text(maxValue, 434, 6);
      textAlign(LEFT, BOTTOM);
      text(minValue, 434, graphRect.height - 3);

      popStyle();
    }
  }

  stroke(100);
  strokeWeight(2);
  noFill();
  rect(0, 0, graphRect.width, graphRect.height);

  pop();
}

void drawValues(Sample sample) {
  push();

  translate(graphRect.left, graphRect.bottom + 24);

  fill(255);
  text("Nombre d'échantillons", 0, 0);
  text("Intervalle de temps", floor(textWidth("Nombre d'échantillons")) + 8F + sampleSizeSlider.getWidth() + 16F + sampleOrTimeGraphToggle.getWidth() + 16F, 0);

  pushMatrix();

  translate(0, 45);

  pushMatrix();

  var col1Text = floor(max(textWidth("Proportionnel"), max(textWidth("Intégral"), textWidth("Dérivatif"), textWidth("Erreur (km/h)"))));
  var col1 = col1Text + 8;

  translate(col1Text, 0);

  textAlign(RIGHT);
  fill(255);
  text("Proportionnel", 0, 0);
  text("Intégral", 0, 30);
  text("Dérivatif", 0, 60);
  text("Erreur (km/h)", 0, 90);
  textAlign(LEFT);

  translate(8, 0);

  stroke(127);
  strokeWeight(1);
  fill(63);
  rect(0, -17, 200, 23);
  rect(0, 13, 200, 23);
  rect(0, 43, 200, 23);
  rect(0, 73, 200, 23);

  var col2 = 200 + 24;
  translate(col2, 0);

  var col3Text = floor(max(textWidth("Vitesse"), max(textWidth("Accélération"), textWidth("Commande servo"))));
  var col3 = col3Text + 8;

  translate(col3Text, 0);

  textAlign(RIGHT);
  fill(255);
  text("Vitesse", 0, 0);
  text("Accélération", 0, 30);
  text("Commande servo", 0, 60);
  textAlign(LEFT);

  translate(8, 0);

  stroke(127);
  strokeWeight(1);
  fill(63);
  rect(0, -17, 100, 23);
  rect(0, 13, 100, 23);
  rect(0, 43, 100, 23);

  var col4 = 100 + 24;
  translate(col4, 0);

  var col5Text = floor(max(textWidth("Satellites"), textWidth("Seuil")));
  var col5 = col5Text + 8;

  translate(col5Text, 0);

  textAlign(RIGHT);
  fill(255);
  text("Satellites", 0, 0);
  text("Seuil", 0, 30);
  textAlign(LEFT);

  translate(8, 0);

  stroke(127);
  strokeWeight(1);
  fill(63);
  rect(0, -17, 50, 23);
  rect(0, 13, 50, 23);

  popMatrix();

  if (sample != null) {
    push();

    fill(255);
    textAlign(LEFT, CENTER);

    translate(col1, 0);

    clip(0, -18, 200, 23);
    text(str(sample.proportional), 4F, -18F + 23F / 2F);

    clip(0, 12, 200, 23);
    text(str(sample.integral), 4F, 12F + 23F / 2F);

    clip(0, 42, 200, 23);
    text(str(sample.derivative), 4F, 42F + 23F / 2F);

    clip(0, 72, 200, 23);
    text(str(sample.error), 4F, 72F + 23F / 2F);

    translate(col2 + col3, 0);

    clip(0, -18, 100, 23);
    text(str(sample.speed), 4F, -18F + 23F / 2F);

    clip(0, 12, 100, 23);
    text(str(sample.acceleration), 4F, 12F + 23F / 2F);

    clip(0, 42, 100, 23);
    text(sample.servo, 4F, 42F + 23F / 2F);

    translate(col4 + col5, 0);

    clip(0, -18, 50, 23);
    text(sample.satellites, 4F, -18F + 23F / 2F);

    clip(0, 12, 50, 23);
    text(sample.minSatellites, 4F, 12F + 23F / 2F);

    noClip();

    pop();
  }

  popMatrix();

  var colScalingLabel = floor(max(textWidth("Erreur"), max(textWidth("Intégral"), max(textWidth("Dérivatif"), max(textWidth("Vitesse"), max(textWidth("Accélération"), textWidth("Servo")))))));

  translate(graphRect.width - autoScaleErrorToggle.getWidth() - 8F - (errorRangeMaxInput.getWidth() + 2F) - 2F - (errorRangeMinInput.getWidth() + 2F) - 8F - colScalingLabel, 0);

  fill(255);

  translate(colScalingLabel, 0);

  textAlign(RIGHT);
  text("Erreur", 0, 24);
  text("Intégral", 0, 48);
  text("Dérivatif", 0, 72);
  text("Vitesse", 0, 96);
  text("Accélération", 0, 120);
  text("Servo", 0, 144);
  textAlign(LEFT);

  translate(8, 0);

  textAlign(CENTER);
  text("Graphe min/max", ((errorRangeMaxInput.getWidth() + 2F) + 2F + (errorRangeMinInput.getWidth() + 2F)) / 2F, 0);

  pop();
}

void drawStatusBar(Sample sample) {
  push();

  stroke(63);
  strokeWeight(1);

  textAlign(CENTER, CENTER);
  textFont(consolas_16_bold);
  textSize(18);

  String stateName;
  if (sample != null) {
    switch (sample.state) {
    case 0:
      stateName = "Standby";
      fill(130, 130, 0);
      break;
    case 1:
      stateName = "Approche";
      fill(0, 0, 170);
      break;
    case 2:
      stateName = "Régulation";
      fill(0, 100, 0);
      break;
    default:
      stateName = "État inconnu [%s]".formatted(sample.state);
      noFill();
    }
  } else {
    stateName = "Déconnecté / OFF";
    fill(47);
  }
  rect(stateRect.left, stateRect.top, stateRect.width, stateRect.height);
  fill(255);
  text(stateName, stateRect.centerX(), stateRect.centerY());

  if (sample == null) {
    fill(47);
  } else if (sample.switchFlag) {
    fill(170, 0, 0);
  } else {
    fill(0, 100, 0);
  }
  rect(switchFlagRect.left, switchFlagRect.top, switchFlagRect.width, switchFlagRect.height);
  fill(255);
  text("SECU interrupteur", switchFlagRect.centerX(), switchFlagRect.centerY());

  if (sample == null) {
    fill(47);
  } else if (sample.speedDiffFlag) {
    fill(170, 0, 0);
  } else {
    fill(0, 100, 0);
  }
  rect(speedDiffFlagRect.left, speedDiffFlagRect.top, speedDiffFlagRect.width, speedDiffFlagRect.height);
  fill(255);
  text("SECU écart", speedDiffFlagRect.centerX(), speedDiffFlagRect.centerY());

  if (sample == null) {
    fill(47);
  } else if (sample.minSpeedFlag) {
    fill(170, 0, 0);
  } else {
    fill(0, 100, 0);
  }
  rect(minSpeedFlagRect.left, minSpeedFlagRect.top, minSpeedFlagRect.width, minSpeedFlagRect.height);
  fill(255);
  text("SECU vitesse min", minSpeedFlagRect.centerX(), minSpeedFlagRect.centerY());

  if (sample == null) {
    fill(47);
  } else if (sample.maxSpeedFlag) {
    fill(170, 0, 0);
  } else {
    fill(0, 100, 0);
  }
  rect(maxSpeedFlagRect.left, maxSpeedFlagRect.top, maxSpeedFlagRect.width, maxSpeedFlagRect.height);
  fill(255);
  text("SECU vitesse max", maxSpeedFlagRect.centerX(), maxSpeedFlagRect.centerY());

  if (sample == null) {
    fill(47);
  } else if (sample.brakeFlag) {
    fill(170, 0, 0);
  } else {
    fill(0, 100, 0);
  }
  rect(brakeFlagRect.left, brakeFlagRect.top, brakeFlagRect.width, brakeFlagRect.height);
  fill(255);
  text("Frein", brakeFlagRect.centerX(), brakeFlagRect.centerY());

  if (sample == null) {
    fill(47);
  } else if (sample.relay) {
    fill(0, 100, 0);
  } else {
    fill(130, 130, 0);
  }
  rect(relayRect.left, relayRect.top, relayRect.width, relayRect.height);
  fill(255);
  text("Relais", relayRect.centerX(), relayRect.centerY());

  if (sample == null) {
    fill(47);
  } else if (sample.satellites < sample.minSatellites) {
    fill(170, 0, 0);
  } else {
    fill(0, 100, 0);
  }
  rect(satellitesRect.left, satellitesRect.top, satellitesRect.width, satellitesRect.height);
  fill(255);
  text("Satellites", satellitesRect.centerX(), satellitesRect.centerY());

  pop();
}

void mousePressed() {
  if (mouseButton == LEFT) {
    if (errorGraphLabelRect.contains(mouseX, mouseY)) {
      showError = !showError;
    } else if (integralGraphLabelRect.contains(mouseX, mouseY)) {
      showIntegral = !showIntegral;
    } else if (derivativeGraphLabelRect.contains(mouseX, mouseY)) {
      showDerivative = !showDerivative;
    } else if (speedGraphLabelRect.contains(mouseX, mouseY)) {
      showSpeed = !showSpeed;
    } else if (accelerationGraphLabelRect.contains(mouseX, mouseY)) {
      showAcceleration = !showAcceleration;
    } else if (servoGraphLabelRect.contains(mouseX, mouseY)) {
      showServo = !showServo;
    }
  }
}

void controlEvent(ControlEvent c) {
  switch (c.getId()) {
  case 1:
  case 2:
  case 3:
    sendKPID();
    return;
  case 4:
    sendCommand(c.getStringValue());
    return;
  }

  if (c.isAssignableFrom(Toggle.class)) {
    var toggle = (Toggle) c.getController();

    if (toggle.getName().startsWith("autoScale")) {
      updateAutoScaleToggleLabel(toggle);
    }
  }
}

void serialEvent(Serial p) {
  if (!hasIgnoredFirstPacket) {
    p.clear();
    hasIgnoredFirstPacket = true;
    return;
  }

  handlePacket(p.readString());
}

static boolean isFloat(String value) {
  return FLOAT_REGEX.matcher(value).matches();
}

static int toInt(String value) {
  try {
    return Integer.parseInt(value);
  }
  catch (NumberFormatException e) {
    return Integer.MIN_VALUE;
  }
}

static boolean intToBool(String value) {
  return value.equals("1");
}

static int toInt(boolean value) {
  return value ? 1 : 0;
}

float[] calculateGraphPosition(float graphMargin, int i, int sampleSize, float value, float minValue, float maxValue, float timestamp, float minTimestamp, float maxTimestamp) {
  var pos = new float[2];
  calculateGraphPosition(pos, graphMargin, i, sampleSize, value, minValue, maxValue, timestamp, minTimestamp, maxTimestamp);
  return pos;
}

void calculateGraphPosition(float[] pos, float graphMargin, int i, int sampleSize, float value, float minValue, float maxValue, float timestamp, float minTimestamp, float maxTimestamp) {
  pos[0] = sampleOrTimeGraph ? map(i, 0, sampleSize - 1, graphRect.width, 0) : map(timestamp, minTimestamp, maxTimestamp, 0, graphRect.width);
  pos[1] = minValue == maxValue ? graphRect.height / 2 : map(value, minValue, maxValue, graphRect.height - graphMargin, graphMargin);
}

void updateAutoScaleToggleLabel(Toggle toggle) {
  toggle.setCaptionLabel(toggle.getBooleanValue() ? "AUTO" : "MANU");
}

void sendKPID() {
  var sample = samples.get();

  if (sample == null) {
    return;
  }

  var sb = new StringBuilder();

  sb.append("kp").append('=');
  if (isFloat(kpInput.getText())) {
    sb.append(kpInput.getText());
  } else {
    sb.append(sample.kp);
  }

  sb.append(',');

  sb.append("ki").append('=');
  if (isFloat(kiInput.getText())) {
    sb.append(kiInput.getText());
  } else {
    sb.append(sample.ki);
  }

  sb.append(',');

  sb.append("kd").append('=');
  if (isFloat(kdInput.getText())) {
    sb.append(kdInput.getText());
  } else {
    sb.append(sample.kd);
  }

  sb.append('\n');

  kpInput.setText("");
  kiInput.setText("");
  kdInput.setText("");

  serial.write(sb.toString());
}

void sendCommand(String command) {
  serial.write(command);
  serial.write('\n');
}

void handlePacket(String packet) {
  var timestamp = millis();

  if (!hasReadSecondPacket) {
    timeZero = timestamp;
    hasReadSecondPacket = true;
  }

  timestamp -= timeZero;

  var previousSample = samples.get();
  var sample = new Sample(timestamp, previousSample == null || previousSample.timestamp / 1000 != timestamp / 1000);

  var variables = packet.split(";");

  for (int i = 0, max = variables.length - 1; i < max; i++) {
    var variable = variables[i];
    var split = variable.split(":", 2);

    var name = split[0];
    var value = split[1];

    switch (name) {
    case "SECUINTER":
      sample.switchFlag = intToBool(value);
      break;
    case "SECUECART":
      sample.speedDiffFlag = intToBool(value);
      break;
    case "SECUVMIN":
      sample.minSpeedFlag = intToBool(value);
      break;
    case "SECUVMAX":
      sample.maxSpeedFlag = intToBool(value);
      break;
    case "KP":
      sample.kp = float(value);
      break;
    case "KI":
      sample.ki = float(value);
      break;
    case "KD":
      sample.kd = float(value);
      break;
    case "VITESSEMIN":
      sample.speedMin = int(value);
      break;
    case "VITESSEMAX":
      sample.speedMax = int(value);
      break;
    case "ETAT":
      sample.state = int(value);
      break;
    case "CONSIGNE":
      sample.consigne = float(value);
      break;
    case "SAT":
      sample.satellites = int(value);
      break;
    case "SATMIN":
      sample.minSatellites = int(value);
      break;
    case "VITESSE":
      sample.speed = float(value);
      break;
    case "ACCEL":
      sample.acceleration = float(value);
      break;
    case "ERREUR":
      sample.error = float(value);
      break;
    case "PROPORTIONNEL":
      sample.proportional = float(value);
      break;
    case "INTEGRAL":
      sample.integral = float(value);
      break;
    case "DERIVATIF":
      sample.derivative = float(value);
      break;
    case "SERVO":
      sample.servo = int(value);
      break;
    case "FREIN":
      sample.brakeFlag = intToBool(value);
      break;
      //case "MILLIS":
      //  timestamp = int(value);
      //  break;
    case "RELAIS":
      sample.relay = intToBool(value);
      break;
    }
  }

  sampleQueue.add(sample);
}

void resetSerial() {
  if (serial != null) {
    serial.stop();
    serial = null;
  }

  hasReadSecondPacket = false;
  timeZero = 0;

  samples.clear();
  sampleQueue.clear();

  try {
    serial = new Serial(this, COM_PORT, 115200);
    serial.bufferUntil('\n');
  }
  catch (Exception e) {
    println(e.getMessage());
  }
}

void saveSamples() {
  var table = new Table();

  table.addColumn("timestamp");
  table.addColumn("drawTimeTick");
  table.addColumn("kp");
  table.addColumn("ki");
  table.addColumn("kd");
  table.addColumn("proportional");
  table.addColumn("integral");
  table.addColumn("derivative");
  table.addColumn("speed");
  table.addColumn("acceleration");
  table.addColumn("consigne");
  table.addColumn("error");
  table.addColumn("servo");
  table.addColumn("state");
  table.addColumn("satellites");
  table.addColumn("minSatellites");
  table.addColumn("speedMin");
  table.addColumn("speedMax");
  table.addColumn("switchFlag");
  table.addColumn("speedDiffFlag");
  table.addColumn("minSpeedFlag");
  table.addColumn("maxSpeedFlag");
  table.addColumn("brakeFlag");
  table.addColumn("relay");

  var iterator = samples.descendingIterator();

  while (iterator.hasNext()) {
    var sample = iterator.next();

    var row = table.addRow();

    row.setInt("timestamp", sample.timestamp);
    row.setInt("drawTimeTick", toInt(sample.drawTimeTick));
    row.setFloat("kp", sample.kp);
    row.setFloat("ki", sample.ki);
    row.setFloat("kd", sample.kd);
    row.setFloat("proportional", sample.proportional);
    row.setFloat("integral", sample.integral);
    row.setFloat("derivative", sample.derivative);
    row.setFloat("speed", sample.speed);
    row.setFloat("acceleration", sample.acceleration);
    row.setFloat("consigne", sample.consigne);
    row.setFloat("error", sample.error);
    row.setInt("servo", sample.servo);
    row.setInt("state", sample.state);
    row.setInt("satellites", sample.satellites);
    row.setInt("minSatellites", sample.minSatellites);
    row.setInt("speedMin", sample.speedMin);
    row.setInt("speedMax", sample.speedMax);
    row.setInt("switchFlag", toInt(sample.switchFlag));
    row.setInt("speedDiffFlag", toInt(sample.speedDiffFlag));
    row.setInt("minSpeedFlag", toInt(sample.minSpeedFlag));
    row.setInt("maxSpeedFlag", toInt(sample.maxSpeedFlag));
    row.setInt("brakeFlag", toInt(sample.brakeFlag));
    row.setInt("relay", toInt(sample.relay));
  }

  saveTable(table, "data/samples.csv");
}