HansonServo/robotconfig.cpp

#include "RobotConfig.h"
#include <FFat.h>

uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
  for (const Motor& motor : motors) {
    if (motor.motorID == motorID) {
      return motor.position;
    }
  }
  // Return 0 if motor not found
  return 2047;
}

uint8_t RobotConfig::getMotorModel(uint8_t motorID) {
  for (const Motor& motor : motors) {
    if (motor.motorID == motorID) {
      return motor.servoModel.minor;
    }
  }
  // Return 0 if motor not found
  return 5;
}

bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
  for (Motor& motor : motors) {
    if (motor.motorID == motorID) {
      motor.position = newPosition;
      return true;
    }
  }
  // Return false if motor not found
  return false;
}

bool RobotConfig::setMotorEnabled(uint8_t motorID, bool enable) {
  for (Motor& motor : motors) {
    if (motor.motorID == motorID) {
      if (motor.isEnabled != enable) {
        motor.isEnabled = enable;
        return true;  // ✅ Only return true if the value changed
      }
      return false;  // ❌ No change
    }
  }
  return false;  // ❌ Motor not found
}


bool RobotConfig::isMotorEnabled(uint8_t motorID) {
  for (Motor& motor : motors) {
    if (motor.motorID == motorID) {
      return motor.isEnabled;
    }
  }
  return false;
}

void RobotConfig::enableAllMotors() {
  for (Motor& motor : motors) {
    motor.isEnabled = true;
  }
}

String RobotConfig::serializeJSON() const {
  String output = "{";
  output += "\"deviceName\":\"" + deviceName + "\",";
  output += "\"firmwareVersion\":{\"major\":" + String(firmwareVersion.major) + ",\"minor\":" + String(firmwareVersion.minor) + "},";

  output += "\"motors\":[";

  for (size_t i = 0; i < motors.size(); ++i) {
    const Motor& m = motors[i];
    output += "{";
    output += "\"motorID\":" + String(m.motorID) + ",";
    output += "\"name\":\"" + m.name + "\",";
    output += "\"model\":" + String(m.servoModel.minor);
    output += "}";

    if (i < motors.size() - 1) {
      output += ",";
    }
  }

  output += "]}";
  return output;
}

std::vector<uint8_t> RobotConfig::serializeToBytes() const {
  std::vector<uint8_t> buffer;

  // Encode deviceName length + characters
  uint8_t nameLen = deviceName.length();
  buffer.push_back(nameLen);
  for (uint8_t i = 0; i < nameLen; ++i) {
    buffer.push_back(deviceName[i]);
  }

  // Encode firmwareVersion (int32_t → 4 bytes)
  buffer.push_back(firmwareVersion.major);
  buffer.push_back(firmwareVersion.minor);


  // Encode number of motors
  uint8_t motorCount = motors.size();
  buffer.push_back(motorCount);

  // Encode each motor
  for (const Motor& m : motors) {
    // motorID
    buffer.push_back(m.motorID);

    // name length + characters
    uint8_t motorNameLen = m.name.length();
    buffer.push_back(motorNameLen);
    for (uint8_t i = 0; i < motorNameLen; ++i) {
      buffer.push_back(m.name[i]);
    }

    // position (uint16_t → 2 bytes)
    buffer.push_back((m.position >> 8) & 0xFF);
    buffer.push_back(m.position & 0xFF);
  }

  return buffer;
}

bool RobotConfig::saveToFFat(const char* path) const {
  File file = FFat.open(path, FILE_WRITE);
  if (!file) return false;

  uint8_t nameLen = deviceName.length();
  file.write(nameLen);
  file.write((const uint8_t*)deviceName.c_str(), nameLen);

  file.write(firmwareVersion.major);
  file.write(firmwareVersion.minor);

  uint8_t motorCount = motors.size();
  file.write(motorCount);

  for (const Motor& m : motors) {
    file.write(m.servoModel.major);
    file.write(m.servoModel.minor);
    file.write(m.motorID & 0xFF);
    file.write((m.motorID >> 8) & 0xFF);

    uint8_t nameLen = m.name.length();
    file.write(nameLen);
    file.write((const uint8_t*)m.name.c_str(), nameLen);
  }

  file.close();
  return true;
}

bool RobotConfig::loadFromFFat(const char* path) {
  File file = FFat.open(path, FILE_READ);
  if (!file) return false;

  motors.clear();

  uint8_t nameLen = file.read();
  char nameBuf[64] = { 0 };
  file.readBytes(nameBuf, nameLen);
  deviceName = String(nameBuf);

  firmwareVersion.major = file.read();
  firmwareVersion.minor = file.read();

  uint8_t motorCount = file.read();

  for (uint8_t i = 0; i < motorCount; ++i) {
    ServoModel model;
    model.major = file.read();
    model.minor = file.read();

    uint16_t id = file.read();
    id |= (file.read() << 8);

    uint8_t motorNameLen = file.read();
    char motorNameBuf[64] = { 0 };
    file.readBytes(motorNameBuf, motorNameLen);

    Motor m;
    m.motorID = id;
    m.servoModel = model;
    m.name = String(motorNameBuf);
    m.position = 0;
    m.isEnabled = true;

    motors.push_back(m);
  }

  file.close();
  return true;
}

bool RobotConfig::loadOrCreateDefault(const char* path) {
  if (FFat.exists(path)) {
    Serial.println("Loading robot config from FFat...");
    return loadFromFFat(path);
  }

  Serial.println("No config found. Creating default config...");

  // 🔧 Define your default config here
  deviceName = "DefaultBot";
  firmwareVersion = { 1, 0 };

  motors.clear();

  return saveToFFat(path);
}