const fs = require("fs");
const i2c = require("i2c-bus");
const { WebSocketServer } = require("ws");
const { sleep } = require("./unit");
const logger = require("./logger");
const status = require("./status");


const ICM20948_ADDR = 0x68;
const AK09916_ADDR = 0x0c;

const REG_BANK_SEL = 0x7f;
const BANK_0 = 0x00;

const REG_PWR_MGMT_1 = 0x06;
const REG_INT_PIN_CFG = 0x0f;

const AK_WIA2 = 0x01;
const AK_CNTL2 = 0x31;
const AK_CNTL3 = 0x32;
const AK_ST1 = 0x10;
const AK_HXL = 0x11;
const AK_ST2 = 0x18;

module.exports = class CompassServer {
  constructor({ server, broadcast }) {
    this.broadcast = broadcast;
    this.intervalMs = Number(process.env.NETWORK_RC_COMPASS_INTERVAL || 200);
    this.bus = null;
    this.running = false;
    this.clients = new Set();
    /** 是否检测到罗盘硬件，未检测到时不再尝试读取、不刷错误日志 */
    this.compassAvailable = true;

    logger.info("罗盘服务启动中...");

    if (server) {
      const path = `/compass`;
      const wss = new WebSocketServer({ noServer: true, path }, () => {
        logger.info("罗盘 websocket 服务已启动");
      });

      server.on("upgrade", (request, socket, head) => {
        if (request.url === path)
          wss.handleUpgrade(request, socket, head, (ws) => {
            wss.emit("connection", ws, request);
          });
      });

      wss.on("connection", (socket) => {
        this.clients.add(socket);
        socket.on("close", () => this.clients.delete(socket));
      });

      this.wss = wss;
    }


    if (!fs.existsSync("/dev/i2c-1")) {
      logger.warn("/dev/i2c-1 不存在，跳过罗盘初始化");
      return;
    }

    try {
      this.bus = i2c.openSync(1);
      this.running = true;
      this.initCompass();
      if (!this.compassAvailable) {
        logger.info("未检测到罗盘硬件，罗盘服务已禁用");
        this.loop(); // 只做空转，不读罗盘
        return;
      }
      this.loop();
      logger.info("罗盘服务启动成功");
    } catch (err) {
      logger.error("罗盘服务启动失败: " + err.message);
    }
  }

  selectBank(bank) {
    this.bus.writeByteSync(ICM20948_ADDR, REG_BANK_SEL, (bank & 0x03) << 4);
  }

  writeByte(addr, reg, value) {
    this.bus.writeByteSync(addr, reg, value);
  }

  readByte(addr, reg) {
    return this.bus.readByteSync(addr, reg);
  }

  initCompass() {
    try {
      this.selectBank(BANK_0);
      this.writeByte(ICM20948_ADDR, REG_PWR_MGMT_1, 0x01);
      this.writeByte(ICM20948_ADDR, REG_INT_PIN_CFG, 0x02);

      const wia2 = this.readByte(AK09916_ADDR, AK_WIA2);
      if (wia2 !== 0x09) {
        logger.warn(`AK09916 WIA2=0x${wia2.toString(16)}，可能未连接，已禁用罗盘服务`);
        this.compassAvailable = false;
        return;
      }

      this.writeByte(AK09916_ADDR, AK_CNTL3, 0x01);
    } catch (err) {
      logger.error("罗盘初始化失败: " + err.message + "，已禁用罗盘服务");
      this.compassAvailable = false;
    }
  }

  async setupMag() {
    try {
      await sleep(10);
      this.writeByte(AK09916_ADDR, AK_CNTL2, 0x08);
    } catch (err) {
      logger.error("罗盘配置失败: " + err.message + "，已禁用罗盘服务");
      this.compassAvailable = false;
    }
  }

  readMag() {
    try {
      const st1 = this.readByte(AK09916_ADDR, AK_ST1);
      if ((st1 & 0x01) === 0) return null;

      const buf = Buffer.alloc(8);
      this.bus.readI2cBlockSync(AK09916_ADDR, AK_HXL, 8, buf);

      const overflow = (buf[7] & 0x08) !== 0;
      if (overflow) return null;

      const x = (buf[1] << 8) | buf[0];
      const y = (buf[3] << 8) | buf[2];
      const z = (buf[5] << 8) | buf[4];

      const nx = x >= 0x8000 ? x - 0x10000 : x;
      const ny = y >= 0x8000 ? y - 0x10000 : y;
      const nz = z >= 0x8000 ? z - 0x10000 : z;

      return { x: nx, y: ny, z: nz };
    } catch (err) {
      if (this.compassAvailable) {
        this.compassAvailable = false;
        logger.warn("罗盘读取失败，已禁用罗盘服务: " + err.message);
      }
      return null;
    }
  }

  calcHeading(x, y) {
    let heading = (Math.atan2(y, x) * 180) / Math.PI;
    if (heading < 0) heading += 360;
    return Number(heading.toFixed(1));
  }

  sendCompassData(payload) {
    if (this.broadcast) {
      this.broadcast("compass data", payload);
    }

    this.clients.forEach((client) => {
      if (client.readyState === client.OPEN) {
        client.send(JSON.stringify({ type: "compass data", payload }));
      }
    });
  }

  async loop() {
    if (this.compassAvailable) {
      await this.setupMag();
    }

    while (this.running) {
      if (!this.compassAvailable || !status.config.compassEnabled) {
        await sleep(this.intervalMs);
        continue;
      }

      const mag = this.readMag();
      if (mag) {
        const heading = this.calcHeading(mag.x, mag.y);
        this.sendCompassData({
          heading,
          pitch: 0,
          roll: 0,
          isConnected: true,
          raw: mag,
        });
      }

      await sleep(this.intervalMs);
    }
  }

};
