#include "demohs.h"

#include <batt.h>
#include <pio.h>



static void toggleLED(uint8 led, uint16 led_state)
{
    if (led_state == led_on)
        setLEDstate(led, led_off);
    else
        setLEDstate(led, led_on);
}


static Delay flashCallback(TimerHandle h)
{
    h=h;

    if (hsState.connectState == pairing)
    {
        /* In pairing mode so ensure the connect LED is off */
        toggleLED(PIO_LED_PAIR, hsState.pairLEDstate);
        setLEDstate(PIO_LED_CONNECT, led_off);

        if (hsState.pairLEDstate)
            return LED_PAIR_ON;
        else
            return LED_PAIR_OFF;
    }
    else 
    {
        /* 
            For now pair LED only used when pairing so ensure its off when
            in any of the connect modes
        */        
        setLEDstate(PIO_LED_PAIR, led_off);

        if (hsState.connectState == connectingAsSlave)
        {
            /* toggle the state of the flashing LED */
            toggleLED(PIO_LED_CONNECT, hsState.connectLEDstate);

            /* Return the next timeout interval */
            if (hsState.connectLEDstate)
                return LED_CONNECT_SLAVE_ON;
            else
                return LED_CONNECT_SLAVE_OFF;
        }
        else if (hsState.connectState == connectingAsMaster)
        {
            /* toggle the state of the flashing LED */
            toggleLED(PIO_LED_CONNECT, hsState.connectLEDstate);

            /* Return the next timeout interval */
            if (hsState.connectLEDstate)
                return LED_CONNECT_MASTER_ON;
            else
                return LED_CONNECT_MASTER_OFF;
        }
        else if (hsState.connectState == connected)
        {   
            /* toggle the state of the flashing LED */
            toggleLED(PIO_LED_CONNECT, hsState.connectLEDstate);

            /* Return the next timeout interval */
            if (hsState.connectLEDstate)
                return LED_CONNECTED_ON;
            else
                return LED_CONNECTED_OFF;        
        }    
        else
        {
            /* in idle or some other unknown state so try again later */
            setLEDstate(PIO_LED_CONNECT, led_off);

            /* Return the next timeout interval */
            return D_SEC(100);
        }
    }
}


/*
    flashInit

    Init the LEDs and start off the flash sequence
*/
void flashInit(void)
{
    /* Set the two PIOs to be outputs */
    PioSetDir((PIO_LED_PAIR | PIO_LED_CONNECT), ~0);

    /* to start with turn off the LEDs */
    setLEDstate((PIO_LED_PAIR | PIO_LED_CONNECT), led_off);

    /* kick off the flashing LEDs routine */
    TimerAdd(D_mSEC(500), flashCallback);
}


void setLEDstate(uint8 led_id, uint8 state)
{    
    /* make sure the led_id doesn't try to drive any non-output PIOs */
    led_id &= (PIO_LED_PAIR |PIO_LED_CONNECT);    
    
    /* store the current LED state */
    if ((PIO_LED_CONNECT & led_id) == PIO_LED_CONNECT)
        hsState.connectLEDstate = state;

    if ((PIO_LED_PAIR & led_id) == PIO_LED_PAIR)
        hsState.pairLEDstate = state;
    
    /* Implementation dependent. In this case driving the PIO to zero lights it */
    if (state == led_off)
    {   
        PioSet(led_id, 0);
    }
    else 
    {        
        PioSet(led_id, ~0);
    }
}


/*
    changeFlash

    Change the flash pattern since the headset state has changed
*/
void changeFlash(void)
{
    /* Cancel the current flashing */
    TimerCancelCallback(flashCallback);
 
    if (hsState.connectState != idle && !hsState.turnOffHs)
    {
        TimerAdd(D_IMMEDIATE, flashCallback);
    }
    else
    {
        setLEDstate(PIO_LED_CONNECT, led_off);
        setLEDstate(PIO_LED_PAIR, led_off);
    }
}