LP_TIMER(1000, []() {
  int power = Output * 255 / 100;
  setPWMDutyCycle(power);
});

#define graphItems 32
LP_TIMER(1000, HandlePwmHeaterDisplay);
void HandlePwmHeaterDisplay() {
  static byte graphStates[graphItems];
  static byte currentGraphItemNumber = 0;

  if (inSelectionMode) {
    digitalWrite(ssrPin, LOW);
    Setpoint = 0;
    return;
  }

  currentGraphItemNumber++;
  if (currentGraphItemNumber >= graphItems) {
    currentGraphItemNumber = 0;
  }

  byte graphHeight = 10;
  byte currentAmount = Output==0 ? 0 : (Output-1)/99.0 * (graphHeight) + 1;

  graphStates[currentGraphItemNumber] = currentAmount;

  byte rightMargin = 127;
  byte topMargin = 63-graphHeight;
  byte itemsDisplayed = 0;

  while (itemsDisplayed < graphItems) {
    int currentItemN = currentGraphItemNumber - itemsDisplayed;
    if (currentItemN < 0) {
      currentItemN += graphItems;
    }
    byte CurrentItemValue = graphStates[currentItemN];
    oled.fastLineV(rightMargin - graphItems + itemsDisplayed, topMargin, topMargin+graphHeight-CurrentItemValue+1, 0);
    if (CurrentItemValue) {
      oled.fastLineV(rightMargin - graphItems + itemsDisplayed, topMargin+graphHeight-CurrentItemValue+1, topMargin+graphHeight, 1);
    }

    itemsDisplayed++;
  }
  
  byte leftPosition = 126-graphItems - 3*6;
  oled.setCursor(leftPosition, 7);
  sprintf(timeBuffer, "%3d", (int)(Output));
  oled.invertText(true);
  oled.print(timeBuffer);
  oled.invertText(false);

  leftPosition -= 6*3;
  oled.setCursor(leftPosition, 7);
  sprintf(timeBuffer, "%3d", (int)(regulator.lastD));
  oled.print(timeBuffer);

  leftPosition -= 6*3;
  oled.setCursor(leftPosition, 7);
  sprintf(timeBuffer, "%3d", (int)(regulator.lastI));
  oled.print(timeBuffer);

  leftPosition -= 6*3;
  oled.setCursor(leftPosition, 7);
  sprintf(timeBuffer, "%3d", (int)(regulator.lastP));
  oled.print(timeBuffer);
}

// LP_TIMER(1000, HandleHeater);
void HandleHeater() {
  static int currentPart = 0;
  static bool states[PARTS];

  if (inSelectionMode) {
    digitalWrite(ssrPin, LOW);
    Setpoint = 0;
    return;
  }

  int current = Output/100 * PARTS;

  int last = 0;
  for (int i = 1; i < PARTS; i++) {
    last += states[i] ? 1 : 0;
    if (i) {
      states[i-1] = states[i];
    }
  }
  bool next = current >= last ? true : false;
  if (Output < 1) next = false;
  states[PARTS-1] = next;

  oled.setCursor(128-6*PARTS, 7);
  sprintf(timeBuffer, "%3d", (int)(Output));
  char symbol;
  for (int i = 0; i < PARTS; i++) {
    int index = i - (PARTS - 3);
    symbol = index < 0 ? ' ' : timeBuffer[index];
    oled.invertText(states[i]);
    oled.print(symbol);
    oled.invertText(false);
  }

  if (temperatureSensorError) {
    next = 0;
  }

  digitalWrite(ssrPin, next);
  currentPart++;
  if (currentPart >= PARTS) {
    currentPart = 0;
  }

#ifdef PlotValues
  Serial.print(Setpoint);
  Serial.print(",");
  Serial.print(Input);
  Serial.print(",");
  Serial.print(Output);
  Serial.println(",0,100");
#endif

}