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

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

char nInitialized = 0;

/// @brief Initialize the io pins
/// @return 
int IO_Init()
{
    if (wiringPiSetupPinType(WPI_PIN_BCM))
    {
        mylog(LOG_ERR, "IO: Set up wiringPi failed!");
        return 1;
    }
    nInitialized = 1;

    // config IO-pins for the keys
    SetupKeyPin(&gpioKeyStop, GPIO_KEY_STOP);
    SetupKeyPin(&gpioKeyPwrUp, GPIO_KEY_PWRUP);
    SetupKeyPin(&gpioKeyPwrDown, GPIO_KEY_PWRDOWN);

    // config IO-pins for outputs
    SetupOutputPin(GPIO_LED_MOTRUN);
    SetupOutputPin(GPIO_OUT_PWRON);

    mylog(LOG_INFO, "IO: Initialized successfull!");
    
    return 0;
}


/// @brief Setup a pin for a key input
/// @param pdata 
/// @param iKeyPin 
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;

    if ((pdata->iKeyPin > 0) && nInitialized)
    {
        //mylog(LOG_INFO, "IO: Config Pin %d as input", iOutPin);
        pinMode(pdata->iKeyPin, INPUT);
        pullUpDnControl(pdata->iKeyPin, PUD_UP);
    }
}


/// @brief Read a key input
/// @param pdata 
void ReadKey(struct GPIO_KEY_DATA *pdata)
{
    if (pdata->iKeyPin > 0)
    {
        int newval = pdata->iKeyValue;

        if (digitalRead(pdata->iKeyPin) == LOW)     // we use pull-up resistors so we have inverted logic
        {
            // key is pressed
            if (pdata->nLowCycleCounter > 0)
            {
                pdata->nLowCycleCounter--;
            }

            if (pdata->nHighCycleCounter < KEY_RISING_FILTERCYCLES)
            {
                pdata->nHighCycleCounter++;
                if (pdata->nHighCycleCounter >= KEY_RISING_FILTERCYCLES)
                {
                    // key is stable pressed
                    newval = 1;
                }
            }
        }
        else
        {
            // key is not pressed
            if (pdata->nHighCycleCounter > 0)
            {
                pdata->nHighCycleCounter--;
            }

            if (pdata->nLowCycleCounter < KEY_FALLING_FILTERCYCLES)
            {
                pdata->nLowCycleCounter++;
                if (pdata->nLowCycleCounter >= KEY_FALLING_FILTERCYCLES)
                {
                    // key is stable not pressed
                    newval = 0;
                }
            }
        }

        if (newval && !pdata->iKeyValue)
        {
            // key was pressed -> rising edge
            pdata->iKeyRisingEdge = 1;
            pdata->iKeyValue = newval;
            pdata->iKeyPressedCycleCounter = 0;
            pdata->iKeyRepeatCycleCounter = 0;
        }
        else if (pdata->iKeyValue && !newval)
        {
            // key was released -> falling edge
            pdata->iKeyFallingEdge = 1;
            pdata->iKeyValue = newval;
        }
        else
        {
            // no change
            pdata->iKeyRisingEdge = 0;
            pdata->iKeyFallingEdge = 0;

            if (pdata->iKeyValue)
            {
                // when key is pressed
                if (pdata->iKeyPressedCycleCounter < KEY_START_REPEAT_CYCLECOUNT)
                {
                    // count cycles
                    pdata->iKeyPressedCycleCounter++;
                }

                if (pdata->iKeyPressedCycleCounter >= KEY_START_REPEAT_CYCLECOUNT)
                {
                    // when key is pressed for more then KEY_START_REPEAT_CYCLECOUNT cycles
                    pdata->iKeyRepeatCycleCounter++;
                    if (pdata->iKeyRepeatCycleCounter >= KEY_REPEAT_CYCLECOUNT)
                    {
                        // signal key press every KEY_REPEAT_CYCLECOUNT cycles
                        pdata->iKeyRisingEdge = 1;
                        pdata->iKeyRepeatCycleCounter = 0;
                    }
                }
            }
        }
    }
}

/// @brief Config pin for output
/// @param iOutPin 
void SetupOutputPin(int iOutPin)
{
    if ((iOutPin > 0) && nInitialized)
    {
        //mylog(LOG_INFO, "IO: Config Pin %d as output", iOutPin);
        pinMode(iOutPin, OUTPUT);
        digitalWrite(iOutPin, LOW);
    }
}

/// @brief Write an output pin to HIGH or LOW
/// @param iOutPin 
/// @param iValue 
void WriteOutputPin(int iOutPin, int iValue)
{
    if ((iOutPin > 0) && nInitialized)
    {
        digitalWrite(iOutPin, iValue);
    }
}


/// @brief look cyclic for the keys
void IO_DoCyclic()
{
    ReadKey(&gpioKeyStop);
    ReadKey(&gpioKeyPwrUp);
    ReadKey(&gpioKeyPwrDown);

    if (gpioKeyStop.iKeyValue)
    {
        // stop key is pressed
        if (gpioKeyStop.iKeyRisingEdge)
        {
            mylog(LOG_INFO, "IO: KEY-Stop: Stop motor.");
            Can_SetMotorGear(0, 0);
        }
    }
    else if (motctrl[0].nDriveReady)    // plus and minus keys only when drive is ready
    {
        if (gpioKeyPwrUp.iKeyRisingEdge)
        {
            // plus key is pressed -> increase power
            if (motctrl[0].iMotorGear == MOTOR_GEAR_NEUTRAL)
            {
                mylog(LOG_INFO, "IO: KEY-Plus: Start motor.");
                Can_SetMotorGear(0, 1);
                Can_SetMotorPower(0, 1);
            }
            else
            {
                mylog(LOG_INFO, "IO: KEY-Plus: Increase power.");
                Can_SetMotorPower(0, motctrl[0].iMotorPowerSteps + 1);
            }
        }

        if (gpioKeyPwrDown.iKeyRisingEdge)
        {
            // minus key is pressed -> decrease power
            if (motctrl[0].iMotorPowerSteps > 1)
            {
                mylog(LOG_INFO, "IO: KEY-Minus: Decrease power.");
                Can_SetMotorPower(0, motctrl[0].iMotorPowerSteps - 1);
            }
            else
            {
                mylog(LOG_INFO, "IO: KEY-Minus: Stop motor.");
                Can_SetMotorGear(0, 0);
            }
        }
    }
}