#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h>

#include "pid.h"
#include "slow_pwm.h"
#include "graph.h"

#define PIN_CONTROL  13
#define PIN_FEEDBACK A3

PID pid(0.404f, 49.0f, 12.5f, 10.0f, 0.0, 100.0);
SlowAsyncPWM pwm;
U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, A5, A4, U8X8_PIN_NONE);

void setup()
{
  Serial.begin(115200);
  pid.setPoint(600);
  pid.resetTimers();
  pwm.init(PIN_CONTROL, 1000);
  u8g2.begin();
  u8g2.setDrawColor(1);
  u8g2.setFont(u8g2_font_5x7_tf);
}

void loop()
{
  static uint16_t feedback = 0;
  static uint8_t control = 0;

  static uint32_t timer1 = 0;
  static uint32_t timer2 = 0;
  static uint32_t timer3 = 0;

  const uint16_t dataLength = 50;
  static uint16_t dataHead = 0;
  static uint16_t dataBuffer[dataLength];

  pwm.tick();

  if (millis() - timer1 > 1000)
  {
    timer1 = millis();
    feedback = analogRead(PIN_FEEDBACK);
    control = (uint8_t)(pid.tick((float)feedback) + 0.5f);
    //control = feedback < 600 ? 100 : 0;
    pwm.forceDuty(control);

    dataBuffer[dataHead] = feedback;
    dataHead = (dataHead + 1) % dataLength;
  }

  if (millis() - timer2 > 1000)
  {
    timer2 = millis();
    Serial.print(millis() / 1000);
    Serial.print(" - ");
    Serial.println(analogRead(PIN_FEEDBACK));
  }

  if (millis() - timer3 > 1000)
  {
    timer3 = millis();
    u8g2.clearBuffer();
    drawGraph(&u8g2, 10, 10, 108, 50, 1023, 0, dataBuffer, dataHead, dataLength);
    u8g2.sendBuffer();
  }
}