#define PIN_ENL A1
#define PIN_LW A2
#define PIN_LB A3
#define PIN_LR A4
#define PIN_LG A5

#define PIN_ENR 7
#define PIN_RW 8
#define PIN_RB 9
#define PIN_RR 10
#define PIN_RG 11

#define BPIN 12

#define STEPNUM 200
#define STEPANG 1.8
#define AXLEDIST 295
#define DIAMETER 100
#define MINTIME 10

int phaseLeft = 1;
int phaseRight = 1;

// these variables were failing to be declared inside a function, so they're global to suppress that error
unsigned long gtime;
long outSteps,inSteps = 0;
long a,b,c,d;
long num,den;

void setup() {
  pinMode(PIN_ENL, OUTPUT);
  pinMode(PIN_LW, OUTPUT);
  pinMode(PIN_LB, OUTPUT);
  pinMode(PIN_LR, OUTPUT);
  pinMode(PIN_LG, OUTPUT);

  pinMode(PIN_ENR, OUTPUT);
  pinMode(PIN_RW, OUTPUT);
  pinMode(PIN_RB, OUTPUT);
  pinMode(PIN_RR, OUTPUT);
  pinMode(PIN_RG, OUTPUT);

  pinMode(BPIN, INPUT_PULLUP);

  writeMotors();

// This loop stalls the program until the start button is pressed
// It also aligns the motors with the phase, to avoid errors
// They are pulsed on and off to waste less battery and prevent overheating
  while (digitalRead(BPIN)==HIGH) {
    killMotors();
    delay(50);
    writeMotors();
    delay(10);
  }

  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH); //built-in LED indicator, turns on to indicate program has started
//  Serial.begin(9600);
  delay(3000);
}

void loop() {
  curve(300,-30,MINTIME,true); // CCW turn, 300mm radius, 30 degrees, forwards
  straight(180,true); // drive forwards 180mm
  curve(0,-64,MINTIME,true); // CCW turn, on the spot, 64 degrees
  straight(600,true); // drive forwards 600mm
  killMotors();
  delay(1000000);
}

// car drives in a straight line for <dist> millimetres
// dir: true->forwards, false->backwards
void straight(long dist,bool dir) {
  for (long i=0;i*DIAMETER*PI<dist*STEPNUM;i++) {
    dir?stepForwardLeft():stepBackLeft();
    dir?stepForwardRight():stepBackRight();
    delay(MINTIME);
  }
}

// car drives in a circular arc
// pivot->centre of circle, in millimetres to the right of the midpoint between the wheels
// angle->number of degrees to turn. positive->clockwise, negative->counter-clockwise
// timer->number of milliseconds between each step of the outer wheel. typically MINTIME, use higher numbers if it struggles with torque
// dir: true->forwards, false->backwards
void curve(long pivot, long angle, long timer, bool dir) {
  gtime=millis();
  outSteps=0;
  inSteps=0;
  num=0;
  den=1;
  while (abs(num/den)<abs(angle)) {
    d=outSteps*timer;
    if (millis()-gtime>=d) {
      (angle>0)?(dir?stepForwardLeft():stepBackRight()):(dir?stepForwardRight():stepBackLeft());
      outSteps++;
    }
    a=inSteps*timer;
    b=(2*pivot)+AXLEDIST;
    c=(2*pivot)-AXLEDIST;
    d=(a*b)/c;
    if (2*pivot>AXLEDIST && millis()-gtime>=d) {
      (angle>0)?(dir?stepForwardRight():stepBackLeft()):(dir?stepForwardLeft():stepBackRight());
      inSteps++;
    }
    if (2*pivot<AXLEDIST && millis()-gtime>=d) {
      (angle>0)?(dir?stepBackRight():stepForwardLeft()):(dir?stepBackLeft():stepForwardRight());
      inSteps--;
    }
    delay(1);
    num=outSteps*DIAMETER*360;
    den=STEPNUM*(2*pivot+AXLEDIST);
  }
}

void stepForwardLeft() {
  phaseLeft++;
  writeMotors();
}

void stepBackLeft() {
  phaseLeft--;
  writeMotors;
}

void stepForwardRight() {
  phaseRight++;
  writeMotors();
}

void stepBackRight() {
  phaseRight--;
  writeMotors();
}

void killMotors() {
  digitalWrite(PIN_ENL, LOW);
  digitalWrite(PIN_LW, LOW);
  digitalWrite(PIN_LB, LOW);
  digitalWrite(PIN_LR, LOW);
  digitalWrite(PIN_LG, LOW);

  digitalWrite(PIN_ENR, LOW);
  digitalWrite(PIN_RW, LOW);
  digitalWrite(PIN_RB, LOW);
  digitalWrite(PIN_RR, LOW);
  digitalWrite(PIN_RG, LOW);
}

void writeMotors() {
  while (phaseLeft>4) phaseLeft-=4;
  while (phaseLeft<1) phaseLeft+=4;
  while (phaseRight>4) phaseRight-=4;
  while (phaseRight<1) phaseRight+=4;
  digitalWrite(PIN_ENL, HIGH);
  switch (phaseLeft) {
    case 1:
      digitalWrite(PIN_LW, HIGH);
      digitalWrite(PIN_LB, LOW);

      digitalWrite(PIN_LR, HIGH);
      digitalWrite(PIN_LG, LOW);
      break;
    case 2:
      digitalWrite(PIN_LW, LOW);
      digitalWrite(PIN_LB, HIGH);

      digitalWrite(PIN_LR, HIGH);
      digitalWrite(PIN_LG, LOW);
      break;
    case 3:
      digitalWrite(PIN_LW, LOW);
      digitalWrite(PIN_LB, HIGH);

      digitalWrite(PIN_LR, LOW);
      digitalWrite(PIN_LG, HIGH);
      break;
    case 4:
      digitalWrite(PIN_LW, HIGH);
      digitalWrite(PIN_LB, LOW);

      digitalWrite(PIN_LR, LOW);
      digitalWrite(PIN_LG, HIGH);
      break;
  }
  digitalWrite(PIN_ENR, HIGH);
  switch (phaseRight) {
    case 1:
      digitalWrite(PIN_RW, HIGH);
      digitalWrite(PIN_RB, LOW);

      digitalWrite(PIN_RR, HIGH);
      digitalWrite(PIN_RG, LOW);
      break;
    case 2:
      digitalWrite(PIN_RW, HIGH);
      digitalWrite(PIN_RB, LOW);

      digitalWrite(PIN_RR, LOW);
      digitalWrite(PIN_RG, HIGH);
      break;
    case 3:
      digitalWrite(PIN_RW, LOW);
      digitalWrite(PIN_RB, HIGH);

      digitalWrite(PIN_RR, LOW);
      digitalWrite(PIN_RG, HIGH);
      break;
    case 4:
      digitalWrite(PIN_RW, LOW);
      digitalWrite(PIN_RB, HIGH);

      digitalWrite(PIN_RR, HIGH);
      digitalWrite(PIN_RG, LOW);
      break;
  }
}