HansonServo/nodegraph.cpp

#include "nodegraph.h"
#include "animation.h"
#include "noise.h"
#include "RobotConfig.h"
#include "sensors.h"
#include <cstring>

extern RobotConfig config;
extern ADXL345 adxl;
extern Radar radar;


// CurveNode evaluation
void CurveNode::evaluate(uint32_t 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
}

void NoiseNode::evaluate(uint32_t tick) {
  float t = (static_cast<float>(tick) / 480.0f) * frequency * 10.0f;

  float raw = perlin1D_octave(seed, t, 4, 0.5f);
  outputValue = (raw + 1.0f) * 2047.5f;  // → [0, 4095]
}


// ServoNode evaluation
void ServoNode::evaluate(uint32_t tick) {
  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;
    case VAR_SERVO_FEEDBACK:
      outputValue = config.getMotorPosition(arg0);
      break;
      
    // ADXL345 accelerometer sources - scale to 0-4095 range
    case VAR_ACCEL_X:
      // X acceleration: -2g to +2g → 0-4095 (2048 = 0g)
      if (adxl.isReady()) {
        float accel = constrain(adxl.getAccelX(), -2.0f, 2.0f);
        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
      } else {
        outputValue = 2048;
      }
      break;
    case VAR_ACCEL_Y:
      // Y acceleration: -2g to +2g → 0-4095 (2048 = 0g)
      if (adxl.isReady()) {
        float accel = constrain(adxl.getAccelY(), -2.0f, 2.0f);
        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
      } else {
        outputValue = 2048;
      }
      break;
    case VAR_ACCEL_Z:
      // Z acceleration: -2g to +2g → 0-4095 (2048 = 0g)
      if (adxl.isReady()) {
        float accel = constrain(adxl.getAccelZ(), -2.0f, 2.0f);
        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
      } else {
        outputValue = 2048;
      }
      break;
    case VAR_TILT_PITCH:
      // Pitch angle: -90° to +90° → 0-4095 (2048 = level)
      if (adxl.isReady()) {
        float pitch = constrain(adxl.getPitch(), -90.0f, 90.0f);
        outputValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
      } else {
        outputValue = 2048;
      }
      break;
    case VAR_TILT_ROLL:
      // Roll angle: -90° to +90° → 0-4095 (2048 = level)
      if (adxl.isReady()) {
        float roll = constrain(adxl.getRoll(), -90.0f, 90.0f);
        outputValue = (uint16_t)(((roll + 90.0f) / 180.0f) * 4095.0f);
      } else {
        outputValue = 2048;
      }
      break;
      
    // Radar sources - primary target (index 0)
    case VAR_RADAR_X:
      // X position: -200cm to +200cm → 0-4095 (2048 = center)
      {
        const RadarTarget& target = radar.getTarget(0);
        if (target.valid) {
          float x = constrain(target.x, -200.0f, 200.0f);
          outputValue = (uint16_t)(((x + 200.0f) / 400.0f) * 4095.0f);
        } else {
          outputValue = 2048;  // Center if no target
        }
      }
      break;
    case VAR_RADAR_Y:
      // Y distance: 0-500cm → 0-4095
      {
        const RadarTarget& target = radar.getTarget(0);
        if (target.valid) {
          float y = constrain(target.y, 0.0f, 500.0f);
          outputValue = (uint16_t)((y / 500.0f) * 4095.0f);
        } else {
          outputValue = 0;  // No target = 0 distance
        }
      }
      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, const Animation& animation) {
  // 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
        }
      }
    }
  }
}



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();
        Node* node = idMap[id];

        // ✅ Reset values here
        node->inputValue = 65535;
        node->outputValue = 65535;

        sorted.push_back(node);

        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;

  // Read node count
  uint16_t nodeCount = packet[offset];
  offset += 1;

  // Parse nodes
  for (uint16_t i = 0; i < nodeCount; ++i) {
    if (offset + 6 > length) break;  // safety check

    uint8_t type = packet[offset++];
    uint8_t id = packet[offset++];
    uint16_t x = packet[offset] | (packet[offset + 1] << 8);
    offset += 2;
    uint16_t y = packet[offset] | (packet[offset + 1] << 8);
    offset += 2;

    Node* node = nullptr;

    switch (type) {
      case TYPE_CURVENODE:
        {  // CurveNode
          if (offset + 1 > length) break;
          uint8_t curveID = packet[offset++];
          auto* curve = new CurveNode();
          curve->id = id;
          curve->type = type;
          curve->x = x;
          curve->y = y;
          curve->curveID = curveID;
          node = curve;
          break;
        }
      case TYPE_SERVONODE:
        {  // ServoNode
          if (offset + 1 > length) break;
          uint8_t motorID = packet[offset++];
          auto* servo = new ServoNode();
          servo->id = id;
          servo->type = type;
          servo->x = x;
          servo->y = y;
          servo->motorID = motorID;
          node = servo;
          break;
        }
      case TYPE_VARIABLENODE:
        {  // ServoNode
          if (offset + 1 > length) break;
          uint8_t source = packet[offset++];
          uint8_t arg0 = packet[offset++];
          auto* newNode = new VariableNode();
          newNode->id = id;
          newNode->type = type;
          newNode->x = x;
          newNode->y = y;
          newNode->source = static_cast<VariableSource>(source);
          newNode->arg0 = arg0;
          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:
        {
          if (offset + 6 > length) break;  // 4 bytes for float + 2 bytes for uint16_t

          float frequency;
          uint16_t seed;

          memcpy(&frequency, &packet[offset], sizeof(float));
          offset += 4;

          memcpy(&seed, &packet[offset], sizeof(uint16_t));
          offset += 2;

          auto* noiseNode = new NoiseNode();
          noiseNode->id = id;
          noiseNode->type = type;
          noiseNode->x = x;
          noiseNode->y = y;
          noiseNode->frequency = frequency;
          noiseNode->seed = seed;
          node = noiseNode;
        }
      default:
        break;
    }

    if (node) {

      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;

  for (uint16_t i = 0; i < connectionCount; ++i) {
    if (offset + 2 > length) break;
    uint8_t fromID = packet[offset++];
    uint8_t toID = packet[offset++];

    graph.connections.push_back({ fromID, toID });
  }
}

String printNodeGraph(const NodeGraph& graph) {
  String output = "📦 NodeGraph Dump\n";

  output += "Nodes:\n";
  for (const Node* node : graph.nodes) {
    output += "  ID " + String(node->id);
    output += " | Type " + String(node->type);
    output += " | Pos (" + String(node->x) + ", " + String(node->y) + ")";

    switch (node->type) {
      case TYPE_CURVENODE:
        {  // CurveNode
          const CurveNode* curve = static_cast<const CurveNode*>(node);
          output += " | CurveID " + String(curve->curveID);
          break;
        }
      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);
          output += " | arg0: " + String(servo->arg0);
          break;
        }
      case TYPE_NOISENODE:
        {  // NoiseNode
          const NoiseNode* noise = static_cast<const NoiseNode*>(node);
          output += " | Frequency " + String(noise->frequency, 4);
          output += " | Seed " + String(noise->seed);
          break;
        }
    }

    output += "\n";
  }

  output += "Connections:\n";
  for (const auto& conn : graph.connections) {
    output += "  " + String(conn.fromID) + " → " + String(conn.toID) + "\n";
  }

  return output;
}

std::vector<uint8_t> NodeGraph::serialize() {
  std::vector<uint8_t> data;

  // Node count
  data.push_back(static_cast<uint8_t>(nodes.size()));

  // Serialize nodes
  for (Node* node : nodes) {
    data.push_back(node->type);
    data.push_back(node->id);

    uint16_t x = node->x;
    uint16_t y = node->y;
    data.insert(data.end(), reinterpret_cast<uint8_t*>(&x), reinterpret_cast<uint8_t*>(&x) + sizeof(uint16_t));
    data.insert(data.end(), reinterpret_cast<uint8_t*>(&y), reinterpret_cast<uint8_t*>(&y) + sizeof(uint16_t));

    switch (node->type) {
      case TYPE_SERVONODE:
        {
          auto* n = static_cast<ServoNode*>(node);
          data.push_back(n->motorID);
          break;
        }
      case TYPE_CURVENODE:
        {
          auto* n = static_cast<CurveNode*>(node);
          data.push_back(n->curveID);
          break;
        }
      case TYPE_VARIABLENODE:
        {
          auto* n = static_cast<VariableNode*>(node);
          data.push_back(static_cast<uint8_t>(n->source));
          data.push_back(static_cast<uint8_t>(n->arg0));
          break;
        }
      case TYPE_MATHNODE:
        {
          auto* n = static_cast<MathNode*>(node);
          data.push_back(static_cast<uint8_t>(n->op));
          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->value), reinterpret_cast<uint8_t*>(&n->value) + sizeof(float));
          break;
        }
      case TYPE_MAPNODE:
        {
          auto* n = static_cast<MapNode*>(node);
          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->inMin), reinterpret_cast<uint8_t*>(&n->inMin) + sizeof(float));
          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->inMax), reinterpret_cast<uint8_t*>(&n->inMax) + sizeof(float));
          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->outMin), reinterpret_cast<uint8_t*>(&n->outMin) + sizeof(float));
          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->outMax), reinterpret_cast<uint8_t*>(&n->outMax) + sizeof(float));
          break;
        }
      case TYPE_NOISENODE:
        {
          auto* n = static_cast<NoiseNode*>(node);
          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->frequency), reinterpret_cast<uint8_t*>(&n->frequency) + sizeof(float));
          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->seed), reinterpret_cast<uint8_t*>(&n->seed) + sizeof(uint16_t));
          break;
        }
    }
  }

  // Connection count
  data.push_back(static_cast<uint8_t>(connections.size()));

  // Serialize connections
  for (const NodeConnection& conn : connections) {
    data.push_back(conn.fromID);
    data.push_back(conn.toID);
  }

  return data;
}