void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32-S2!");
  pinMode(33, OUTPUT);
  pinMode(34, OUTPUT);
  pinMode(35, OUTPUT);
}

double normalizePts(int pts) {
  return (pts / 4095.5) - 1;
}

bool conv(double sinOrCos) {
  if (sinOrCos >= 0) {
    return 1;
  } else {
    return 0;
  }
}

const double SHIFT_120 = (2 * PI)  / 3;
const int POLES = 5;

void loop() {
  // put your main code here, to run repeatedly:
  delay(100); // this speeds up the simulation
  double sine = normalizePts(analogRead(7));
  double cosine = normalizePts(analogRead(8));
  double angle = atan2(sine, cosine);
  if (angle < 0) {
    angle = angle + 6.28;
  }

  angle *= POLES;

  digitalWrite(
    33,
    conv(sin(angle))
  );

  angle += SHIFT_120;

  digitalWrite(
    34,
    conv(sin(angle))
  );

  angle += SHIFT_120;

  digitalWrite(
    35,
    conv(sin(angle))
  );

  //Serial.print(", ");
  //Serial.println(ptsToV(analogRead(8)));
}