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::addCurveSegment(const CurveSegment& segment) {
  if (segment.motorID < NUM_CHANNELS) {
    curves[segment.motorID].push_back(segment);
  }
}

void Animation::clearCurves(uint8_t motorID) {
  if (motorID < NUM_CHANNELS) {
    curves[motorID].clear();
  }
}

void Animation::clearAllCurves() {
  for (int i = 0; i < NUM_CHANNELS; ++i) {
    curves[i].clear();
  }
}

uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
  if (motorID >= NUM_CHANNELS) return 0;

  for (const auto& seg : curves[motorID]) {
    if (timeCS >= seg.startTime && timeCS <= seg.endTime) {
      // Convert uint16_t to float in range -1 to 1
      auto toFloat = [](uint16_t v) {
        return (float(v) / 65535.0f) * 2.0f - 1.0f;
      };

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

      float y0 = toFloat(seg.startPointY);
      float y1 = toFloat(seg.startHandleY);
      float y2 = toFloat(seg.endHandleY);
      float y3 = toFloat(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 + 1.0f) * 2047.5f, 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) {
  // Auto-detect actual frame count
  uint16_t lastFrame = 0;
  // for (uint16_t frame = 0; frame < MAX_FRAMES; frame++) {
  //   for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
  //     if (data[frame][ch] != 0) {
  //       lastFrame = frame;
  //       break;
  //     }
  //   }
  // }
  //header.frameCount = lastFrame + 1;

  File file = FFat.open(filename, FILE_WRITE);
  if (!file) return false;

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

  // Count total curve segments
  uint16_t curveCount = 0;
  for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
    curveCount += curves[ch].size();
  }

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

  // Write all curve segments
  for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
    for (const CurveSegment& seg : curves[ch]) {
      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 motion data
  // size_t expectedSize = sizeof(data);
  // if (file.read((uint8_t*)data, expectedSize) != expectedSize) {
  //   file.close();
  //   return false;
  // }

  // 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;
    }

    if (seg.motorID < NUM_CHANNELS) {
      curves[seg.motorID].push_back(seg);
    }
  }

  file.close();
  return true;
}


void Animation::printCurves() {
  Serial.println("=== Curve Segments ===");

  auto toFloat = [](uint16_t v) -> float {
    return (float(v) / 65535.0f) * 2.0f - 1.0f;
  };

  for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) {
    if (curves[motorID].empty()) continue;

    Serial.print("Motor ");
    Serial.print(motorID);
    Serial.println(":");

    for (size_t i = 0; i < curves[motorID].size(); ++i) {
      const CurveSegment& seg = curves[motorID][i];

      Serial.print("  Segment ");
      Serial.print(i);
      Serial.print(" | Time: ");
      Serial.print(seg.startTime * 0.01f, 2);
      Serial.print("s → ");
      Serial.print(seg.endTime * 0.01f, 2);
      Serial.println("s");

      Serial.print("    Start Point Y: ");
      Serial.println(toFloat(seg.startPointY), 3);

      Serial.print("    Start Handle: (");
      Serial.print(seg.startHandleX * 0.01f, 2);
      Serial.print("s, ");
      Serial.print(toFloat(seg.startHandleY), 3);
      Serial.println(")");

      Serial.print("    End Handle:   (");
      Serial.print(seg.endHandleX * 0.01f, 2);
      Serial.print("s, ");
      Serial.print(toFloat(seg.endHandleY), 3);
      Serial.println(")");

      Serial.print("    End Point Y: ");
      Serial.println(toFloat(seg.endPointY), 3);
    }
  }

  Serial.println("======================");
}



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);
}