// OLED Eyes — Gripper Expression System
#include <Adafruit_SSD1306.h>
#define SCREEN_W 128
#define SCREEN_H 64
Adafruit_SSD1306 display(SCREEN_W, SCREEN_H, &Wire);

enum EyeState { IDLE, CONNECTED, GRIPPING, BLINK };

void drawEyes(EyeState state) {
  display.clearDisplay();
  int eyeH = (state == GRIPPING) ? 14 :
              (state == BLINK)    ? 4  :
              (state == IDLE)     ? 22 : 30;
  display.fillRoundRect(20, (64-eyeH)/2, 36, eyeH, eyeH/2, WHITE);
  display.fillRoundRect(72, (64-eyeH)/2, 36, eyeH, eyeH/2, WHITE);
  if (state != BLINK) {
    display.fillCircle(38, 32, 7, BLACK);
    display.fillCircle(90, 32, 7, BLACK);
  }
  display.display();
}

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  drawEyes(IDLE);
}

void loop() {
  // drawEyes(CONNECTED) when BLE connects
  // drawEyes(GRIPPING) when angles[] > 60
  // drawEyes(BLINK) every 4 seconds
}

// BLE Transmitter — Wearable Glove
// XIAO ESP32-C3 | Fab Academy W15
#include <BLEDevice.h>
#define SERVICE_UUID "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHAR_UUID    "beb5483e-36e1-4688-b7f5-ea07361b26a8"
#define FLEX1 A0
#define FLEX2 A1
#define FLEX3 A2
BLECharacteristic *pChar;

void setup() {
  BLEDevice::init("OctopusGlove");
  BLEServer *srv = BLEDevice::createServer();
  BLEService *svc = srv->createService(SERVICE_UUID);
  pChar = svc->createCharacteristic(
    CHAR_UUID, BLECharacteristic::PROPERTY_NOTIFY);
  svc->start(); BLEDevice::startAdvertising();
}

void loop() {
  int a1 = map(analogRead(FLEX1), 0, 4095, 0, 120);
  int a2 = map(analogRead(FLEX2), 0, 4095, 0, 120);
  int a3 = map(analogRead(FLEX3), 0, 4095, 0, 120);
  String pkt = String(a1)+","+String(a2)+","+String(a3);
  pChar->setValue(pkt.c_str()); pChar->notify();
  delay(50);
}

// BLE Receiver — Gripper Controller
// XIAO nRF52840 | Fab Academy W15
#include <BLEDevice.h>
#include <ESP32Servo.h>
Servo s1, s2, s3;
int angles[3] = {0,0,0};

class CB: public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *p) {
    String v = p->getValue().c_str();
    int i1=v.indexOf(','), i2=v.indexOf(',',i1+1);
    angles[0]=v.substring(0,i1).toInt();
    angles[1]=v.substring(i1+1,i2).toInt();
    angles[2]=v.substring(i2+1).toInt();
  }
};

void setup() {
  s1.attach(3); s2.attach(4); s3.attach(5);
  BLEDevice::init("");
}

void loop() {
  s1.write(angles[0]);
  s2.write(angles[1]);
  s3.write(angles[2]);
  delay(20);
}