#include "robotconfig.h"
#include "behaviors.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;
}

uint16_t RobotConfig::getMotorCurrent(uint8_t motorID) const {
  for (const Motor& motor : motors) {
    if (motor.motorID == motorID) {
      return motor.current;
    }
  }
  return 0;
}

uint8_t RobotConfig::getMotorModel(uint8_t motorID) {
  for (const Motor& motor : motors) {
    if (motor.motorID == motorID) {
      return motor.servoModel.major;
    }
  }
  // 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::setMotorCurrent(uint8_t motorID, uint16_t newCurrent) {
  for (Motor& motor : motors) {
    if (motor.motorID == motorID) {
      motor.current = newCurrent;
      return true;
    }
  }
  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.major);
    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]);
    }

    buffer.push_back(m.servoModel.major);
    buffer.push_back(m.servoModel.minor);

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

// Legacy method - calls new version with null pointers
bool RobotConfig::loadOrCreateDefault(const char* path) {
  return loadOrCreateDefault(path, nullptr, nullptr);
}

// ============================================================================
// Key-Value Format V2 (Compact, Extensible)
// ============================================================================

bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) const {
  File file = FFat.open(path, FILE_WRITE);
  if (!file) return false;

  // Write version header
  file.write((uint8_t)0x02);  // Version 2

  // Count settings (we'll write this after we know the count)
  uint16_t settingCount = 0;
  size_t countPos = file.position();
  file.write((uint8_t)0);  // Placeholder for count low byte
  file.write((uint8_t)0);  // Placeholder for count high byte

  // Setting 1: Device Name
  if (deviceName.length() > 0) {
    file.write((uint8_t)(KEY_DEVICE_NAME & 0xFF));
    file.write((uint8_t)((KEY_DEVICE_NAME >> 8) & 0xFF));
    file.write(TYPE_STRING);
    uint8_t nameLen = deviceName.length();
    file.write(nameLen);
    file.write((const uint8_t*)deviceName.c_str(), nameLen);
    settingCount++;
  }

  // Setting 2: Firmware Major
  file.write((uint8_t)(KEY_FIRMWARE_MAJOR & 0xFF));
  file.write((uint8_t)((KEY_FIRMWARE_MAJOR >> 8) & 0xFF));
  file.write(TYPE_UINT8);
  file.write(firmwareVersion.major);
  settingCount++;

  // Setting 3: Firmware Minor
  file.write((uint8_t)(KEY_FIRMWARE_MINOR & 0xFF));
  file.write((uint8_t)((KEY_FIRMWARE_MINOR >> 8) & 0xFF));
  file.write(TYPE_UINT8);
  file.write(firmwareVersion.minor);
  settingCount++;

  // Setting 4: Motor Array
  if (!motors.empty()) {
    file.write((uint8_t)(KEY_MOTOR_ARRAY & 0xFF));
    file.write((uint8_t)((KEY_MOTOR_ARRAY >> 8) & 0xFF));
    file.write(TYPE_MOTOR_ARRAY);
    
    uint8_t motorCount = motors.size();
    file.write(motorCount);
    
    for (const Motor& m : motors) {
      file.write(m.motorID);
      file.write(m.servoModel.major);
      file.write(m.servoModel.minor);
      
      uint8_t nameLen = m.name.length();
      file.write(nameLen);
      if (nameLen > 0) {
        file.write((const uint8_t*)m.name.c_str(), nameLen);
      }
    }
    settingCount++;
  }

  // Setting 5: Behavior States
  if (behaviorManager) {
    file.write((uint8_t)(KEY_BEHAVIOR_STATES & 0xFF));
    file.write((uint8_t)((KEY_BEHAVIOR_STATES >> 8) & 0xFF));
    file.write(TYPE_BEHAVIOR_STATES);
    
    // Count enabled behaviors
    uint8_t behaviorCount = behaviorManager->getBehaviorCount();
    file.write(behaviorCount);
    
    // Write behaviorID + enabled state pairs
    for (uint8_t i = 0; i < behaviorCount; i++) {
      BehaviorID id;
      bool enabled;
      if (behaviorManager->getBehaviorInfo(i, id, enabled)) {
        file.write((uint8_t)id);
        file.write(enabled ? 1 : 0);
      }
    }
    settingCount++;
  }

  // Setting 6: Viseme Array
  if (visemeBehavior) {
    const std::vector<Viseme>& visemes = visemeBehavior->getVisemes();
    if (!visemes.empty()) {
      file.write((uint8_t)(KEY_VISEME_ARRAY & 0xFF));
      file.write((uint8_t)((KEY_VISEME_ARRAY >> 8) & 0xFF));
      file.write(TYPE_VISEME_ARRAY);
      
      uint8_t visemeCount = visemes.size();
      file.write(visemeCount);
      
      for (const Viseme& v : visemes) {
        file.write(v.id);
        
        // Label (3 bytes)
        file.write((uint8_t)v.label[0]);
        file.write((uint8_t)v.label[1]);
        file.write((uint8_t)v.label[2]);
        
        // Motor positions
        uint8_t motorCount = v.motorPositions.size();
        file.write(motorCount);
        
        for (const VisemeMotorPosition& mp : v.motorPositions) {
          file.write(mp.motorID);
          file.write((uint8_t)(mp.position & 0xFF));         // position low
          file.write((uint8_t)((mp.position >> 8) & 0xFF));  // position high
        }
      }
      settingCount++;
    }
  }

  // Write setting count at the beginning
  size_t endPos = file.position();
  file.seek(countPos);
  file.write((uint8_t)(settingCount & 0xFF));
  file.write((uint8_t)((settingCount >> 8) & 0xFF));
  file.seek(endPos);

  file.close();
  return true;
}

bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
  File file = FFat.open(path, FILE_READ);
  if (!file) return false;

  // Read version
  uint8_t version = file.read();
  if (version != 0x02) {
    file.close();
    return false;  // Wrong version
  }

  // Read setting count
  uint16_t settingCount = file.read();
  settingCount |= (file.read() << 8);

  motors.clear();

  // Parse each setting
  for (uint16_t i = 0; i < settingCount; i++) {
    // Read key
    uint16_t key = file.read();
    key |= (file.read() << 8);
    
    // Read type
    uint8_t type = file.read();

    switch (key) {
      case KEY_DEVICE_NAME: {
        if (type == TYPE_STRING) {
          uint8_t nameLen = file.read();
          char nameBuf[64] = {0};
          if (nameLen > 0 && nameLen < sizeof(nameBuf)) {
            file.readBytes(nameBuf, nameLen);
            deviceName = String(nameBuf);
          }
        }
        break;
      }

      case KEY_FIRMWARE_MAJOR: {
        if (type == TYPE_UINT8) {
          firmwareVersion.major = file.read();
        }
        break;
      }

      case KEY_FIRMWARE_MINOR: {
        if (type == TYPE_UINT8) {
          firmwareVersion.minor = file.read();
        }
        break;
      }

      case KEY_MOTOR_ARRAY: {
        if (type == TYPE_MOTOR_ARRAY) {
          uint8_t motorCount = file.read();
          
          for (uint8_t j = 0; j < motorCount; j++) {
            Motor m;
            m.motorID = file.read();
            m.servoModel.major = file.read();
            m.servoModel.minor = file.read();
            
            uint8_t nameLen = file.read();
            char nameBuf[64] = {0};
            if (nameLen > 0 && nameLen < sizeof(nameBuf)) {
              file.readBytes(nameBuf, nameLen);
              m.name = String(nameBuf);
            }
            
            m.position = 0;
            m.isEnabled = true;
            motors.push_back(m);
          }
        }
        break;
      }

      case KEY_BEHAVIOR_STATES: {
        if (type == TYPE_BEHAVIOR_STATES && behaviorManager) {
          uint8_t behaviorCount = file.read();
          
          for (uint8_t j = 0; j < behaviorCount; j++) {
            BehaviorID id = (BehaviorID)file.read();
            bool enabled = file.read() != 0;
            behaviorManager->setBehaviorEnabled(id, enabled);
          }
        }
        break;
      }

      case KEY_VISEME_ARRAY: {
        if (type == TYPE_VISEME_ARRAY && visemeBehavior) {
          uint8_t visemeCount = file.read();
          
          for (uint8_t j = 0; j < visemeCount; j++) {
            uint8_t visemeID = file.read();
            
            // Read label (3 bytes)
            char label[4];
            label[0] = file.read();
            label[1] = file.read();
            label[2] = file.read();
            label[3] = '\0';
            
            // Read motor positions
            uint8_t motorCount = file.read();
            std::vector<VisemeMotorPosition> positions;
            
            for (uint8_t k = 0; k < motorCount; k++) {
              VisemeMotorPosition mp;
              mp.motorID = file.read();
              uint16_t posLow = file.read();
              uint16_t posHigh = file.read();
              mp.position = posLow | (posHigh << 8);
              positions.push_back(mp);
            }
            
            // Create or update viseme
            visemeBehavior->createOrUpdateViseme(visemeID, label, positions);
          }
        }
        break;
      }

      default:
        // Unknown key - skip based on type
        switch (type) {
          case TYPE_UINT8: file.read(); break;
          case TYPE_UINT16: file.read(); file.read(); break;
          case TYPE_UINT32: file.read(); file.read(); file.read(); file.read(); break;
          case TYPE_BOOL: file.read(); break;
          case TYPE_INT8: file.read(); break;
          case TYPE_INT16: file.read(); file.read(); break;
          case TYPE_INT32: file.read(); file.read(); file.read(); file.read(); break;
          case TYPE_FLOAT: file.read(); file.read(); file.read(); file.read(); break;
          case TYPE_STRING: {
            uint8_t len = file.read();
            for (uint8_t k = 0; k < len; k++) file.read();
            break;
          }
          default:
            file.close();
            return false;  // Unknown type
        }
        break;
    }
  }

  file.close();
  return true;
}

bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
  if (FFat.exists(path)) {
    Serial.println("Loading robot config from FFat...");
    // Try V2 format first
    if (loadFromFFatV2(path, behaviorManager, visemeBehavior)) {
      Serial.println("Loaded V2 format");
      return true;
    }
    // Fall back to V1 format
    if (loadFromFFat(path)) {
      Serial.println("Loaded V1 format (legacy)");
      // Upgrade to V2 format
      saveToFFatV2(path, behaviorManager, visemeBehavior);
      return true;
    }
  }

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

  // 🔧 Define your default config here
  deviceName = "DefaultBot";
  firmwareVersion = { 1, 0 };
  motors.clear();

  return saveToFFatV2(path, behaviorManager, visemeBehavior);
}