#include "io.h"
#include <can/can_client.h>

struct GPIO_KEY_DATA gpioKeyStop;
struct GPIO_KEY_DATA gpioKeyPwrUp;
struct GPIO_KEY_DATA gpioKeyPwrDown;

int IO_Init()
{
    if (wiringPiSetupPinType(WPI_PIN_BCM))
    {
        printf("IO: Set up wiringPi failed!\n");
        return 1;
    }

    // IO-Pins für Tasten konfigurieren
    SetupKeyPin(&gpioKeyStop, GPIO_KEY_STOP);
    SetupKeyPin(&gpioKeyPwrUp, GPIO_KEY_PWRUP);
    SetupKeyPin(&gpioKeyPwrDown, GPIO_KEY_PWRDOWN);

    // IO-Pins für Ausgänge konfigurieren
    SetupOutputPin(GPIO_LED_MOTRUN);

    // Einschaltsequenz
    WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
    delay(500);
    WriteOutputPin(GPIO_LED_MOTRUN, LOW);
    delay(500);
    WriteOutputPin(GPIO_LED_MOTRUN, HIGH);
    delay(500);
    WriteOutputPin(GPIO_LED_MOTRUN, LOW);
    delay(500);

    printf("IO initialized successfull!\n");

    return 0;
}


void SetupKeyPin(struct GPIO_KEY_DATA *pdata, int iKeyPin)
{
    pdata->iKeyPin = iKeyPin;
    pdata->iKeyValue = 0;
    pdata->iKeyRisingEdge = 0;
    pdata->iKeyFallingEdge = 0;
    pdata->nHighCycleCounter = 0;
    pdata->nLowCycleCounter = 0;
    pdata->iKeyPressedCycleCounter = 0;
    pdata->iKeyRepeatCycleCounter = 0;

    // Wenn Eingang verwendet wird
    if (pdata->iKeyPin > 0)
    {
        pinMode(pdata->iKeyPin, INPUT);
        pullUpDnControl(pdata->iKeyPin, PUD_UP);
    }
}


void ReadKey(struct GPIO_KEY_DATA *pdata)
{
    if (pdata->iKeyPin > 0)
    {
        int newval = pdata->iKeyValue;

        if (digitalRead(pdata->iKeyPin) == LOW)     // invertierte Logik weil wir PullUp-Widerstand bei Betätigung auf low ziehen
        {
            // Signal liegt an
            if (pdata->nLowCycleCounter > 0)
            {
                pdata->nLowCycleCounter--;
            }

            if (pdata->nHighCycleCounter < KEY_RISING_FILTERCYCLES)
            {
                pdata->nHighCycleCounter++;
                if (pdata->nHighCycleCounter >= KEY_RISING_FILTERCYCLES)
                {
                    // gewünschte Anzahl Zyklen stabil
                    newval = 1;
                }
            }
        }
        else
        {
            // Signal liegt nicht an
            if (pdata->nHighCycleCounter > 0)
            {
                pdata->nHighCycleCounter--;
            }

            if (pdata->nLowCycleCounter < KEY_FALLING_FILTERCYCLES)
            {
                pdata->nLowCycleCounter++;
                if (pdata->nLowCycleCounter >= KEY_FALLING_FILTERCYCLES)
                {
                    // gewünschte Anzahl Zyklen stabil
                    newval = 0;
                }
            }
        }

        if (newval && !pdata->iKeyValue)
        {
            // Taster wurde betätigt
            pdata->iKeyRisingEdge = 1;
            pdata->iKeyValue = newval;
            pdata->iKeyPressedCycleCounter = 0;
            pdata->iKeyRepeatCycleCounter = 0;
        }
        else if (pdata->iKeyValue && !newval)
        {
            // Taster wurde losgelassen
            pdata->iKeyFallingEdge = 1;
            pdata->iKeyValue = newval;
        }
        else
        {
            // Keine Änderung
            pdata->iKeyRisingEdge = 0;
            pdata->iKeyFallingEdge = 0;

            if (pdata->iKeyValue)
            {
                // Wenn Taste gedrückt ist
                if (pdata->iKeyPressedCycleCounter < KEY_START_REPEAT_CYCLECOUNT)
                {
                    // Zyklen zählen
                    pdata->iKeyPressedCycleCounter++;
                }

                if (pdata->iKeyPressedCycleCounter >= KEY_START_REPEAT_CYCLECOUNT)
                {
                    // Wenn Taste länger als KEY_START_REPEAT_CYCLECOUNT gedrückt ist
                    pdata->iKeyRepeatCycleCounter++;
                    if (pdata->iKeyRepeatCycleCounter >= KEY_REPEAT_CYCLECOUNT)
                    {
                        // alle KEY_REPEAT_CYCLECOUNT Zyklen Tastendruck signalisieren
                        pdata->iKeyRisingEdge = 1;
                        pdata->iKeyRepeatCycleCounter = 0;
                    }
                }
            }
        }
    }
}


void SetupOutputPin(int iOutPin)
{
    if (iOutPin > 0)
    {
        pinMode(iOutPin, OUTPUT);
        digitalWrite(iOutPin, LOW);
    }
}

void WriteOutputPin(int iOutPin, int iValue)
{
    if (iOutPin > 0)
    {
        digitalWrite(iOutPin, iValue);
    }
}


void IO_DoCyclic()
{
    ReadKey(&gpioKeyStop);
    ReadKey(&gpioKeyPwrUp);
    ReadKey(&gpioKeyPwrDown);

    if (gpioKeyStop.iKeyValue)
    {
        // Stop-Taste betätigt -> hat Vorrang vor den anderen Tasten
        if (gpioKeyStop.iKeyRisingEdge)
        {
            Can_SetMotorGear(0, 0);
        }
    }
    else
    {
        if (gpioKeyPwrUp.iKeyRisingEdge)
        {
            // Leistung erhöhen
            if (motctrl[0].iMotorGear == MOTOR_GEAR_NEUTRAL)
            {
                Can_SetMotorGear(0, 1);
                Can_SetMotorPower(0, MOTOR_PWR_MIN_PCT);
            }
            else
            {
                Can_SetMotorPower(0, motctrl[0].iMotorPowerPct + MOTOR_PWR_STEP);
            }
        }

        if (gpioKeyPwrDown.iKeyRisingEdge)
        {
            // Leistung verringern
            if (motctrl[0].iMotorPowerPct > MOTOR_PWR_MIN_PCT)
            {
                Can_SetMotorPower(0, motctrl[0].iMotorPowerPct - MOTOR_PWR_STEP);
            }
            else
            {
                Can_SetMotorGear(0, 0);
            }
        }
    }
}