#include "Controls.h"


uint8_t controlPins[] = {
    PIN_A,
    PIN_B,
    PIN_KEY,
};



uint16_t prevState[3];

auto lastChange = micros();
uint8_t pinChanges[128];
uint8_t pinChangeIndex = 0;

void Controls_direction_reset(){
    lastChange = 0;
    pinChangeIndex = 0;
}

auto lastKey = micros();

ControlsCallback handler = NULL;

void Controls_handler(ControlsCallback newHandler){
    handler = newHandler;
}

void Controls_send_event(ControlEvent event){
    if(handler){
        handler(event);
    }
}

void Controls_loop(void *pvParameters){
    lastKey = 0;
    lastChange = 0;
    
    while(true){
        for(uint8_t a=0; a<sizeof(controlPins); a++){
            auto pin = controlPins[a];
            auto currentValueAnalog = analogRead(pin);
            auto currentValue = currentValueAnalog > 3072;
            auto prevValue = prevState[a];

            if(CONTROLS_DEBUG){
                Serial.printf("Pin %d: %d->%d (%d)\n", pin, prevValue, currentValue, currentValueAnalog);
            }

            // if(abs(currentValue - prevValue) > 100){
            if(currentValue != prevValue){
                if(pin == PIN_KEY){
                    if(currentValue){
                        auto duration = micros() - lastKey;
                        if(duration > KEY_DURATION_MS * 1000){
                            if(CONTROLS_DEBUG){
                                Serial.printf("Key: %d\n", duration);
                            }
                            Controls_send_event(Key);
                        }else{
                            if(CONTROLS_DEBUG){
                                Serial.println("Too short");
                            }
                        }
                        lastKey = 0;
                    } else {
                        lastKey = micros();
                    }

                }else{
                    pinChanges[pinChangeIndex++] = pin;
                    lastChange = micros();
                }
            }

            prevState[a] = currentValue;
        }

        //maybe we have a valid sequence
        if(pinChangeIndex >= 4){
            // Serial.printf("Have %d changes\n", pinChangeIndex);
            for(uint8_t base = 0; base + 4 <= pinChangeIndex; base += 4){
                if(
                    pinChanges[base + 0] == PIN_A &&
                    pinChanges[base + 1] == PIN_B &&
                    pinChanges[base + 2] == PIN_A &&
                    pinChanges[base + 3] == PIN_B
                    ){

                    if(CONTROLS_DEBUG){
                        Serial.println("Dir A");
                    }
                    Controls_send_event(Counterclockwise);
                }else if(
                    pinChanges[base + 0] == PIN_B &&
                    pinChanges[base + 1] == PIN_A &&
                    pinChanges[base + 2] == PIN_B &&
                    pinChanges[base + 3] == PIN_A
                ){

                    if(CONTROLS_DEBUG){
                        Serial.println("Dir B");
                    }
                    Controls_send_event(Clockwise);
                }else{
                    if(CONTROLS_DEBUG){
                        Serial.println("Unknown direction");
                    }
                }
            }
            Controls_direction_reset();
        }

        if(lastChange){
            if(micros() - lastChange > 100 * 1000){

                if(CONTROLS_DEBUG){
                    Serial.println("changes expired");
                }
                Controls_direction_reset();
            }
        }

        vTaskDelay(1/portTICK_PERIOD_MS);

    }
}


void Controls_setup(){
    pinMode(PIN_A, INPUT_PULLDOWN);
    pinMode(PIN_B, INPUT_PULLDOWN);
    pinMode(PIN_KEY, INPUT_PULLDOWN);

    
    TaskHandle_t taskControls = NULL;
    xTaskCreate(Controls_loop, "Controls_loop", 1000, NULL, tskIDLE_PRIORITY, &taskControls);
}