/**************************************************************************
 * Meeting Indicator - Desk Node (MASTER)
 * ESP32-C6 + MPU6050 + WS2812 Ring
 * Uses ESP-NOW
 **************************************************************************/

#include <WiFi.h>
#include <esp_now.h>
#include <Wire.h>

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_NeoPixel.h>

/************ Pins ************/
#define SDA_PIN     6
#define SCL_PIN     7

#define LED_PIN     2
#define LED_COUNT   8

/************ Status ************/
enum Status
{
   IDLE = 0,
    AVAILABLE ,
    BUSY,
    BRB,
    MEETING
};

struct StatusPacket
{
    uint8_t status;
};

/************ Objects ************/
Adafruit_MPU6050 mpu;
Adafruit_NeoPixel ring(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

/************ Variables ************/
Status currentStatus = AVAILABLE;
Status previousStatus = AVAILABLE;
Status candidateStatus = AVAILABLE;

unsigned long stableStart = 0;

StatusPacket packet;

/************ Door Node MAC ************/
// Replace later
uint8_t receiverMAC[] =
{
    0xB4,0x3A,0x45,
    0x89,0x73,0x50
};

/********************************************************************/

void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
    Serial.print("Send Status: ");

    if (status == ESP_NOW_SEND_SUCCESS)
        Serial.println("Success");
    else
        Serial.println("Failed");
}

/********************************************************************/

void showStatus(Status s)
{
    uint32_t color;

    switch(s)
    {
      case IDLE:
            color = ring.Color(0, 0, 0);
            break;
      
        case AVAILABLE:
            color = ring.Color(0,255,0);
            break;

        case BUSY:
            color = ring.Color(255,0,0);
            break;

        case BRB:
            color = ring.Color(255,180,0);
            break;

        case MEETING:
            color = ring.Color(0,0,255);
            break;

        default:
            color = ring.Color(255,255,255);
            break;
    }

    for(int i=0;i<LED_COUNT;i++)
        ring.setPixelColor(i,color);

    ring.show();
}

/********************************************************************/

void sendStatus(Status s)
{
    packet.status = s;

    esp_err_t result = esp_now_send(
        receiverMAC,
        (uint8_t *)&packet,
        sizeof(packet)
    );

    if(result != ESP_OK)
    {
        Serial.print("Send Error : ");
        Serial.println(result);
    }
}

/********************************************************************/

Status detectFace()
{
    sensors_event_t accel, gyro, temp;

    mpu.getEvent(&accel,&gyro,&temp);

    float ax = accel.acceleration.x / 9.81;
    float ay = accel.acceleration.y / 9.81;
    float az = accel.acceleration.z / 9.81;

    Serial.print("X:");
    Serial.print(ax,2);

    Serial.print("  Y:");
    Serial.print(ay,2);

    Serial.print("  Z:");
    Serial.println(az,2);

    if (fabs(az) > 0.8)
            return IDLE;

    if(ay < -0.80)
        return AVAILABLE;

    if(ax < -0.80)
        return BUSY;

    if(ax > 0.80)
        return BRB;

    if(ay > 0.80)
        return MEETING;

    return currentStatus;
}

/********************************************************************/

void initMPU()
{
    if(!mpu.begin())
    {
        Serial.println("MPU6050 NOT FOUND");

        while(1)
            delay(100);
    }

    mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
    mpu.setGyroRange(MPU6050_RANGE_250_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

    Serial.println("MPU6050 Initialized");
}

/********************************************************************/

void initESPNow()
{
    WiFi.mode(WIFI_STA);

    if(esp_now_init()!=ESP_OK)
    {
        Serial.println("ESP-NOW Init Failed");

        while(1)
            delay(100);
    }

    esp_now_register_send_cb(OnDataSent);

    esp_now_peer_info_t peerInfo = {};

    memcpy(peerInfo.peer_addr,receiverMAC,6);

    peerInfo.channel = 0;
    peerInfo.encrypt = false;

    if(esp_now_add_peer(&peerInfo)!=ESP_OK)
    {
        Serial.println("Failed to Add Peer");

        while(1)
            delay(100);
    }

    Serial.println("ESP-NOW Ready");
}

/********************************************************************/

void setup()
{
    Serial.begin(115200);

    delay(1000);

    Serial.println();
    Serial.println("Meeting Indicator Desk Node");

    Wire.begin(SDA_PIN,SCL_PIN);

    ring.begin();
    ring.clear();
    ring.setBrightness(200);
    ring.show();

    initMPU();

    initESPNow();

    currentStatus = AVAILABLE;
    previousStatus = AVAILABLE;
    candidateStatus = AVAILABLE;

    showStatus(currentStatus);

    Serial.println("System Ready");
}

/**************************************************************************
 * Helper Functions
 **************************************************************************/

// White flash to indicate status change
void flashConfirm()
{
    for (int i = 0; i < LED_COUNT; i++)
        ring.setPixelColor(i, ring.Color(255, 255, 255));

    ring.show();
    delay(120);

    showStatus(currentStatus);
}

/**************************************************************************
 * Main Loop
 **************************************************************************/

void loop()
{
    // Read current cube orientation
    Status detected = detectFace();

    // New orientation detected?
    if (detected != candidateStatus)
    {
        candidateStatus = detected;
        stableStart = millis();
    }

    // Has it remained stable long enough?
    if ((millis() - stableStart) > 500)
    {
        if (candidateStatus != currentStatus)
        {
            previousStatus = currentStatus;
            currentStatus = candidateStatus;

            Serial.println();
            Serial.print("Status Changed -> ");

            switch(currentStatus)
            {
                case AVAILABLE:
                    Serial.println("AVAILABLE");
                    break;

                case BUSY:
                    Serial.println("BUSY");
                    break;

                case BRB:
                    Serial.println("BE RIGHT BACK");
                    break;

                case MEETING:
                    Serial.println("MEETING");
                    break;
            }

            // Update local LEDs
            showStatus(currentStatus);

            // Confirmation flash
            flashConfirm();

            // Send to Door Node
            sendStatus(currentStatus);
        }
    }

    delay(50);
}
/********************************************************************/
// loop() will be added in Part 2
/********************************************************************/