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));
  frameData.clear();
}

void Animation::setFrameData(uint16_t frameIndex, const std::vector<MotorPosition>& motors) {
  if (frameIndex >= frameData.size()) {
    frameData.resize(frameIndex + 1);
  }
  frameData[frameIndex] = motors;
}

const std::vector<MotorPosition>* Animation::getFrameData(uint16_t frameIndex) const {
  if (frameIndex >= frameData.size()) {
    return nullptr;
  }
  return &frameData[frameIndex];
}

void Animation::clearFrameData() {
  frameData.clear();
}

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

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

  if (header.version == 2) {
    // Version 2: Write frame data
    // For each frame, write all motor positions
    for (uint16_t frameIndex = 0; frameIndex < frameData.size() && frameIndex < header.frameCount; frameIndex++) {
      const auto& frame = frameData[frameIndex];
      for (const auto& motorPos : frame) {
        file.write((uint8_t*)&motorPos, sizeof(MotorPosition));
      }
    }
  } else {
    // Version 1: Write curves and node graph
    uint16_t curveCount = 0;
    for (const auto& [motorID, segments] : curves) {
      curveCount += segments.size();
    }
    file.write((uint8_t*)&curveCount, sizeof(curveCount));
    for (const auto& [motorID, segments] : curves) {
      for (const CurveSegment& seg : segments) {
        file.write((uint8_t*)&seg, sizeof(CurveSegment));
      }
    }

    // ✅ Write serialized node graph
    std::vector<uint8_t> graphData = nodeGraph.serialize();
    file.write(graphData.data(), graphData.size());
  }

  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) {
    file.close();
    return false;
  }

  if (tempHeader.version != 1 && tempHeader.version != 2) {
    file.close();
    return false;
  }

  header = tempHeader;

  if (header.version == 2) {
    // Version 2: Read frame data
    clearFrameData();
    frameData.reserve(header.frameCount);
    
    // Calculate motor count from file size
    size_t fileSize = file.size();
    size_t headerSize = sizeof(AnimationHeader);
    size_t frameDataSize = fileSize - headerSize;
    size_t frameSize = frameDataSize / header.frameCount;  // bytes per frame
    uint16_t motorCount = frameSize / sizeof(MotorPosition);  // motors per frame
    
    if (frameSize % sizeof(MotorPosition) != 0) {
      file.close();
      return false;  // Invalid frame data size
    }
    
    // Read all frames
    for (uint16_t frameIndex = 0; frameIndex < header.frameCount; frameIndex++) {
      std::vector<MotorPosition> frame;
      frame.reserve(motorCount);
      
      for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) {
        MotorPosition motorPos;
        if (file.read((uint8_t*)&motorPos, sizeof(MotorPosition)) != sizeof(MotorPosition)) {
          file.close();
          return false;
        }
        frame.push_back(motorPos);
      }
      
      frameData.push_back(frame);
    }
  } else {
    // Version 1: Read curves and node graph
    // Read curve count
    uint16_t curveCount;
    if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
      file.close();
      return false;
    }

    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;
      }
      curves[seg.motorID].push_back(seg);
    }

    // ✅ Read remaining bytes into buffer
    size_t remaining = file.available();
    if (remaining > 0) {
      std::vector<uint8_t> buffer(remaining);
      if (file.read(buffer.data(), remaining) != remaining) {
        file.close();
        return false;
      }

      // ✅ Load node graph from buffer
      nodeGraph.nodes.clear();
      nodeGraph.connections.clear();
      loadNodeGraph(buffer.data(), buffer.size(), nodeGraph);
      nodeGraph.bindAnimationContext(this);
    }
  }

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

String Animation::printAnim() {
  String output = "ANIMATION INFO\n";
  output += "==============\n";
  output += "Version: " + String(header.version) + "\n";
  output += "Frame Count: " + String(header.frameCount) + "\n";
  output += "Frame Rate: " + String(header.frameRate) + " fps\n";
  
  if (header.frameRate > 0) {
    float duration = (float)header.frameCount / (float)header.frameRate;
    output += "Duration: " + String(duration, 2) + " seconds\n";
  }
  
  output += "Active: " + String(isActive() ? "Yes" : "No") + "\n";
  
  if (header.version == 1) {
    // Version 1: curves and node graph
    uint16_t curveCount = 0;
    for (const auto& [motorID, segments] : curves) {
      curveCount += segments.size();
    }
    output += "Curve Segments: " + String(curveCount) + "\n";
    output += "Motors with Curves: " + String(curves.size()) + "\n";
    output += "Node Graph Nodes: " + String(nodeGraph.nodes.size()) + "\n";
    output += "Node Graph Connections: " + String(nodeGraph.connections.size()) + "\n";
  } else if (header.version == 2) {
    // Version 2: frame data
    if (!frameData.empty()) {
      uint16_t motorCount = frameData[0].size();
      output += "Motors per Frame: " + String(motorCount) + "\n";
      output += "Frames Stored: " + String(frameData.size()) + "\n";
      
      // Show motor IDs from first frame
      if (!frameData[0].empty()) {
        output += "Motor IDs: ";
        for (size_t i = 0; i < frameData[0].size(); i++) {
          if (i > 0) output += ", ";
          output += String(frameData[0][i].motorID);
        }
        output += "\n";
      }
    } else {
      output += "Frame Data: Empty\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);
}