variable, math, and map() nodes are implemented

HansonServo.ino

@@ -38,8 +38,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE];  // Global or static
 #define RE_PIN 8       // Receiver Enable
 
 Animation anim;
-Animation sweep;
+Animation* currentAnimation;
-Animation stagger;
 
 bool streamPositions = false;
 unsigned long lastStreamPositions = 0;
@@ -59,45 +58,15 @@ uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 };
 uint16_t pos1STS[] = { 0, 0 };
 uint16_t pos2STS[] = { 1023, 1023 };
 
-const uint8_t testPayload[] = {
+#define WAVE_PERIOD_CS 400  // 4 seconds = 400 centiseconds
-  // 🔹 Filename block (length = 17)
+#define WAVE_MAX 4095
-  17, 0,
-  '/', 'p', 'o', 'i', 'n', 't', 'c', 'u', 'r', 'v', 'e', '2', '.', 'a', 'n', 'i', 'm',
 
-  // 🔹 Header: "ANIM", frameCount=800, version=1, frameRate=50, reserved[8]
+uint16_t getSineWaveValue(unsigned long centiseconds) {
-  'A', 'N', 'I', 'M',
+  float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS;
-  32, 3,                   // frameCount = 800 (little-endian)
+  float sine = sin(theta);                         // ranges from -1 to +1
-  1,                       // version
+  float scaled = (sine + 1.0) * (WAVE_MAX / 2.0);  // scale to 0–4095
-  50,                      // frameRate
+  return (uint16_t)round(scaled);
-  0, 0, 0, 0, 0, 0, 0, 0,  // reserved
+}
-
-  // 🔹 Curve count = 2
-  2, 0,
-
-  // 🔹 Curve Segment 1
-  10,     // motorID
-  0, 0,   // startTime
-  40, 0,  // endTime
-  44, 1,  // startPointY = 300
-  10, 0,  // startHandleX
-  44, 1,  // startHandleY = 300
-  30, 0,  // endHandleX
-  0, 0,   // endHandleY
-  0, 0,   // endPointY
-
-  // 🔹 Curve Segment 2
-  10,      // motorID
-  40, 0,   // startTime
-  32, 3,   // endTime = 800
-  0, 0,    // startPointY
-  200, 0,  // startHandleX
-  0, 0,    // startHandleY
-  88, 2,   // endHandleX = 600
-  44, 1,   // endHandleY = 300
-  44, 1    // endPointY = 300
-};
-
-const uint16_t testPayloadLength = sizeof(testPayload);
 
 
 void setup() {
@@ -108,6 +77,9 @@ void setup() {
     delay(500);
   }
 
+  pinMode(6, INPUT);
+  pinMode(7, INPUT);
+
   servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN);  // SCS
   servos[0]->setFeetechMode(Feetech::MODE_SCS);
   servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN);  // STS
@@ -659,31 +631,35 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
   for (uint16_t i = 0; i < curveCount; i++) {
     CurveSegment seg;
     memcpy(&seg, ptr, sizeof(CurveSegment));
-    Serial.print("Segment ");
+    // Serial.print("Segment ");
-    Serial.print(i);
+    // Serial.print(i);
-    Serial.print(": motorID=");
+    // Serial.print(": motorID=");
-    Serial.print(seg.motorID);
+    // Serial.print(seg.motorID);
-    Serial.print(", startTime=");
+    // Serial.print(", startTime=");
-    Serial.print(seg.startTime);
+    // Serial.print(seg.startTime);
-    Serial.print(", endTime=");
+    // Serial.print(", endTime=");
-    Serial.print(seg.endTime);
+    // Serial.print(seg.endTime);
-    Serial.print(", startPointY=");
+    // Serial.print(", startPointY=");
-    Serial.print(seg.startPointY);
+    // Serial.print(seg.startPointY);
-    Serial.print(", startHandleX=");
+    // Serial.print(", startHandleX=");
-    Serial.print(seg.startHandleX);
+    // Serial.print(seg.startHandleX);
-    Serial.print(", startHandleY=");
+    // Serial.print(", startHandleY=");
-    Serial.print(seg.startHandleY);
+    // Serial.print(seg.startHandleY);
-    Serial.print(", endHandleX=");
+    // Serial.print(", endHandleX=");
-    Serial.print(seg.endHandleX);
+    // Serial.print(seg.endHandleX);
-    Serial.print(", endHandleY=");
+    // Serial.print(", endHandleY=");
-    Serial.print(seg.endHandleY);
+    // Serial.print(seg.endHandleY);
-    Serial.print(", endPointY=");
+    // Serial.print(", endPointY=");
-    Serial.println(seg.endPointY);
+    // Serial.println(seg.endPointY);
+
 
     animation.addCurveSegment(seg);
     ptr += sizeof(CurveSegment);
   }
-
+  sendMessage(anim.printCurves());
+  // for (int i = 0; i < 800; i++){
+  //  sendMessage(String(anim.getMotorPosition(11, i)));
+  // }
   // ✅ Advance ptr to node graph payload
   //ptr += curveCount * sizeof(CurveSegment);
 
@@ -691,18 +667,22 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
   uint8_t nodeCount = ptr[0];
   Serial.print("Node count: ");
   Serial.println(nodeCount);
-  sendMessage(String("Node count: ") + String(nodeCount));
+  // sendMessage(String("Node count: ") + String(nodeCount));
-  sendMessage(String("ptr offset: ") + String(ptr - payload));
+  // sendMessage(String("ptr offset: ") + String(ptr - payload));
 
 
   uint16_t remainingLength = length - (ptr - payload);
   if (remainingLength > 0) {
     loadNodeGraph(ptr, remainingLength, animation.nodeGraph);
+    animation.nodeGraph.bindAnimationContext(&animation);
   } else {
     Serial.println("No node graph data found");
   }
   sendMessage(printNodeGraph(animation.nodeGraph));
 
+  currentAnimation = &animation;
+  currentAnimation->setActive(true);  // ✅ mark it as running
+
   // 🔹 Save using received filename
   animation.saveToFile(("/" + String(filename)).c_str());
 
@@ -849,6 +829,8 @@ bool flip = false;
 unsigned long lastSend = 0;
 void loop() {
 
+
+  runNodeAnimation();
   // for (int i = 0; i < 1023; i++) {
   //   servos[0]->sendWritePos(10, i);
   //   Serial.println(servos[0]->waitOnData1Byte(10));
@@ -920,6 +902,64 @@ void loop() {
   }
 }
 
+void runNodeAnimation() {
+  static uint32_t lastTickTime = 0;
+  static uint32_t currentTick = 0;
+  const uint16_t FRAME_INTERVAL_MS = 1000 / 48;
+
+  if (!currentAnimation) return;  // ✅ Prevent crash
+  if (!currentAnimation->isActive()) return;
+
+  uint32_t now = millis();
+  if (now - lastTickTime >= FRAME_INTERVAL_MS) {
+    lastTickTime = now;
+
+    currentAnimation->nodeGraph.tick(currentTick, *currentAnimation);
+
+    // if (currentTick == 100){
+    //   message += String(anim.getMotorPosition(10, currentTick));
+    //   message += "\n";
+    // }
+
+    auto outputs = currentAnimation->nodeGraph.getServoOutputs();
+
+    std::vector<uint8_t> motorIDs;
+    std::vector<uint16_t> positions;
+    String message = String(currentTick) + String("\n");
+
+    for (const auto& [motorID, value] : outputs) {
+      motorIDs.push_back(motorID);
+      positions.push_back(map(value, 0, 4095, 0, 1023));
+
+      message += "Motor ";
+      message += String(motorID);
+      message += " → ";
+      message += String(value);
+      message += "\n";
+    }
+
+    uint8_t motorCount = motorIDs.size();  // ✅ number of motors
+
+    if (currentTick % 20 == 0) {
+      sendMessage(message);
+    }
+
+    // ✅ Send to servo controller
+    servos[0]->syncWritePos(motorIDs.data(), positions.data(), motorCount);
+
+
+
+    currentTick++;
+
+    if (currentTick > currentAnimation->getFrameCount()) {
+      currentAnimation->setActive(false);
+      currentTick = 0;  // optional: reset for next run
+    }
+  }
+}
+
+
+
 void SendMotorPositions() {
 
   uint16_t p0 = servos[0]->getPosition(10);
@@ -1100,29 +1140,29 @@ void playAnimationOLD(Animation& anim) {
 
 
 
-void playLayeredAnimation(Animation& base, Animation& overlay) {
+// void playLayeredAnimation(Animation& base, Animation& overlay) {
-  uint16_t basePositions[NUM_CHANNELS];
+//   uint16_t basePositions[NUM_CHANNELS];
-  uint16_t overlayPositions[NUM_CHANNELS];
+//   uint16_t overlayPositions[NUM_CHANNELS];
-  uint16_t finalPositions[NUM_CHANNELS];
+//   uint16_t finalPositions[NUM_CHANNELS];
 
-  const uint16_t frameCount = base.getFrameCount();
+//   const uint16_t frameCount = base.getFrameCount();
-  const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
+//   const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
-  uint32_t nextFrameTime = millis();
+//   uint32_t nextFrameTime = millis();
 
-  for (uint16_t frame = 0; frame < frameCount; frame++) {
+//   for (uint16_t frame = 0; frame < frameCount; frame++) {
-    while (millis() < nextFrameTime) delay(1);
+//     while (millis() < nextFrameTime) delay(1);
 
-    base.getFramePositions(frame, basePositions);
+//     base.getFramePositions(frame, basePositions);
-    overlay.getFramePositions(frame, overlayPositions);
+//     overlay.getFramePositions(frame, overlayPositions);
 
-    for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
+//     for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
-      finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2;
+//       finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2;
-    }
+//     }
 
-    servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS);
+//     servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS);
-    nextFrameTime += frameDelay;
+//     nextFrameTime += frameDelay;
-  }
+//   }
-}
+// }
 
 
 void SCSPingAll() {

animation.cpp

@@ -9,6 +9,11 @@ Animation::Animation() {
   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);
 }
@@ -24,14 +29,10 @@ void Animation::clearAllCurves() {
 
 
 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 = [](int16_t v) {
-        return (float(v) / 65535.0f) * 2.0f - 1.0f;
-      };
 
       // Define control points
       float x0 = seg.startTime;
@@ -39,10 +40,11 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
       float x2 = seg.endHandleX;
       float x3 = seg.endTime;
 
-      float y0 = toFloat(seg.startPointY);
+      float y0 = seg.startPointY;
-      float y1 = toFloat(seg.startHandleY);
+      float y1 = seg.startHandleY;
-      float y2 = toFloat(seg.endHandleY);
+      float y2 = seg.endHandleY;
-      float y3 = toFloat(seg.endPointY);
+      float y3 = seg.endPointY;
+
 
       // Solve for t such that Bézier x(t) ≈ timeCS
       auto bezierX = [&](float t) {
@@ -67,7 +69,8 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
       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 constrain(value, 0, 4095);
+
     }
   }
 
@@ -169,40 +172,38 @@ bool Animation::loadFromFile(const char* filename) {
 }
 
 
-void Animation::printCurves() {
+String Animation::printCurves() {
-  Serial.println("PRINTING CURVES");
+  String output = "PRINTING CURVES\n";
+
   for (const auto& [motorID, segments] : curves) {
-    Serial.print("Motor ");
+    output += "Motor ";
-    Serial.print(motorID);
+    output += String(motorID);
-    Serial.print(": ");
+    output += ": ";
-    Serial.println(segments.size());
+    output += String(segments.size());
+    output += "\n";
 
     for (const auto& seg : segments) {
-      Serial.print("  Segment: ");
+      output += "  Segment: ";
-      Serial.print("startTime=");
+      output += "startTime=" + String(seg.startTime);
-      Serial.print(seg.startTime);
+      output += ", endTime=" + String(seg.endTime);
-      Serial.print(", endTime=");
+      output += ", startPointY=" + String(seg.startPointY);
-      Serial.print(seg.endTime);
+      output += ", startHandleX=" + String(seg.startHandleX);
-      Serial.print(", startPointY=");
+      output += ", startHandleY=" + String(seg.startHandleY);
-      Serial.print(seg.startPointY);
+      output += ", endHandleX=" + String(seg.endHandleX);
-      Serial.print(", startHandleX=");
+      output += ", endHandleY=" + String(seg.endHandleY);
-      Serial.print(seg.startHandleX);
+      output += ", endPointY=" + String(seg.endPointY);
-      Serial.print(", startHandleY=");
+      output += "\n";
-      Serial.print(seg.startHandleY);
-      Serial.print(", endHandleX=");
-      Serial.print(seg.endHandleX);
-      Serial.print(", endHandleY=");
-      Serial.print(seg.endHandleY);
-      Serial.print(", endPointY=");
-      Serial.println(seg.endPointY);
     }
   }
+
+  return output;
 }
 
 
 
 
 
+
 void Animation::createBasicSCurve() {
   clearAllCurves();
 

animation.h

@@ -7,6 +7,7 @@
 #include "nodegraph.h"
 
 
+
 #define NUM_CHANNELS 5
 #define FRAMES_PER_SECOND 50
 #define MAX_DURATION_SECONDS 10
@@ -39,15 +40,17 @@ struct __attribute__((packed)) CurveSegment {
 class Animation {
 public:
   Animation();
+  bool isActive() const { return active; }
+  void setActive(bool state);
+
 
   void setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value);
   uint16_t getFrame(uint16_t frameIndex, uint16_t channel) const;
-  bool getFramePositions(uint16_t frameIndex, uint16_t* outPositions);
 
   void addCurveSegment(const CurveSegment& segment);
   void clearCurves(uint8_t motorID);
   void clearAllCurves();
-  void printCurves();
+  String printCurves();
   uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS);
   
   void clear();
@@ -66,6 +69,6 @@ public:
 private:
   //uint16_t data[MAX_FRAMES][NUM_CHANNELS];
   std::unordered_map<uint8_t, std::vector<CurveSegment>> curves;
-
+  bool active = false;
 };
 

nodegraph.cpp

@@ -1,26 +1,164 @@
 #include "nodegraph.h"
+#include "animation.h"
 #include <cstring>
 
 // CurveNode evaluation
 void CurveNode::evaluate(uint32_t tick) {
-    //outputValue = getCurveValue(curveID, tick);
+  if (!animation) {
+    outputValue = 2048;
+    return;
   }
 
+  outputValue = animation->getMotorPosition(curveID, tick);
+}
+
+
+Node* NodeGraph::findNodeByID(uint8_t id) const {
+  for (Node* node : nodes) {
+    if (node->id == id) {
+      return node;
+    }
+  }
+  return nullptr;  // not found
+}
+
+
 // ServoNode evaluation
 void ServoNode::evaluate(uint32_t tick) {
-    //setMotorPWM(motorID, inputValue);
+  outputValue = inputValue;
 }
 
+extern uint16_t getSineWaveValue(uint32_t tick);
+// extern int faceDetectX();
+// extern int faceDetectY();
+
+void VariableNode::evaluate(uint32_t currentTick) {
+  switch (source) {
+    case VAR_SINE:
+      outputValue = getSineWaveValue(currentTick);
+      break;
+    case VAR_FACE_X:
+      //outputValue = faceDetectX();
+      break;
+    case VAR_FACE_Y:
+      //outputValue = faceDetectY();
+      break;
+    case VAR_ANALOG:
+      outputValue = analogRead(7);  // or whichever pin
+      break;
+  }
+}
+
+void MathNode::evaluate(uint32_t tick) {
+  float input = static_cast<float>(inputValue);
+  float result = 0;
+
+  switch (op) {
+    case OP_MULTIPLY: result = input * value; break;
+    case OP_DIVIDE: result = value != 0 ? input / value : 0; break;
+    case OP_ADD: result = input + value; break;
+    case OP_SUBTRACT: result = input - value; break;
+  }
+
+  outputValue = static_cast<uint16_t>(result);
+}
+
+void MapNode::evaluate(uint32_t tick) {
+  int32_t input = inputValue;
+  if (inMax == inMin) {
+    outputValue = outMin;
+    return;
+  }
+
+  int32_t result = (input - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
+  outputValue = static_cast<uint16_t>(result);
+}
+
+
 // NodeGraph tick
-void NodeGraph::tick(uint32_t currentTick) {
+void NodeGraph::tick(uint32_t currentTick, const Animation& animation) {
-    // First pass: evaluate all nodes
+  // Step 1: Evaluate each node and propagate outputs immediately
   for (Node* node : nodes) {
     node->evaluate(currentTick);
+
+    for (const NodeConnection& conn : connections) {
+      if (conn.fromID == node->id) {
+        Node* to = findNodeByID(conn.toID);
+        if (to) {
+          to->inputValue = node->outputValue;  // ✅ generic propagation
+        }
+      }
+    }
+  }
 }
 
-    // Optional: if you want to simulate wiring between nodes,
+
-    // you could add a second pass here to propagate values
+
+std::vector<std::pair<uint8_t, uint16_t>> NodeGraph::getServoOutputs() const {
+  std::vector<std::pair<uint8_t, uint16_t>> outputs;
+
+  for (Node* node : nodes) {
+    if (node->type == TYPE_SERVONODE) {
+      const ServoNode* servo = static_cast<const ServoNode*>(node);
+      outputs.emplace_back(servo->motorID, servo->outputValue);
     }
+  }
+
+  return outputs;
+}
+
+
+std::vector<Node*> NodeGraph::getSortedNodes() {
+  std::unordered_map<uint8_t, std::vector<uint8_t>> adj;
+  std::unordered_map<uint8_t, int> inDegree;
+  std::unordered_map<uint8_t, Node*> idMap;
+
+  for (Node* node : nodes) {
+    idMap[node->id] = node;
+    inDegree[node->id] = 0;
+  }
+
+  for (const NodeConnection& conn : connections) {
+    adj[conn.fromID].push_back(conn.toID);
+    inDegree[conn.toID]++;
+  }
+
+  std::vector<Node*> sorted;
+  std::queue<uint8_t> q;
+
+  for (const auto& [id, deg] : inDegree) {
+    if (deg == 0) q.push(id);
+  }
+
+  while (!q.empty()) {
+    uint8_t id = q.front();
+    q.pop();
+    sorted.push_back(idMap[id]);
+
+    for (uint8_t neighbor : adj[id]) {
+      if (--inDegree[neighbor] == 0) {
+        q.push(neighbor);
+      }
+    }
+  }
+
+  return sorted;
+}
+
+// This function links nodes to their outside inputs
+void NodeGraph::bindAnimationContext(Animation* animation) {
+  for (Node* node : nodes) {
+    if (node->type == TYPE_CURVENODE) {
+      CurveNode* curveNode = static_cast<CurveNode*>(node);
+      curveNode->animation = animation;  // ✅ link from outside
+    }
+
+    // Add other node types here as needed
+  }
+}
+
+
+
 
 void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
   size_t offset = 0;
@@ -35,13 +173,16 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
 
     uint8_t type = packet[offset++];
     uint8_t id = packet[offset++];
-        uint16_t x = packet[offset] | (packet[offset + 1] << 8); offset += 2;
+    uint16_t x = packet[offset] | (packet[offset + 1] << 8);
-        uint16_t y = packet[offset] | (packet[offset + 1] << 8); offset += 2;
+    offset += 2;
+    uint16_t y = packet[offset] | (packet[offset + 1] << 8);
+    offset += 2;
 
     Node* node = nullptr;
 
     switch (type) {
-            case TYPE_CURVENODE: { // CurveNode
+      case TYPE_CURVENODE:
+        {  // CurveNode
           if (offset + 1 > length) break;
           uint8_t curveID = packet[offset++];
           auto* curve = new CurveNode();
@@ -53,7 +194,8 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
           node = curve;
           break;
         }
-            case TYPE_SERVONODE: { // ServoNode
+      case TYPE_SERVONODE:
+        {  // ServoNode
           if (offset + 1 > length) break;
           uint8_t motorID = packet[offset++];
           auto* servo = new ServoNode();
@@ -65,7 +207,68 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
           node = servo;
           break;
         }
-            case TYPE_NOISENODE: { // NoiseNode
+      case TYPE_VARIABLENODE:
+        {  // ServoNode
+          if (offset + 1 > length) break;
+          uint8_t source = packet[offset++];
+          auto* newNode = new VariableNode();
+          newNode->id = id;
+          newNode->type = type;
+          newNode->x = x;
+          newNode->y = y;
+          newNode->source = static_cast<VariableSource>(source);
+          node = newNode;
+          break;
+        }
+      case TYPE_MATHNODE:
+        {
+          if (offset + 5 > length) break;  // 1 byte op + 4 bytes float
+          uint8_t rawOp = packet[offset++];
+          float val;
+          memcpy(&val, &packet[offset], sizeof(float));
+          offset += sizeof(float);
+
+          auto* math = new MathNode();
+          math->id = id;
+          math->type = type;
+          math->x = x;
+          math->y = y;
+          math->op = static_cast<MathOperator>(rawOp);
+          math->value = val;
+          node = math;
+          break;
+        }
+      case TYPE_MAPNODE:
+        {
+          if (offset + 16 > length) break;  // 4 x float32 = 16 bytes
+
+          float inMin, inMax, outMin, outMax;
+          memcpy(&inMin, &packet[offset], sizeof(float));
+          offset += 4;
+          memcpy(&inMax, &packet[offset], sizeof(float));
+          offset += 4;
+          memcpy(&outMin, &packet[offset], sizeof(float));
+          offset += 4;
+          memcpy(&outMax, &packet[offset], sizeof(float));
+          offset += 4;
+
+          auto* mapNode = new MapNode();
+          mapNode->id = id;
+          mapNode->type = type;
+          mapNode->x = x;
+          mapNode->y = y;
+          mapNode->inMin = inMin;
+          mapNode->inMax = inMax;
+          mapNode->outMin = outMin;
+          mapNode->outMax = outMax;
+          node = mapNode;
+          break;
+        }
+
+
+
+      case TYPE_NOISENODE:
+        {  // NoiseNode
           if (offset + 17 > length) break;
           offset += 17;  // skip for now
           break;
@@ -78,9 +281,14 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) {
 
       graph.nodes.push_back(node);
     }
+
+    // Sort node list topologically for execution.
+    graph.nodes = graph.getSortedNodes();
   }
 
   // Parse connections
+  graph.connections.clear();
+
   if (offset + 2 > length) return;
   uint16_t connectionCount = packet[offset];
   offset += 1;
@@ -104,16 +312,24 @@ String printNodeGraph(const NodeGraph& graph) {
     output += " | Pos (" + String(node->x) + ", " + String(node->y) + ")";
 
     switch (node->type) {
-      case TYPE_CURVENODE: { // CurveNode
+      case TYPE_CURVENODE:
+        {  // CurveNode
           const CurveNode* curve = static_cast<const CurveNode*>(node);
           output += " | CurveID " + String(curve->curveID);
           break;
         }
-      case TYPE_SERVONODE: { // ServoNode
+      case TYPE_SERVONODE:
+        {  // ServoNode
           const ServoNode* servo = static_cast<const ServoNode*>(node);
           output += " | MotorID " + String(servo->motorID);
           break;
         }
+      case TYPE_VARIABLENODE:
+        {  // VariableNode
+          const VariableNode* servo = static_cast<const VariableNode*>(node);
+          output += " | source: " + String(servo->source);
+          break;
+        }
         // case 2: { // NoiseNode
         //   const NoiseNode* noise = static_cast<const NoiseNode*>(node);
         //   output += " | Noise a=" + String(noise->a, 2);

nodegraph.h

@@ -1,13 +1,33 @@
 #pragma once
 #include <vector>
 #include <cstdint>
+#include <queue>
+#include <unordered_map>
 #include <Arduino.h>
+class Animation;
 
 #define TYPE_NODE 0x01
 #define TYPE_SERVONODE 0x02
 #define TYPE_CURVENODE 0x03
 #define TYPE_NOISENODE 0x04
+#define TYPE_VARIABLENODE 0x05
+#define TYPE_MATHNODE 0x06
+#define TYPE_MAPNODE 0x07
 
+enum VariableSource {
+  VAR_FACE_X,
+  VAR_FACE_Y,
+  VAR_SINE,
+  VAR_ANALOG
+};
+
+
+enum MathOperator {
+  OP_MULTIPLY,
+  OP_DIVIDE,
+  OP_ADD,
+  OP_SUBTRACT
+};
 
 
 // Base Node class
@@ -18,12 +38,14 @@ struct Node {
   uint16_t y;
   virtual void evaluate(uint32_t tick) = 0;
   virtual ~Node() {}
+  uint16_t inputValue;
+  uint16_t outputValue = 0;
 };
 
 // CurveNode: evaluates a curve at a given tick
 struct CurveNode : public Node {
   uint8_t curveID;
-  uint16_t outputValue;
+  Animation* animation = nullptr;  // ✅ no need to include animation.h
 
   void evaluate(uint32_t tick) override;
 };
@@ -31,11 +53,34 @@ struct CurveNode : public Node {
 // ServoNode: sends a value to a motor
 struct ServoNode : public Node {
   uint8_t motorID;
-  uint16_t inputValue;
 
   void evaluate(uint32_t tick) override;
 };
 
+struct VariableNode : public Node {
+  VariableSource source = VAR_SINE;  // default
+
+  void evaluate(uint32_t tick) override;
+  void setSource(VariableSource src) { source = src; }
+};
+
+struct MathNode : public Node {
+  MathOperator op = OP_MULTIPLY;
+  float value = 1.0f;
+
+  void evaluate(uint32_t tick) override;
+};
+
+struct MapNode : public Node {
+  float inMin = 0;
+  float inMax = 1023;
+  float outMin = 0;
+  float outMax = 255;
+
+  void evaluate(uint32_t tick) override;
+};
+
+
 // NodeGraph container
 struct NodeConnection {
   uint8_t fromID;
@@ -46,8 +91,14 @@ class NodeGraph {
 public:
   std::vector<Node*> nodes;
   std::vector<NodeConnection> connections;
+Node* findNodeByID(uint8_t id) const;
+
+  void tick(uint32_t currentTick, const Animation& animation);
+  std::vector<std::pair<uint8_t, uint16_t>> getServoOutputs() const;
+  std::vector<Node*> getSortedNodes();
+  void bindAnimationContext(Animation* animation);
+
 
-  void tick(uint32_t currentTick);
 };