#include <avr/io.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#include "io.h"
#include "buzzer.h"
#include "keypad.h"
#include "twi.h"

/////////////////////
// I/O ports setup //
/////////////////////

// Buzzer on PC2
DECLARE_IO(BUZZER, C, 2);
// Red LED pn PC0
DECLARE_IO(LED_RED, C, 0);
// Green LED on PC1
DECLARE_IO(LED_GREEN, C, 1);

// Keypad
DECLARE_IO(KPAD_COL3, D, 0);
DECLARE_IO(KPAD_ROW3, D, 1);
DECLARE_IO(KPAD_COMMON, D, 2);
DECLARE_IO(KPAD_COL2, D, 3);
DECLARE_IO(KPAD_ROW1, D, 4);
DECLARE_IO(KPAD_ROW2, D, 5);
DECLARE_IO(KPAD_ROW4, D, 6);
DECLARE_IO(KPAD_COL1, D, 7);

#define KEYPAD_BUFFER_SIZE 16
static volatile uint8_t KeypadBytes[KEYPAD_BUFFER_SIZE];
static volatile uint8_t KeypadIndex = 0;
static volatile uint8_t KeypadShouldRead = 0;

#define KEYPAD_STATE_IDLE 0
#define KEYPAD_STATE_GETTING_BYTES 1
#define KEYPAD_STATE_TIMEOUT 2
#define KEYPAD_STATE_BYTES_READY 3
static volatile uint8_t KeypadState = KEYPAD_STATE_IDLE;

#define KEYPAD_I2C_GET_STATE 0x00
#define KEYPAD_I2C_GET_BYTES_WAITING 0x01
#define KEYPAD_I2C_GET_BYTES 0x02
#define KEYPAD_I2C_REQUEST_BYTES 0x03
#define KEYPAD_I2C_SOUND_OK 0x04
#define KEYPAD_I2C_SOUND_DENIED 0x05
#define KEYPAD_I2C_SET_IDLE 0x06

static void fsm_idle(void)
{
    KeypadState = KEYPAD_STATE_IDLE;
    KeypadShouldRead = 0;
    KeypadIndex = 0;
    IO_OUT(LED_RED, 0);
    IO_OUT(LED_GREEN, 0);
}

static void fsm_get_bytes(uint8_t required)
{
    KeypadState = KEYPAD_STATE_GETTING_BYTES;
    KeypadShouldRead = required;
    KeypadIndex = 0;
}

static void fsm_bytes_ready(void)
{
    KeypadState = KEYPAD_STATE_BYTES_READY;
    KeypadShouldRead = 0;
}

int main (void)
{
    // Setup outputs
    IO_SET_OUTPUT(BUZZER);
    IO_SET_OUTPUT(LED_RED);
    IO_SET_OUTPUT(LED_GREEN);

    buzzer_init();
    keypad_init();
    TWI_Slave_Initialize((uint8_t)((0x10 << TWI_ADR_BITS) | (1 << TWI_GEN_BIT) ));
    sei();
    TWI_Start_Transceiver();

    // Flash LEDs and buzz buzzer for debug
    IO_OUT(LED_GREEN, 1);
    IO_OUT(LED_RED, 1);
    buzzer_signal_boot();
    IO_OUT(LED_GREEN, 0);
    IO_OUT(LED_RED, 0);

    // Zero out the keypad buffer
    for (uint8_t i = 0; i < KEYPAD_BUFFER_SIZE; i++)
        KeypadBytes[i] = 0;

    KeypadShouldRead = 0;
    KeypadIndex = 0;
    KeypadState = KEYPAD_STATE_IDLE;


    uint8_t Command[2];
    uint8_t OutBuffer[16];

    for (;;)
    {
        if (!TWI_Transceiver_Busy())
        {
            if (TWI_statusReg.RxDataInBuf)
            {
                TWI_Get_Data_From_Transceiver(Command, 2);
                switch(Command[0])
                {
                    case KEYPAD_I2C_GET_STATE:
                        OutBuffer[0] = KeypadState;
                        TWI_Start_Transceiver_With_Data(OutBuffer, 1);
                        break;
                    case KEYPAD_I2C_GET_BYTES_WAITING:
                        OutBuffer[0] = KeypadIndex;
                        TWI_Start_Transceiver_With_Data(OutBuffer, 1);
                        break;
                    case KEYPAD_I2C_GET_BYTES:
                        for (uint8_t i = 0; i < KeypadIndex; i++)
                            OutBuffer[i] = KeypadBytes[i];
                        TWI_Start_Transceiver_With_Data(OutBuffer, KeypadIndex);
                        fsm_idle();
                        break;
                    case KEYPAD_I2C_REQUEST_BYTES:
                        fsm_get_bytes(Command[1]);
                        TWI_Start_Transceiver();
                        buzzer_start(TONE_HIGH);
                        _delay_ms(100);
                        buzzer_stop();
                        fsm_logic();
                        break;
                    case KEYPAD_I2C_SOUND_OK:
                        TWI_Start_Transceiver();
                        IO_OUT(LED_GREEN, 1);
                        buzzer_start(TONE_HIGH);
                        _delay_ms(100);
                        buzzer_stop();
                        _delay_ms(50);
                        buzzer_start(TONE_HIGH);
                        _delay_ms(100);
                        buzzer_stop();
                        _delay_ms(50);
                        buzzer_start(TONE_HIGH);
                        _delay_ms(100);
                        buzzer_stop();
                        _delay_ms(50);
                        buzzer_start(TONE_HIGH);
                        _delay_ms(100);
                        buzzer_stop();
                        break;
                     case KEYPAD_I2C_SOUND_DENIED:
                        TWI_Start_Transceiver();
                        IO_OUT(LED_RED, 1);
                        buzzer_start(TONE_HIGH);
                        _delay_ms(100);
                        buzzer_stop();
                        _delay_ms(50);
                        buzzer_start(TONE_HIGH);
                        _delay_ms(100);
                        buzzer_stop();
                        _delay_ms(50);
                        buzzer_start(TONE_LOW);
                        _delay_ms(1000);
                        buzzer_stop();
                        break;
                    case KEYPAD_I2C_SET_IDLE:
                        TWI_Start_Transceiver();
                        fsm_idle();
                    default:
                        TWI_Start_Transceiver();
                }
            }
            else
                fsm_logic();
        }
    }
}

static volatile uint16_t ButtonsPrevious = 0;
void fsm_logic(void)
{
        switch(KeypadState)
        {
            case KEYPAD_STATE_GETTING_BYTES:
            {
                uint16_t Buttons = keypad_get_buttons();
                if (KeypadShouldRead > 0 && Buttons != ButtonsPrevious && Buttons != 0)
                {
                    // Debounce...
                    _delay_ms(20);
                    Buttons = keypad_get_buttons();
                    if (Buttons != 0)
                    {
                        // Make sure that only one button is pressed down == one bit is present
                        uint16_t Buttons_ = Buttons;
                        while ((Buttons_ & 1) != 1)
                            Buttons_ = Buttons_ >> 1;

                        Buttons_ = Buttons_ >> 1;
                        if (Buttons_ == 0)
                        {
                            // Okay, only one button is pressed down
                            uint8_t Byte = 0;
                            if (Buttons & (1 << KEY_1))
                                Byte = '1';
                            else if (Buttons & (1 << KEY_2))
                                Byte = '2';
                            else if (Buttons & (1 << KEY_3))
                                Byte = '3';
                            else if (Buttons & (1 << KEY_4))
                                Byte = '4';
                            else if (Buttons & (1 << KEY_5))
                                Byte = '5';
                            else if (Buttons & (1 << KEY_6))
                                Byte = '6';
                            else if (Buttons & (1 << KEY_7))
                                Byte = '7';
                            else if (Buttons & (1 << KEY_8))
                                Byte = '8';
                            else if (Buttons & (1 << KEY_9))
                                Byte = '9';
                            else if (Buttons & (1 << KEY_0))
                                Byte = '0';

                            if (Byte != 0)
                            {
                                KeypadBytes[KeypadIndex] = Byte;
                                KeypadIndex++;
                                if (KeypadShouldRead == 1)
                                    fsm_bytes_ready();
                                KeypadShouldRead--;

                                buzzer_start(TONE_HIGH);
                                _delay_ms(100);
                                buzzer_stop();
                            }
                        }
                    }
                }
                ButtonsPrevious = Buttons;
            }
        }
}