  *
  */
 
+#define DEBUG
+
 // Analog pin defines
-#define RIGHTLEFT 4
-#define TOPBOTTOM 5
-#define NE 0
-#define NW 1
-#define SE 2
-#define SW 3
+#define RIGHTLEFT 0
+#define TOPBOTTOM 1
+#define ACCEL 2
 
 // Digital pin defines
-#define ACCEL_L 4
-#define ACCEL_R 5
-#define L_E1 2
-#define T_E1 3
-#define LR_EN 7
-#define L_SP 6  // pwm
-#define LR_DIR 8
-#define R_SP 9  // pwm
-#define TB_EN 13
+#define RX 0 // Reserved
+#define TX 1 // Reserved
+#define L_E1 2 // Interrupt
+#define T_E1 3 // Interrupt
+#define NE 4
+#define NW 5
+#define SE 6
+#define SW 7
+// NOT_DEFINED 8
+#define LR_SP 9  // pwm
+#define B_SP 10 // pwm
 #define T_SP 11 // pwm
 #define TB_DIR 12
-#define B_SP 10 // pwm
+#define B_EN 13
 
 // Constant defines
+#define HIGH_THRESHOLD 16000
+#define LOW_THRESHOLD 14000
 #define MAX_DEFECT_POINTS 1000
 #define SPEED 192
+#define STALL_TORQUE 128
+#define LR_OFFSET 64
 #define OFFSET 10
 
-#define TOP 0
-#define BOTTOM 1
-#define LEFT 2
-#define RIGHT 3
+#define LEFT LOW
+#define RIGHT HIGH
 
 volatile int state = 0;
 volatile int distance = 0;
 volatile int defect_array_index;
 
-volatile int at_left = 0;
-volatile int at_right = 0;
-volatile int at_top = 0;
-volatile int at_bottom = 0;
-
-volatile int NE_defect = 0;
-volatile int NW_defect = 0;
-volatile int SE_defect = 0;
-volatile int SW_defect = 0;
-
 volatile int adc_pin = 0;
 volatile int adc_val[6] = {0,0,0,0,0,0};
 
-volatile int defect_points[4][MAX_DEFECT_POINTS];
+volatile int defect_points[8][MAX_DEFECT_POINTS];
 
 void setup () {
     // PWM

    adc_pin = 0;
    ADMUX |= B01000000; ADMUX &= B11010000; ADCSRA |= B11101111;
    
    // Motors
    pinMode(L_EN, OUTPUT); pinMode(L_SP, OUTPUT);
    pinMode(R_EN, OUTPUT); pinMode(R_SP, OUTPUT);
    pinMode(T_EN, OUTPUT); pinMode(T_SP, OUTPUT);
    pinMode(B_EN, OUTPUT); pinMode(B_SP, OUTPUT);
    pinMode(L_E1, OUTPUT); pinMode(T_E1, OUTPUT);

    attachInterrupt(0, enUD, CHANGE);
    attachInterrupt(1, enLR, CHANGE);


        distance += 1;
        if (distance > ((3 * WIDTH / 2) + (WIDTH * state))) {
            state += 1;
            digitalWrite(B_EN, LOW);
            analogWrite(T_SP, 0);
            analogWrite(B_SP, 0);
            if (state % 6 == 1) analogWrite(LR_SP, STALL_TORQUE - LR_OFFSET);
            else                analogWrite(LR_SP, STALL_TORQUE + LR_OFFSET);
        }
    }
}

ISR(TIMER0_COMPA_vect) {
    // TODO: Later. Add tilt correction.
    // TODO: Add height correction while moving along top and bottom.
    // TODO: make corrections use PID
}

ISR(ADC_vect)
{
    int analogVal = (ADCL | (ADCH << 8)) << 4;
    int curr_adc_pin = adc_pin;
    adc_pin = (adc_pin + 1) % 3;

    // Start the next ADC
    ADMUX &= B11110000; ADMUX |= adc_pin; ADCSRA |= B01000000;
