 #define DEBUG
 
 // Analog pin defines
-#define RIGHTLEFT 0
-#define TOPBOTTOM 1
+#define LEFTRIGHT 1
+#define TOPBOTTOM 0
 #define ACCEL 2
 
 // Digital pin defines

#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_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 LEFT LOW
#define RIGHT HIGH

volatile int state = 0;
volatile int distance = 0;
volatile int defect_array_index;

volatile int adc_pin = 0;
volatile int adc_val[6] = {0,0,0,0,0,0};

volatile int defect_points[8][MAX_DEFECT_POINTS];

void setup () {
    // PWM
    TCNT0  = 0; OCR0A = 15; TCCR0A = (1 << WGM01);
    TCCR0B = (1 << CS02) | (1 << CS00); TIMSK0 |= (1 << OCIE0A);
    
    // ADC
    adc_pin = 0;

    pinMode(SE, INPUT); pinMode(SW, INPUT);

    // Motors
    pinMode(LR1, OUTPUT); pinMode(R2, OUTPUT); pinMode(L2, OUTPUT);
    pinMode(TB1, OUTPUT); pinMode(TB2, OUTPUT);
    
    pinMode(13, OUTPUT);
    
    aWrite(LR1, 0); aWrite(L2, 0); aWrite(R2, 0);
    aWrite(TB2, HIGHSPEED);  aWrite(TB1, 0);
    distance_LR = 0; distance_TB = 0;
    
    state = 0;
    distance_TB = 0;
    distance_LR = 0;

    // Encoders
    //pinMode(L_E1, INPUT); pinMode(T_E1, INPUT);
    
    attachInterrupt(0, enTB, RISING);
    attachInterrupt(1, enLR, RISING);

    sei();
}

void loop() {

    
    // TODO: Later. Agregate defect data after up/down pass.

    delay(250); 
    digitalWrite(13, HIGH);
    delay(250);
    digitalWrite(13, LOW);
    /*
    if (digitalRead(NE) == HIGH) Serial.print("NE Defect\n");
    if (digitalRead(NW) == HIGH) Serial.print("NW Defect\n");
    if (digitalRead(SE) == HIGH) Serial.print("SE Defect\n");
    if (digitalRead(SW) == HIGH) Serial.print("SW Defect\n"); */
    // Serial.print("State: ");
    // Serial.print(state);
    //Serial.print(adc_val[0]);
   //  Serial.print(state);
    //if (adc_val[0] > 300) Serial.print("!!! DEFECT !!! DEFECT !!! DEFECT !!!! DEFECT !!!\n");
    //else
    // Serial.print("\n");
    
    Serial.print("\nState: ");
    Serial.print(state);
    Serial.print(" TB min: ");
    Serial.print(min_val[TOPBOTTOM]);
    Serial.print("  max: ");
    Serial.print(max_val[TOPBOTTOM]);
    Serial.print(" LR min: ");
    Serial.print(min_val[LEFTRIGHT]);
    Serial.print("  max: ");
    Serial.print(max_val[LEFTRIGHT]);
    for (int i = 0; i < 6; i++) {
        min_val[i] = 0x8000;
        max_val[i] = 0;
    }
}

// PID control
ISR(TIMER2_COMPA_vect) {
    // adc_pin = adc_pin % 6;
    // TODO: Later. Add tilt correction.
    // TODO: Add height correction while moving along top and bottom.
    // TODO: make corrections use PID
}

// PWM timer
ISR(TIMER1_COMPA_vect) {
    
    // Moving up and down
    if ((state % 9 == 3) || (state % 9 == 4) || (state % 9 == 5)) {
        distance_LR++;
        if ((state % 9 == 3) && (distance_LR > HEIGHT - BUFFER)) {
            state++;
            if (state % 18 < 9) {
                aWrite(L2, DOWNLOWSPEED2);
                aWrite(R2, DOWNLOWSPEED2);
                aWrite(LR1, DOWNLOWSPEED1);
            }
            else {
                aWrite(L2, UPLOWSPEED);
                aWrite(R2, UPLOWSPEED);
            }
        }
        else if ((state % 9 == 4) && (distance_LR > HEIGHT))
            state++;
        else if ((state % 9 == 5) && (distance_LR > HEIGHT + SLACK)) {
            state++;
            aWrite(L2, STALL_TORQUE);
            aWrite(R2, STALL_TORQUE);
            aWrite(LR1, 0);
            aWrite(TB1, HIGHSPEED);
            distance_TB = 0;
        }
        if (defect_array_index < MAX_DEFECT_POINTS) {
            switch (distance_LR % COLOR_SCALAR) {
                case 0: //defect_points[NE][defect_array_index] = digitalRead(NE); break;
                case 1: //defect_points[NW][defect_array_index] = digitalRead(NW); break;
                case 2: //defect_points[SE][defect_array_index] = digitalRead(SE); break;
                case 3: //defect_points[SW][defect_array_index] = digitalRead(SW); break;
                case 4: defect_array_index++; break;
            }
        }
    }
    
    
    pwmCtr = (pwmCtr + PWMRES) % 256;
    
    if (pwmCtr < pwm_out[TB1])
        digitalWrite(TB1, HIGH);
    else
        digitalWrite(TB1,  LOW);
    if (pwmCtr < pwm_out[TB2])
        digitalWrite(TB2, HIGH);
    else
        digitalWrite(TB2,  LOW);
    if (pwmCtr < pwm_out[L2])
        digitalWrite(L2, HIGH);
    else
        digitalWrite(L2,  LOW);
    if (pwmCtr < pwm_out[R2])
        digitalWrite(R2, HIGH);
    else
        digitalWrite(R2,  LOW);
    if (pwmCtr < pwm_out[LR1])
        digitalWrite(LR1, HIGH);
    else
        digitalWrite(LR1,  LOW); 
}

ISR(ADC_vect)
{
    unsigned long analogVal = (ADCL | (ADCH << 8)) << 4;
    int curr_adc_pin = adc_pin;
    if (state % 9 == 5) adc_pin = TOPBOTTOM;
    else if (state % 9 == 8) adc_pin = LEFTRIGHT;
    else adc_pin = ACCEL;

    // Start the next ADC
    ADMUX &= B11110000; ADMUX |= adc_pin; ADCSRA |= B01000000;
    
    adc_val[curr_adc_pin] += (analogVal >> 6) - (adc_val[curr_adc_pin] >> 6);
    if (adc_val[curr_adc_pin] < min_val[curr_adc_pin]) 
        min_val[curr_adc_pin] = adc_val[curr_adc_pin];
    if (adc_val[curr_adc_pin] > max_val[curr_adc_pin])
        max_val[curr_adc_pin] = adc_val[curr_adc_pin];

    // Update things that use ADC values
    
    // Moving up/down
    if ((curr_adc_pin == TOPBOTTOM) && (
            ((state % 18 == 5 ) && (adc_val[curr_adc_pin] <  LOW_THRESHOLD_TB)) ||
            ((state % 18 == 14) && (adc_val[curr_adc_pin] > HIGH_THRESHOLD_TB)))) {
        state++;
        aWrite(L2, STALL_TORQUE);
        aWrite(R2, STALL_TORQUE);
        aWrite(LR1, 0);
        aWrite(TB1, HIGHSPEED);
        distance_TB = 0;
    }
    // Moving Left
    else if (((curr_adc_pin == LEFTRIGHT) && (state % 9 == 8)) &&
             (adc_val[curr_adc_pin] > HIGH_THRESHOLD_LR)) {
        state++;
        distance_TB = 0;
        aWrite(TB2, 0);
        aWrite(TB1, HIGHSPEED);
    }
}

void aWrite(int pin, int sp) {
    pwm_out[pin] = sp;
}
