#define QUAD_A_PIN            2
#define QUAD_B_PIN            3
#define DEBUG_PIN             4
#define MOTOR_SPEED_PIN       5
#define MOTOR_DIR_PIN         6
#define MOTOR_SLEEP_PIN       7
#define MOTOR_MODE_PIN        8
#define STEP_PIN              11
#define DIR_PIN               12
#define ENABLE_PIN            13

volatile int position;
int target;

char incoming; // incoming serial data
char cmd_arr[80]={0};
int _i=0;

bool newStep = false;
bool newEnable = false;
bool oldStep = false;
bool oldEnable = true;
bool dir = false;
bool olddir = false;

void setup()
{
    Serial.begin(115200);

    while(!Serial.available()){
      Serial.println("A");
      __builtin_avr_delay_cycles(10000000);
    }
    Serial.flush();

    Serial.println("MakerBot DC Servo Controller v1.0");

    pinMode(QUAD_A_PIN, INPUT);
    pinMode(QUAD_B_PIN, INPUT);

    pinMode(DEBUG_PIN, OUTPUT);

    pinMode(MOTOR_SLEEP_PIN, OUTPUT);
    digitalWrite(MOTOR_SLEEP_PIN, LOW);

    pinMode(MOTOR_MODE_PIN, OUTPUT);
    digitalWrite(MOTOR_MODE_PIN, LOW);

    pinMode(MOTOR_SPEED_PIN, OUTPUT);
    digitalWrite(MOTOR_SPEED_PIN, LOW);

    pinMode(MOTOR_DIR_PIN, OUTPUT);
    digitalWrite(MOTOR_DIR_PIN, LOW);

    pinMode(STEP_PIN, INPUT);
    pinMode(DIR_PIN, INPUT);
    pinMode(ENABLE_PIN, INPUT);

    attachInterrupt(0, read_quadrature_a, CHANGE);
    attachInterrupt(1, read_quadrature_b, CHANGE);
}

void loop()
{
    newEnable = digitalRead(ENABLE_PIN);
    newStep = digitalRead(STEP_PIN);
    dir = digitalRead(DIR_PIN);
    byte motor_speed = 0;


    if (Serial.available()) {
        incoming = Serial.read();
        if (incoming == 's'){  //step
            newStep ^= 1;
            Serial.flush();
            Serial.print("newStep=");
            Serial.println(newStep,DEC);
        }else if (incoming == 'e'){  //enable
            newEnable ^= 1;
            Serial.flush();
            Serial.print("newEnable=");
            Serial.println(newEnable,DEC);
        }else if (incoming == 'a'){  //direction
            digitalWrite(MOTOR_DIR_PIN, HIGH);
            Serial.flush();
            Serial.println("Dir=1");
        }else if (incoming == 'd') { //direction
            digitalWrite(MOTOR_DIR_PIN, LOW);
            Serial.flush();
            Serial.println("Dir=0");
        }else if (incoming == '\n' || incoming == '\r'){ //enter
            cmd_arr[_i]=0;
            motor_speed=atoi(cmd_arr);
            analogWrite(MOTOR_SPEED_PIN, motor_speed);
            Serial.flush();
                        Serial.println();
            Serial.print("motor_speed=");
            Serial.println(motor_speed,DEC);
            _i=0;
            cmd_arr[_i]=0;
        }else if ((incoming > 48) or (incoming < 57)){ //numbers only
            cmd_arr[_i]=incoming; //copy the serial byte to the array
            _i++; // increase the array index
            Serial.flush();
            //Serial.print(_i);
            Serial.print(incoming);
        }
    }


    //low to high transition
    if (!oldEnable && newEnable)
    {
        //enable is active low, so disable.
        digitalWrite(MOTOR_SLEEP_PIN, LOW);
        digitalWrite(MOTOR_SPEED_PIN, LOW);
    }
    // high to low transition
    else if (oldEnable && !newEnable)
    {
        //enable is active low, so enable.
        digitalWrite(MOTOR_SLEEP_PIN, HIGH);
        delay(1); //give it a millisecond to turn on.
    }

    //enable is active low, so only do this if we're enabled.
    if (!newEnable)
    {

        if (olddir != dir) {
          target=0;
          position=0;
      olddir = dir;
        }

        // step signal is on the low to high transition.
        if (!oldStep && newStep)
        {
            if (dir)
                target++;
            else
                target--;
        }

                motor_speed = 0;
                int distance = abs(position-target);

                if (distance > 255)
                  motor_speed = 255;
                else
                  motor_speed = distance;

        //super primitive control of the motor.
        if (position > target)
        {
            digitalWrite(MOTOR_DIR_PIN, HIGH);
            analogWrite(MOTOR_SPEED_PIN, motor_speed);
        }
        else if (position < target)
        {
            digitalWrite(MOTOR_DIR_PIN, LOW);
            analogWrite(MOTOR_SPEED_PIN, motor_speed);
        }
        else
        {
            digitalWrite(MOTOR_SPEED_PIN, LOW);
        }

        oldStep = newStep;
    }

    oldEnable = newEnable;

        /*
          delay(1000);
          Serial.print("Pos:");
          Serial.println(position, DEC);
        */
}

void read_quadrature_a()
{
    // found a low-to-high on channel A
    if (digitalRead(QUAD_A_PIN) == HIGH)
    {
        // check channel B to see which way
        if (digitalRead(QUAD_B_PIN) == LOW)
            position--;
        else
            position++;
    } // found a high-to-low on channel A
    else
    { // check channel B to see which way
         if (digitalRead(QUAD_B_PIN) == LOW)
             position++;
         else
            position--;
    }
}

void read_quadrature_b()
{
    // found a low-to-high on channel A
    if (digitalRead(QUAD_B_PIN) == HIGH)
    {
        // check channel B to see which way
        if (digitalRead(QUAD_A_PIN) == LOW)
            position++;
        else
            position--;
    } // found a high-to-low on channel A
    else
    { // check channel B to see which way
         if (digitalRead(QUAD_A_PIN) == LOW)
             position--;
         else
            position++;
    }
}