int pos = 0;
int dir_pin = 3;
int step_pin = 2;
int dir_out_a_pin = 4;
int pwm_out_a_pin = 5;
int dir_out_b_pin = 7;
int pwm_out_b_pin = 6;

int dir_out_a = 1;
int dir_out_b = 1;
int pwm_out_a = 0;
int pwm_out_b = 0;
int micro_steps = 32;
float angle = 0;
const float pi = 3.14159267;

void setup() {
  Serial.begin(9600);
  setPwmFrequency(5, 1);
  pinMode(dir_pin, INPUT_PULLUP);
  pinMode(step_pin, INPUT_PULLUP);
  pinMode(dir_out_a_pin, OUTPUT);
  pinMode(dir_out_a_pin, OUTPUT);
  pinMode(pwm_out_a_pin, OUTPUT);
  pinMode(dir_out_b_pin, OUTPUT);
  pinMode(pwm_out_b_pin, OUTPUT);
  attachInterrupt(digitalPinToInterrupt(step_pin), step, FALLING);
}

void loop() {
//  Serial.println(digitalRead(dir_pin));
  delay(1);
}

void setPwmFrequency(int pin, int divisor) {
  byte mode;
  if(pin == 5 || pin == 6 || pin == 9 || pin == 10) {
    switch(divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 64: mode = 0x03; break;
      case 256: mode = 0x04; break;
      case 1024: mode = 0x05; break;
      default: return;
    }
    if(pin == 5 || pin == 6) {
      TCCR0B = TCCR0B & 0b11111000 | mode;
    } else {
      TCCR1B = TCCR1B & 0b11111000 | mode;
    }
  } else if(pin == 3 || pin == 11) {
    switch(divisor) {
      case 1: mode = 0x01; break;
      case 8: mode = 0x02; break;
      case 32: mode = 0x03; break;
      case 64: mode = 0x04; break;
      case 128: mode = 0x05; break;
      case 256: mode = 0x06; break;
      case 1024: mode = 0x07; break;
      default: return;
    }
    TCCR2B = TCCR2B & 0b11111000 | mode;
  }
}

float degree(float radians) {
  return radians / 2 / pi * 360;
}

float radian(float degrees) {
  return degrees / 360 * 2 * pi;
}

float abs_cos(float angle_in_degree) {
  return abs(cos(radian(angle_in_degree)));
}

float abs_sin(float angle_in_degree) {
  return abs(sin(radian(angle_in_degree)));
}

int pwm_out(float input) {
  return 255/1*input;
}

void step() {
  // restart pos loop if we reach set microsteps in count up
  if(pos == micro_steps) pos = 0;
  // restart pos loop if we reach set microsteps in count down
  if(pos == -1) pos = micro_steps-1;
  // find angle from pos
  angle = 360.0f/micro_steps*pos;
  // set phase a
  if(angle >= 90.0f) dir_out_a = 0;
  if(angle >= 270.0f) dir_out_a = 1;
  // set phase b
  if(angle >= 0.0f) dir_out_b = 1;
  if(angle >= 180.0f) dir_out_b = 0;
  // read dir input
  int dir=digitalRead(dir_pin);
  // count up or down depending on dir input
    if(dir == 1) {
    pos++;
  } else {
    pos--;
  }
  // write to pins
  // dir pins
  digitalWrite(dir_out_a_pin, dir_out_a);
  digitalWrite(dir_out_b_pin, dir_out_b);
  // pwm pins
  analogWrite(pwm_out_a_pin, pwm_out(abs_cos(angle)));
  analogWrite(pwm_out_b_pin, pwm_out(abs_sin(angle)));
  /*
  Serial.print("pos: ");
  Serial.print(pos);
  Serial.print(" dir_a: ");
  Serial.print(dir_out_a);
  Serial.print(" cur_a: ");
  Serial.print(pwm_out(abs_cos(angle)));
  Serial.print(" dir_b: ");
  Serial.print(dir_out_b);
  Serial.print(" cur_b: ");
  Serial.print(pwm_out(abs_sin(angle)));
  Serial.print(" Angle: ");
  Serial.println(angle, DEC);
  */
}