#include <Wire.h>
#include <paj7620.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define GES_REACTION_TIME   500
#define GES_ENTRY_TIME      800
#define GES_QUIT_TIME       1000
#define SDA_PIN 21
#define SCL_PIN 22
#define OLED_RESET 4

Adafruit_SSD1306 display(OLED_RESET);

void setup() {
    Wire.begin(SDA_PIN, SCL_PIN);
    Serial.begin(9600);
    uint8_t error = paj7620Init();
    if (error) {
        Serial.print("INIT ERROR, CODE: ");
        Serial.println(error);
    } else {
        Serial.println("INIT OK");
    }
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
    display.display();
    delay(2000);
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0,0);
    display.println("Gesture Recognition");
    display.display();
    delay(2000);
    display.clearDisplay();
}

void loop() {
    uint8_t data = 0, data1 = 0, error;
    error = paj7620ReadReg(0x43, 1, &data);
    if (!error) {
        switch (data) {
            case GES_RIGHT_FLAG:
                delay(GES_ENTRY_TIME);
                paj7620ReadReg(0x43, 1, &data);
                if (data == GES_FORWARD_FLAG) {
                    displayGesture("Forward");
                    delay(GES_QUIT_TIME);
                } else if (data == GES_BACKWARD_FLAG) {
                    displayGesture("Backward");
                    delay(GES_QUIT_TIME);
                } else {
                    displayGesture("Right");
                }
                break;
            case GES_LEFT_FLAG:
                delay(GES_ENTRY_TIME);
                paj7620ReadReg(0x43, 1, &data);
                if (data == GES_FORWARD_FLAG) {
                    displayGesture("Forward");
                    delay(GES_QUIT_TIME);
                } else if (data == GES_BACKWARD_FLAG) {
                    displayGesture("Backward");
                    delay(GES_QUIT_TIME);
                } else {
                    displayGesture("Left");
                }
                break;
            case GES_UP_FLAG:
                delay(GES_ENTRY_TIME);
                paj7620ReadReg(0x43, 1, &data);
                if (data == GES_FORWARD_FLAG) {
                    displayGesture("Forward");
                    delay(GES_QUIT_TIME);
                } else if (data == GES_BACKWARD_FLAG) {
                    displayGesture("Backward");
                    delay(GES_QUIT_TIME);
                } else {
                    displayGesture("Up");
                }
                break;
            case GES_DOWN_FLAG:
                delay(GES_ENTRY_TIME);
                paj7620ReadReg(0x43, 1, &data);
                if (data == GES_FORWARD_FLAG) {
                    displayGesture("Forward");
                    delay(GES_QUIT_TIME);
                } else if (data == GES_BACKWARD_FLAG) {
                    displayGesture("Backward");
                    delay(GES_QUIT_TIME);
                } else {
                    displayGesture("Down");
                }
                break;
            case GES_FORWARD_FLAG:
                displayGesture("Forward");
                delay(GES_QUIT_TIME);
                break;
            case GES_BACKWARD_FLAG:
                displayGesture("Backward");
                delay(GES_QUIT_TIME);
                break;
            case GES_CLOCKWISE_FLAG:
                displayGesture("Clockwise");
                break;
            case GES_COUNT_CLOCKWISE_FLAG:
                displayGesture("Anti-clockwise");
                break;
            default:
                paj7620ReadReg(0x44, 1, &data1);
                if (data1 == GES_WAVE_FLAG) {
                    displayGesture("Wave");
                }
                break;
        }
    }
    delay(100);
}

void displayGesture(String gesture) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0,0);
    display.println("Detected Gesture:");
    display.setTextSize(2);
    display.setCursor(0,16);
    display.println(gesture);
    display.display();
    delay(2000);
    display.clearDisplay();
}