#include <Arduino.h>

#define LED 18
#define HALL 25

volatile unsigned long ts[4]={0,0,0,0};
volatile int tsi = 0;

void IRAM_ATTR recordTime() {
  tsi+=1;
  tsi&=3;
  ts[tsi] = micros();
}

void setup() {
  pinMode(LED, OUTPUT);
  pinMode(HALL, INPUT);
  digitalWrite(LED, HIGH);
  Serial.begin(115200);
  attachInterrupt(digitalPinToInterrupt(HALL), recordTime, FALLING);
}

double step = 0.25;
double golden = 1.0-(sqrt(5)-1)/2;
double nextAngle = golden;

int lasti = 0;
bool full = false;

double dts[4]={0,0,0,0};

void loop() {
  if (tsi!=lasti) { // Tick has happend;
    lasti = tsi;
    if (tsi==0) {
      full = true;
    }
    if (full) {
      double angle = tsi*step;
      if (nextAngle>angle && nextAngle-angle<step) {
        double dt = ts[tsi]-ts[(tsi+3)&3];
        dts[tsi] = 0.8*dts[tsi]+0.2*dt;
        unsigned int w = dts[(tsi+1)&3]*(nextAngle-angle)/step;
        if (w<200000) {
          delayMicroseconds(w);
        }
        digitalWrite(LED, LOW);
        delayMicroseconds(200);
        digitalWrite(LED, HIGH);
        nextAngle += golden;
        if (nextAngle>1.0) {
          nextAngle -= 1.0;
        }
      }
    }
  }
}