#include "robotconfig.h"
#include "behaviors.h"
#include "websocket_client.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)
// ============================================================================

// ---- Helpers for reading/writing multi-byte values to file ----

static void writeU16(File& f, uint16_t v) {
  f.write((uint8_t)(v & 0xFF));
  f.write((uint8_t)((v >> 8) & 0xFF));
}

static uint16_t readU16(File& f) {
  uint16_t v = f.read();
  v |= ((uint16_t)f.read() << 8);
  return v;
}

static void writeU32(File& f, uint32_t v) {
  f.write((uint8_t)(v & 0xFF));
  f.write((uint8_t)((v >> 8) & 0xFF));
  f.write((uint8_t)((v >> 16) & 0xFF));
  f.write((uint8_t)((v >> 24) & 0xFF));
}

static uint32_t readU32(File& f) {
  uint32_t v = f.read();
  v |= ((uint32_t)f.read() << 8);
  v |= ((uint32_t)f.read() << 16);
  v |= ((uint32_t)f.read() << 24);
  return v;
}

static void writeFloat(File& f, float v) {
  uint32_t raw;
  memcpy(&raw, &v, 4);
  writeU32(f, raw);
}

static float readFloat(File& f) {
  uint32_t raw = readU32(f);
  float v;
  memcpy(&v, &raw, 4);
  return v;
}

static void writeStr(File& f, const char* s, uint8_t maxLen) {
  uint8_t len = strlen(s);
  if (len > maxLen) len = maxLen;
  f.write(len);
  f.write((const uint8_t*)s, len);
}

static void readStr(File& f, char* dst, uint8_t maxLen) {
  uint8_t len = f.read();
  if (len > maxLen) len = maxLen;
  f.readBytes(dst, len);
  dst[len] = '\0';
  // Skip extra bytes if stored length exceeded maxLen
}

bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) 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++;
    }
  }

  // Setting 7: Focus Behavior Settings
  if (focusBehavior) {
    const FocusSettings& fs = focusBehavior->getSettings();
    
    file.write((uint8_t)(KEY_FOCUS_SETTINGS & 0xFF));
    file.write((uint8_t)((KEY_FOCUS_SETTINGS >> 8) & 0xFF));
    file.write(TYPE_FOCUS_SETTINGS);
    
    // Motor IDs
    file.write(fs.eyeMotor1);
    file.write(fs.eyeMotor2);
    file.write(fs.neckMotor);
    
    // Eye servo range
    writeU16(file, fs.eyeCenter);
    writeU16(file, fs.eyeMin);
    writeU16(file, fs.eyeMax);
    
    // Neck servo range
    writeU16(file, fs.neckCenter);
    writeU16(file, fs.neckMin);
    writeU16(file, fs.neckMax);
    
    // Face x range
    writeFloat(file, fs.faceXMin);
    writeFloat(file, fs.faceXMax);
    
    // Interpolation speeds
    writeFloat(file, fs.eyeSpeed);
    writeFloat(file, fs.neckSpeed);
    writeFloat(file, fs.eyeReturnSpeed);
    
    // Neck delay
    writeU32(file, (uint32_t)fs.neckDelayMs);
    
    // Neck contribution
    writeFloat(file, fs.neckContribution);
    
    // Neck invert
    file.write(fs.neckInvert ? 1 : 0);
    
    // Centering speeds
    writeFloat(file, fs.eyeCenteringSpeed);
    writeFloat(file, fs.neckCenteringSpeed);
    
    settingCount++;
  }

  // Setting 8: WiFi / WebSocket Settings
  {
    file.write((uint8_t)(KEY_WIFI_SETTINGS & 0xFF));
    file.write((uint8_t)((KEY_WIFI_SETTINGS >> 8) & 0xFF));
    file.write(TYPE_WIFI_SETTINGS);
    
    writeStr(file, wifiSettings.ssid, 32);
    writeStr(file, wifiSettings.password, 64);
    writeStr(file, wifiSettings.host, 63);
    writeU16(file, wifiSettings.port);
    writeStr(file, wifiSettings.path, 31);
    
    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, FocusBehavior* focusBehavior) {
  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;
      }

      case KEY_FOCUS_SETTINGS: {
        if (type == TYPE_FOCUS_SETTINGS && focusBehavior) {
          FocusSettings& fs = focusBehavior->getSettings();
          
          // Motor IDs
          fs.eyeMotor1 = file.read();
          fs.eyeMotor2 = file.read();
          fs.neckMotor = file.read();
          
          // Eye servo range
          fs.eyeCenter = readU16(file);
          fs.eyeMin = readU16(file);
          fs.eyeMax = readU16(file);
          
          // Neck servo range
          fs.neckCenter = readU16(file);
          fs.neckMin = readU16(file);
          fs.neckMax = readU16(file);
          
          // Face x range
          fs.faceXMin = readFloat(file);
          fs.faceXMax = readFloat(file);
          
          // Interpolation speeds
          fs.eyeSpeed = readFloat(file);
          fs.neckSpeed = readFloat(file);
          fs.eyeReturnSpeed = readFloat(file);
          
          // Neck delay
          fs.neckDelayMs = (unsigned long)readU32(file);
          
          // Neck contribution
          fs.neckContribution = readFloat(file);
          
          // Neck invert
          fs.neckInvert = file.read() != 0;
          
          // Centering speeds
          fs.eyeCenteringSpeed = readFloat(file);
          fs.neckCenteringSpeed = readFloat(file);
          
          Serial.println("[Config] Focus settings loaded");
        } else {
          // Skip the focus settings blob (52 bytes) if no focusBehavior
          for (int k = 0; k < 52; k++) file.read();
        }
        break;
      }

      case KEY_WIFI_SETTINGS: {
        if (type == TYPE_WIFI_SETTINGS) {
          readStr(file, wifiSettings.ssid, 32);
          readStr(file, wifiSettings.password, 64);
          readStr(file, wifiSettings.host, 63);
          wifiSettings.port = readU16(file);
          readStr(file, wifiSettings.path, 31);
          
          Serial.println("[Config] WiFi settings loaded");
        } else {
          // Skip: 5 length-prefixed strings + 2 byte port - can't know exact size
          // Best effort: skip based on stored lengths
          for (int s = 0; s < 3; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); }
          file.read(); file.read(); // port
          for (int s = 0; s < 2; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); }
        }
        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, FocusBehavior* focusBehavior) {
  if (FFat.exists(path)) {
    Serial.println("Loading robot config from FFat...");
    // Try V2 format first
    if (loadFromFFatV2(path, behaviorManager, visemeBehavior, focusBehavior)) {
      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, focusBehavior);
      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, focusBehavior);
}