#include "hFramework.h"
#include "hCloudClient.h"

hPIDRegulator myReg;

void setupMotorPID() {
  myReg.setScale(1);
  myReg.setKP(5);
  myReg.setKI(0.02);
  myReg.setKD(100.0);
  myReg.dtMs = 5;
  myReg.stableRange = 30;
  myReg.stableTimes = 3;

  hMotA.attachPositionRegulator(myReg);
  hMotA.resetEncoderCnt();
}

void hMain() {
  platform.begin(&RPi);
  platform.ui.setProjectId("@@@PROJECT_ID@@@");

  platform.ui.configHandler = []() {
    platform.ui.loadHtml({Resource::URL, "/ui.html"});
    platform.ui.video.enable();
  };

  setupMotorPID();

  platform.ui.onKeyEvent = [](KeyEventType type, KeyCode code) {
    if (type == KeyEventType::Pressed && code == KeyCode::Key_A) {
      hMotA.rotRel(90); // rotate DC motor with encoder
    }
  };

  sys.taskCreate([]() {
    auto& relayPin = hExt.pin4;
    while (1) {
      relayPin.toggle();
      sys.delay(1000);
    }
  });
}