#include <Arduino.h>
#include <Benchmark.h>

#include "SqPlanner.h"
#include "SqPlanner8.h"
#include "SpeedPlanner.h"
#include "SpeedPlannerFast.h"

#include "AccelDriverPrd.h"
AccelDriverPrd dr;

SqPlanner<> epl;

#include "ubutton.h"
uButton b0(2);
uButton b1(3);
uButton b2(4);

void splanner_test() {
  //SpeedPlanner
  SpeedPlanner sp;
  // SpeedPlannerFast sp;
  sp.setSpeed(500);
  sp.setAccel(400);

  // if (!sp.plan(50, 250, 1000)) {
  //   Serial.println("ERROR");
  //   while (1);
  // }
    if (!sp.plan(0, 350)) {
    Serial.println("ERROR");
    while (1);
  }

  Serial.print(sp.getPos());
  Serial.print(',');
  Serial.print(sp.getSpeed());
  Serial.println();
  while (1) {
    delayMicroseconds(sp.getPeriod());
    // benchBegin();
    if (sp.calcNext()) break;
    // benchEnd(&Serial);
    Serial.print(sp.getPos());
    Serial.print(',');
    Serial.print(sp.getSpeed());
    Serial.println();
  }
  Serial.print(sp.getPos());
  Serial.print(',');
  Serial.print(sp.getSpeed());
  Serial.println();
  while (1);
}

void setup() {
  Serial.begin(115200);
  Serial.println(0);
  delay(1000);

  splanner_test();


  // for (int i = 0; i <= 255; i++) {
  //   Serial.println(ease8(i));
  //   delay(15);
  // }

  // for (int i = 0; i <= 255; i+=2) {
  //   Serial.print(round(ease(i / 511.0) * 511));
  //   Serial.print(',');
  // delay(15);
  // }

  // while (1);

  epl.setSpeed(1000);
  epl.setTarget(500);

  // Serial.println("0,0");
  Serial.println(0);
  delay(1000);
  // Serial.println("0,0");

  // dr.setStepsPerTick(20);
  // dr.setSpeed(500);
  // dr.setAccel(200 * 1);
  // dr.setTarget(1000);

}

void loop() {
  int a0 = analogRead(0);
  int a1 = analogRead(1);
  int a2 = analogRead(2);

  if (epl.tick()) {
    epl.setTarget(random(-1000, 1000));
    // Serial.print(epl.getPos());
    // Serial.print(',');
    // Serial.println(0);
  }
  /*
    //SpeedPlanner
    if (dr.tick()) {
      Serial.print(dr.getSpeed());
      Serial.print(',');
      Serial.print(dr.getPos());
      Serial.print(',');
      Serial.print(dr.getTarget());
      Serial.print(',');
      Serial.print(a0);
      Serial.print(',');
      Serial.print(0);
      Serial.println();

      // static bool f = 0;
      // if (!f && dr.getPos() == 200) {
      //   f = 0;
      //   dr.setTarget(201);
      // }
    }
  */

  // static uint32_t tmr;
  // if (millis() - tmr >= 10) {
  //   tmr = millis();
  // dr.setTarget(a0);
  // }

  if (b0) {
    dr.setTarget(0);
  }
  if (b1) {
    dr.setTarget(500);
  }
  if (b2) {
    dr.stop();
  }
}