#include "unit_rolleri2c.hpp"
#include <M5Unified.h>


/*
Going to random position every 1 second - trying to move with ease in/out, but very artificial/jittery - not smooth.
*/


UnitRollerI2C RollerI2C;

const int32_t UNITS_PER_DEGREE   = 100;
const int32_t POSITION_TOLERANCE = 150;   // ~1.5° arrival threshold
const uint32_t MOVE_DURATION_MS  = 3000;  // total time per move (ms) — tune 1000–5000
const uint32_t STEP_MS           = 40;    // trajectory update interval (ms)
const uint32_t PAUSE_MS          = 1000;  // pause at target before next move

int32_t basePosition = 0;

// ── Ease-in / ease-out using a sine curve ──────────────────────────────────
// t=0.0 → 0.0  |  t=0.5 → 0.5  |  t=1.0 → 1.0
// Accelerates from rest, decelerates to rest — smooth S-curve.
float easeInOut(float t) {
    return (1.0f - cosf(t * PI)) / 2.0f;
}

// Drives from startUnits → targetUnits over MOVE_DURATION_MS with ease in/out.
// Returns true when the motor confirms arrival within tolerance.
bool easedMoveTo(int32_t startUnits, int32_t targetUnits) {
    uint32_t moveStart = millis();

    while (true) {
        uint32_t elapsed = millis() - moveStart;
        float t = min((float)elapsed / (float)MOVE_DURATION_MS, 1.0f);

        // Compute eased intermediate setpoint
        float   eased        = easeInOut(t);
        int32_t intermediate = startUnits + (int32_t)((targetUnits - startUnits) * eased);
        RollerI2C.setPos(intermediate);

        // Once we've sent the final setpoint, check arrival
        if (t >= 1.0f) {
            int32_t actual = RollerI2C.getPosReadback();
            if (abs(targetUnits - actual) <= POSITION_TOLERANCE) {
                return true;  // ✅ arrived
            }
            // Give a small extra window for the PID to settle
            if (elapsed > MOVE_DURATION_MS + 1000) {
                return false; // ⏱️ timed out even after easing finished
            }
        }

        delay(STEP_MS);
    }
}

int32_t degreesToUnits(float deg) { return (int32_t)(deg * UNITS_PER_DEGREE); }
float   unitsToDegrees(int32_t u)  { return u / (float)UNITS_PER_DEGREE; }

void setup() {
    M5.begin();
    Serial.begin(115200);
    delay(500);
    randomSeed(analogRead(0));

    bool found = RollerI2C.begin(&Wire, 0x64, 2, 1, 400000);
    if (!found) {
        Serial.println("❌ Roller485 NOT found on I2C! Check wiring.");
        while (true) delay(1000);
    }
    Serial.println("✅ Roller485 found!");
    Serial.print("Vin (x0.01V): "); Serial.println(RollerI2C.getVin());
    Serial.print("ErrorCode: ");    Serial.println(RollerI2C.getErrorCode());

    RollerI2C.setOutput(0);
    RollerI2C.setMode(ROLLER_MODE_POSITION);
    RollerI2C.setPosMaxCurrent(80000);
    RollerI2C.setOutput(1);
    delay(100);

    basePosition = RollerI2C.getPosReadback();
    Serial.print("Base position (raw): "); Serial.println(basePosition);
    Serial.println("Ready. Starting eased random position loop...\n");
    delay(500);
}

int32_t currentTarget = 0; // tracks last target so we ease FROM it

void loop() {
    // Random angle offset from base: -180° to +180°
    float   targetDegrees = random(-180, 181);
    int32_t targetUnits   = basePosition + degreesToUnits(targetDegrees);
    int32_t fromUnits     = RollerI2C.getPosReadback(); // start from actual current pos

    Serial.print("➡️  ");
    Serial.print(unitsToDegrees(fromUnits - basePosition), 1);
    Serial.print("°  →  ");
    Serial.print(targetDegrees, 1);
    Serial.println("°  (easing...)");

    bool arrived = easedMoveTo(fromUnits, targetUnits);

    float actualDeg = unitsToDegrees(RollerI2C.getPosReadback() - basePosition);

    if (arrived) {
        Serial.print("✅ Arrived at ");
    } else {
        Serial.print("⏱️  Stopped at ");
    }
    Serial.print(actualDeg, 1);
    Serial.println("°");
    Serial.print("   ErrorCode: "); Serial.println(RollerI2C.getErrorCode());
    Serial.println();

    delay(PAUSE_MS); // pause 1 second at target
}