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) {
      float t = float(timeCS - seg.startTime) / (seg.endTime - seg.startTime);

      // Convert uint16_t to float in range -1 to 1
      auto toFloat = [](uint16_t v) {
        return (float(v) / 65535.0f) * 2.0f - 1.0f;
      };

      float p0 = toFloat(seg.startPoint);
      float p1 = toFloat(seg.startHandle);
      float p2 = toFloat(seg.endHandle);
      float p3 = toFloat(seg.endPoint);

      float u = 1.0f - t;
      float value = u*u*u*p0 + 3*u*u*t*p1 + 3*u*t*t*p2 + t*t*t*p3;

      // Remap back to 0–4095 for PWM
      return constrain((value + 1.0f) * 2047.5f, 0, 4095);
    }
  }

  return 2048; // Default center if no segment matches
}



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

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

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

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