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::setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value) {
  if (frameIndex < MAX_FRAMES && channel < NUM_CHANNELS) {
    data[frameIndex][channel] = value;
  }
}

uint16_t Animation::getFrame(uint16_t frameIndex, uint16_t channel) const {
  if (frameIndex < MAX_FRAMES && channel < NUM_CHANNELS) {
    return data[frameIndex][channel];
  }
  return 0;
}

bool Animation::getFramePositions(uint16_t frameIndex, uint16_t* outPositions) {
  if (frameIndex >= header.frameCount) return false;

  for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
    outPositions[ch] = getFrame(frameIndex, ch);
  }
  return true;
}

void Animation::addKeyframe(uint8_t motorId, uint16_t frame, uint16_t position) {
  keyframes.push_back({ motorId, frame, position });
}

const std::vector<Keyframe>& Animation::getKeyframes() const {
  return keyframes;
}

void Animation::printKeyframes() {
  const std::vector<Keyframe>& frames = getKeyframes();
  Serial.println("Keyframes:");
  for (size_t i = 0; i < frames.size(); ++i) {
    const Keyframe& kf = frames[i];
    Serial.print("  [");
    Serial.print(i);
    Serial.print("] Motor ID: ");
    Serial.print(kf.motorId);
    Serial.print(", Frame: ");
    Serial.print(kf.frame);
    Serial.print(", Position: ");
    Serial.println(kf.position);
  }
}

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;  // +1 because frame index starts at 0

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

    // Write keyframe count
  uint16_t keyframeCount = keyframes.size();
  file.write((uint8_t*)&keyframeCount, sizeof(keyframeCount));

  // Write keyframes
  for (const Keyframe& kf : keyframes) {
    file.write(kf.motorId);
    file.write((uint8_t*)&kf.frame, sizeof(kf.frame));
    file.write((uint8_t*)&kf.position, sizeof(kf.position));
  }



  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 keyframe count
  uint16_t keyframeCount;
  if (file.read((uint8_t*)&keyframeCount, sizeof(keyframeCount)) != sizeof(keyframeCount)) {
    file.close();
    return false;
  }

  // Read keyframes
  keyframes.clear();
  for (uint16_t i = 0; i < keyframeCount; i++) {
    Keyframe kf;
    if (file.read(&kf.motorId, 1) != 1 ||
        file.read((uint8_t*)&kf.frame, sizeof(kf.frame)) != sizeof(kf.frame) ||
        file.read((uint8_t*)&kf.position, sizeof(kf.position)) != sizeof(kf.position)) {
      file.close();
      return false;
    }
    keyframes.push_back(kf);
  }


  file.close();
  return true;
}


void Animation::createSampleSweep(uint8_t seconds) {
  clear();

  const uint16_t sweepFrames = FRAMES_PER_SECOND * seconds;
  const uint16_t totalFrames = sweepFrames * 2;  // Up and down

  for (uint16_t frame = 0; frame < totalFrames; frame++) {
    float progress;
    if (frame < sweepFrames) {
      // Sweep up: 0 → 1023
      progress = (float)frame / (sweepFrames - 1);
    } else {
      // Sweep down: 1023 → 0
      progress = 1.0f - ((float)(frame - sweepFrames) / (sweepFrames - 1));
    }

    uint16_t value = (uint16_t)(progress * 1023);

    for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
      setFrame(frame, ch, value);
    }
  }

  header.frameCount = totalFrames;

  // 🧩 Add keyframes to match the sweep motion
  for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
    addKeyframe(ch, 0, 0);                          // Start at 0
    addKeyframe(ch, sweepFrames - 1, 1023);         // Peak
    addKeyframe(ch, totalFrames - 1, 0);            // Return to 0
  }

}

void Animation::createStaggeredSweep(uint8_t seconds) {
  clear();

  const uint16_t sweepFrames = FRAMES_PER_SECOND * seconds;
  const uint16_t totalFrames = sweepFrames * 2;  // Up and down

  for (uint16_t frame = 0; frame < totalFrames; frame++) {
    float progress;
    if (frame < sweepFrames) {
      progress = (float)frame / (sweepFrames - 1);  // 0 → 1
    } else {
      progress = 1.0f - ((float)(frame - sweepFrames) / (sweepFrames - 1));  // 1 → 0
    }

    for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
      float channelProgress = (ch % 2 == 0) ? progress : (1.0f - progress);
      uint16_t value = (uint16_t)(channelProgress * 1023);
      setFrame(frame, ch, value);
    }
  }

  header.frameCount = totalFrames;
}