HansonServo/animation.cpp

#include "animation.h"

Animation::Animation() {
  clear();
  memcpy(header.magic, "ANIM", 4);
  header.version = 1;
  header.frameRate = FRAMES_PER_SECOND;
  header.frameCount = MAX_FRAMES;
  memset(header.reserved, 0, sizeof(header.reserved));
}

void Animation::setActive(bool state) {
  active = state;
}


void Animation::addCurveSegment(const CurveSegment& segment) {
  curves[segment.motorID].push_back(segment);
}


void Animation::clearCurves(uint8_t motorID) {
  curves.erase(motorID);  // Completely remove the entry
}

void Animation::clearAllCurves() {
  curves.clear();  // Wipe the entire map
}


uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {

  for (const auto& seg : curves[motorID]) {
    if (timeCS >= seg.startTime && timeCS <= seg.endTime) {
      // Convert uint16_t to float in range -1 to 1

      // Define control points
      float x0 = seg.startTime;
      float x1 = seg.startHandleX;
      float x2 = seg.endHandleX;
      float x3 = seg.endTime;

      float y0 = seg.startPointY;
      float y1 = seg.startHandleY;
      float y2 = seg.endHandleY;
      float y3 = seg.endPointY;


      // Solve for t such that Bézier x(t) ≈ timeCS
      auto bezierX = [&](float t) {
        float u = 1.0f - t;
        return u * u * u * x0 + 3 * u * u * t * x1 + 3 * u * t * t * x2 + t * t * t * x3;
      };

      float t = 0.5f;
      float lower = 0.0f;
      float upper = 1.0f;

      for (int i = 0; i < 20; ++i) {
        float x = bezierX(t);
        if (fabs(x - timeCS) < 0.5f) break;
        if (x < timeCS) lower = t;
        else upper = t;
        t = (lower + upper) * 0.5f;
      }

      // Evaluate Bézier y(t)
      float u = 1.0f - t;
      float value = u * u * u * y0 + 3 * u * u * t * y1 + 3 * u * t * t * y2 + t * t * t * y3;

      // Remap to PWM range
      return constrain(value, 0, 4095);

    }
  }

  return 2048;  // Default center
}




void Animation::clear() {
  //memset(data, 0, sizeof(data));
}

// uint16_t* Animation::getRawData() {
//   return &data[0][0];
// }

// size_t Animation::getSize() const {
//   return sizeof(data);
// }

void Animation::setFrameCount(uint16_t count) {
  header.frameCount = count;
}

uint16_t Animation::getFrameCount() const {
  return header.frameCount;
}

bool Animation::saveToFile(const char* filename) {
  File file = FFat.open(filename, FILE_WRITE);
  if (!file) return false;

  // Write header
  file.write((uint8_t*)&header, sizeof(header));

  // Count total curve segments
  uint16_t curveCount = 0;
  for (const auto& [motorID, segments] : curves) {
    curveCount += segments.size();
  }

  // Write curve count
  file.write((uint8_t*)&curveCount, sizeof(curveCount));

  // Write all curve segments
  for (const auto& [motorID, segments] : curves) {
    for (const CurveSegment& seg : segments) {
      file.write((uint8_t*)&seg, sizeof(CurveSegment));
    }
  }

  file.close();
  return true;
}

bool Animation::loadFromFile(const char* filename) {
  File file = FFat.open(filename, FILE_READ);
  if (!file) return false;

  // Read and validate header
  AnimationHeader tempHeader;
  if (file.read((uint8_t*)&tempHeader, sizeof(tempHeader)) != sizeof(tempHeader)) {
    file.close();
    return false;
  }

  if (strncmp(tempHeader.magic, "ANIM", 4) != 0 || tempHeader.version != 1) {
    file.close();
    return false;
  }

  header = tempHeader;

  // Read curve count
  uint16_t curveCount;
  if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
    file.close();
    return false;
  }

  // Clear existing curves
  clearAllCurves();

  // Read curve segments
  for (uint16_t i = 0; i < curveCount; i++) {
    CurveSegment seg;
    if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) {
      file.close();
      return false;
    }

    // Store segment directly — no motorID filtering
    curves[seg.motorID].push_back(seg);
  }

  file.close();
  return true;
}


String Animation::printCurves() {
  String output = "PRINTING CURVES\n";

  for (const auto& [motorID, segments] : curves) {
    output += "Motor ";
    output += String(motorID);
    output += ": ";
    output += String(segments.size());
    output += "\n";

    for (const auto& seg : segments) {
      output += "  Segment: ";
      output += "startTime=" + String(seg.startTime);
      output += ", endTime=" + String(seg.endTime);
      output += ", startPointY=" + String(seg.startPointY);
      output += ", startHandleX=" + String(seg.startHandleX);
      output += ", startHandleY=" + String(seg.startHandleY);
      output += ", endHandleX=" + String(seg.endHandleX);
      output += ", endHandleY=" + String(seg.endHandleY);
      output += ", endPointY=" + String(seg.endPointY);
      output += "\n";
    }
  }

  return output;
}






void Animation::createBasicSCurve() {
  clearAllCurves();

  // Helper to convert float [-1, 1] to uint16_t [0, 65535]
  auto toUint16 = [](float v) -> uint16_t {
    return constrain((v + 1.0f) * 32767.5f, 0, 65535);
  };

  setFrameCount(800);  // 8.00 seconds

  // First segment: -1 at 0s → +1 at 0.40s
  CurveSegment seg1;
  seg1.motorID = 0;
  seg1.startTime = 0;  // 0.00s
  seg1.endTime = 40;   // 0.40s = 40 centiseconds

  seg1.startPointY = toUint16(-1.0f);  // P0.y
  seg1.startHandleX = 10;              // P1.x (early pull)
  seg1.startHandleY = toUint16(-1.0f);

  seg1.endHandleX = 30;  // P2.x (late pull)
  seg1.endHandleY = toUint16(+1.0f);

  seg1.endPointY = toUint16(+1.0f);  // P3.y

  addCurveSegment(seg1);

  // Second segment: +1 at 0.40s → -1 at 8.00s
  CurveSegment seg2;
  seg2.motorID = 0;
  seg2.startTime = 40;  // 0.40s
  seg2.endTime = 800;   // 8.00s

  seg2.startPointY = toUint16(+1.0f);  // P0.y
  seg2.startHandleX = 200;             // P1.x (early pull)
  seg2.startHandleY = toUint16(+1.0f);

  seg2.endHandleX = 600;  // P2.x (late pull)
  seg2.endHandleY = toUint16(-1.0f);

  seg2.endPointY = toUint16(-1.0f);  // P3.y

  addCurveSegment(seg2);
}


void Animation::createEaseOutCurve() {
  clearAllCurves();

  // Helper to convert float [-1, 1] to uint16_t [0, 65535]
  auto toUint16 = [](float v) -> uint16_t {
    return constrain((v + 1.0f) * 32767.5f, 0, 65535);
  };

  setFrameCount(400);  // 8.00 seconds total

  // Segment 1: Ease out from -1 to +1 over 4 seconds
  CurveSegment seg;
  seg.motorID = 0;
  seg.startTime = 0;  // 0.00s
  seg.endTime = 200;  // 4.00s

  seg.startPointY = toUint16(-1.0f);  // P0.y
  seg.startHandleX = 50;              // P1.x (early pull → slow start)
  seg.startHandleY = toUint16(-1.0f);

  seg.endHandleX = 180;  // P2.x (late pull → fast finish)
  seg.endHandleY = toUint16(+0.5f);

  seg.endPointY = toUint16(+1.0f);  // P3.y

  addCurveSegment(seg);

  // Segment 2: Ease out from +1 to -1 over 4 seconds
  CurveSegment returnSeg;
  returnSeg.motorID = 0;
  returnSeg.startTime = 200;  // 4.00s
  returnSeg.endTime = 400;    // 8.00s

  returnSeg.startPointY = toUint16(+1.0f);  // P0.y
  returnSeg.startHandleX = 250;             // P1.x (early pull → slow start)
  returnSeg.startHandleY = toUint16(+1.0f);

  returnSeg.endHandleX = 380;  // P2.x (late pull → fast finish)
  returnSeg.endHandleY = toUint16(-0.5f);

  returnSeg.endPointY = toUint16(-1.0f);  // P3.y

  addCurveSegment(returnSeg);
}