#include <M5Stack.h>

#include "Wire.h"

#define FACE_JOY_ADDR 0x5e

void Init() {
    Wire.begin();
    for (int i = 0; i < 256; i++) {
        Wire.beginTransmission(FACE_JOY_ADDR);
        Wire.write(i % 4);
        Wire.write(random(256) * (256 - i) / 256);
        Wire.write(random(256) * (256 - i) / 256);
        Wire.write(random(256) * (256 - i) / 256);
        Wire.endTransmission();
        delay(2);
    }
    Led(0, 0, 0, 0);
    Led(1, 0, 0, 0);
    Led(2, 0, 0, 0);
    Led(3, 0, 0, 0);
}

void Led(int indexOfLED, int r, int g, int b) {
    Wire.beginTransmission(FACE_JOY_ADDR);
    Wire.write(indexOfLED);
    Wire.write(r);
    Wire.write(g);
    Wire.write(b);
    Wire.endTransmission();
}

void setup() {
    M5.begin();
    M5.Power.begin();
    M5.Lcd.clear();
    M5.Lcd.setCursor(60, 0, 4);
    M5.Lcd.printf("FACE JOYSTICK");

    Init();
}

uint8_t x_data_L;
uint8_t x_data_H;
uint16_t x_data;
uint8_t y_data_L;
uint8_t y_data_H;
uint16_t y_data;
uint8_t button_data;
char data[100];

void loop() {
    Wire.requestFrom(FACE_JOY_ADDR, 5);
    if (Wire.available()) {
        y_data_L = Wire.read();
        y_data_H = Wire.read();
        x_data_L = Wire.read();
        x_data_H = Wire.read();

        button_data = Wire.read();  // Z(0: released 1: pressed)

        x_data = x_data_H << 8 | x_data_L;
        y_data = y_data_H << 8 | y_data_L;

        sprintf(data, "x:%d y:%d button:%d\n", x_data, y_data, button_data);
        Serial.print(data);

        M5.Lcd.setCursor(45, 120);
        M5.Lcd.println(data);

        if (x_data > 600) {
            Led(2, 0, 0, 50);
            Led(0, 0, 0, 0);
        } else if (x_data < 400) {
            Led(0, 0, 0, 50);
            Led(2, 0, 0, 0);
        } else {
            Led(0, 0, 0, 0);
            Led(2, 0, 0, 0);
        }

        if (y_data > 600) {
            Led(3, 0, 0, 50);
            Led(1, 0, 0, 0);
        } else if (y_data < 400) {
            Led(1, 0, 0, 50);
            Led(3, 0, 0, 0);
        } else {
            Led(1, 0, 0, 0);
            Led(3, 0, 0, 0);
        }
    }
    delay(200);
}