#include <Arduino.h>

#define PIN_HALFDONE  4
#define PIN_DONE      5
#define PIN_MATH      6
#define PIN_SEQUENCE 14
#define PIN_CAM      15
#define PIN_INPUT    A0

int TPS = 0;
int sequence[48] = {0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0};
int cam[48] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
int tooth = 0;
int low = 0;
int high = 0;
int RPMlow = 0;
int RPMhigh = 0;
int math = 1;
int halfdone = 0;
int done = 1;
int currentmicros = 0;
int previousmicros = 0;

void setup() {
  Serial.begin(115200);
  while (!Serial);
  Serial.println("Hello from the Pico!"); // not working but would be nice
  pinMode(PIN_INPUT, INPUT);
  pinMode(PIN_SEQUENCE, OUTPUT);
  pinMode(PIN_CAM, OUTPUT);
  pinMode(PIN_HALFDONE, OUTPUT);
  pinMode(PIN_DONE, OUTPUT);
  pinMode(PIN_MATH, OUTPUT);
}

void loop() {
  // update outputs
  digitalWrite(PIN_HALFDONE, halfdone);
  digitalWrite(PIN_DONE, done);
  digitalWrite(PIN_MATH, math);
  digitalWrite(PIN_SEQUENCE, sequence[tooth]);
  digitalWrite(PIN_CAM, cam[tooth]);

  if (halfdone == 0) {
    if (currentmicros - previousmicros >= RPMlow) {
      halfdone = 1;
      done = 0;
      previousmicros = currentmicros;
    }
  }

  if (done == 0) {
    if (currentmicros - previousmicros >= RPMhigh) {
      done = 1;
      math = 1;
      previousmicros = currentmicros;
    }
  }

  // increment output state logic
  if (math == 1) {
    tooth++;
    if (tooth == 48) {
      tooth = 0;
    }

    TPS = analogRead(A0);
    sequence[tooth];
    cam[tooth];
    if (sequence[tooth] == 0) {
      low = 1;
      high = 2;
    }
    else {
      low = 2;
      high = 1;
    }
    halfdone = 0;
    math = 0;
    RPMlow = low * TPS;
    RPMhigh = high * TPS;
    //RPMlow = 100; // for debugging
    //RPMhigh = 100;
  }
  currentmicros+=1000;
}