const mqtt = require("mqtt");
const _ = require("lodash")
const proj4 = require("proj4")
module.exports = (agent) => {
  agent.messenger.on("egg-ready", () => {
    const client = mqtt.connect("mqtt://192.168.1.102:1883", {
      // const client = mqtt.connect("mqtt://127.0.0.1:1883", {
      clean: true,
      connectTimeout: 4000,
      clientId: 'map-editor-' + Math.random().toString(16).substr(2),
      username: 'map-editor',
      password: 'leador',
    });
    client.on("connect", function () {
      console.log("agentmqtt服务器连接成功");
      //本系统
      client.subscribe(`/robot4inspection/fa49e558574df1ec/sensor/battery`, { qos: 0 }, (a, b, c) => {
      }); // 订阅电池电量
      client.subscribe(`/robot4inspection/fa49e558574df1ec/sensor/gps`, { qos: 0 }, (a, b, c) => {
      }); // 订阅位置信息
      client.subscribe(`/robot4inspection/fa49e558574df1ec/localization/pose`, { qos: 0 }, (a, b, c) => {
      }); // 订阅位姿信息
      client.subscribe(`/robot4inspection/fa49e558574df1ec/message/error`, { qos: 0 }, (a, b, c) => {
      }); // 订阅错误消息
      client.subscribe(`/robot4inspection/fa49e558574df1ec/message/warn`, { qos: 0 }, (a, b, c) => {
      }); // 订阅警告消息
      client.subscribe(`/robot4inspection/fa49e558574df1ec/message/info`, { qos: 0 }, (a, b, c) => {
      }); // 订阅日志消息
      client.subscribe(`/robot4inspection/fa49e558574df1ec/planning/service/plan/request`, { qos: 2 }, (a, b, c) => {
      }); // 订阅路径规划
      client.subscribe(`/robot4inspection/fa49e558574df1ec/planning/trajectory/2d/compact`, { qos: 2, retain: 0 }, (a, b, c) => {
      }); // 订阅当前行驶轨迹
      client.subscribe(`/robot4inspection/fa49e558574df1ec/task/target/action/goto`, { qos: 2, retain: 0 }, (a, b, c) => {
      }); // 订阅前往目标点
      //指控中心
      client.subscribe(`/Plan`, { qos: 2 }, (a, b, c) => {
      }); // 订阅生成任务
      client.subscribe(`/startPlan`, { qos: 0 }, (a, b, c) => {
      }); // 订阅开始任务
      client.subscribe(`/pausePlan`, { qos: 0 }, (a, b, c) => {
      }); // 订阅暂停任务
      client.subscribe(`/overPlan`, { qos: 0 }, (a, b, c) => {
      }); // 订阅取消任务
      client.subscribe(`/delPlan`, { qos: 0 }, (a, b, c) => {
      }); // 订阅删除任务
    });
    let current = {
      lon: 0,
      lat: 0,
      x: 0,
      y: 0,
    }
    const lonlatToxy = function (lon, lat) {
      let detx = Number(lon - current.lon) * 100000 + Number(current.x)
      let dety = Number(lat - current.lat) * 100000 + Number(current.y)
      return [detx, dety]
    }
    const batteryThrottle = _.throttle(function (message) {
      let resObj = JSON.parse(message.toString())
      let retMessage = resObj.args[0]
      let timestamp = resObj.timestamp
      let postMessagr = {
        message: {
          capacity: retMessage.capacity,
          charging: retMessage.charging,
          current: retMessage.current,
          temperature: retMessage.temperature,
          voltage: retMessage.voltage,
        },
        timestamp,
      }
      let messStr = JSON.stringify(postMessagr)
      // client.publish("/battery", messStr, {
      //   qos: 1,
      //   retain: true,
      // })
    }, 3000, { leading: true })
    const temperatureThrottle = _.throttle(function (message) {
      let resObj = JSON.parse(message.toString())
      let retMessage = resObj.args[0]
      let timestamp = resObj.timestamp
      let postMessagr = {
        message: {
          humidity: retMessage.humidity,
          env: retMessage.env,
          cpu: retMessage.cpu,
        },
        timestamp,
      }
      let messStr = JSON.stringify(postMessagr)
      // client.publish("/temperature", messStr, {
      //   qos: 1,
      //   retain: true,
      // })
    }, 3000, { leading: true })
    const gpsThrottle = _.throttle(function (message) {
      let resObj = JSON.parse(message.toString())
      let retMessage = resObj.args[0]
      let timestamp = resObj.timestamp
      let postMessagr = {
        message: {
          lat: retMessage.blh[0],
          lon: retMessage.blh[1],
          height: retMessage.blh[2],
          heading: retMessage.heading,
          speed: retMessage.speed,
          star: retMessage.star,
          status: retMessage.status,
        },
        timestamp,
      }
      let messStr = JSON.stringify(postMessagr)

      // client.publish("/location", messStr, {
      //   qos: 1,
      //   retain: true,
      // })

    }, 100, { leading: true })
    const poseThrottle = _.throttle(function (message) {
      let resObj = JSON.parse(message.toString())
      let retMessage = resObj.args[0]
      let timestamp = resObj.timestamp
      let postMessagr = {
        message: {
          confidence: retMessage.confidence,
          odometer: retMessage.odometer,
          poseB: retMessage.pose.blh[0],
          poseL: retMessage.pose.blh[1],
          poseH: retMessage.pose.blh[2],

          poseR: retMessage.pose.rpy[0],
          poseP: retMessage.pose.rpy[1],
          poseY: retMessage.pose.rpy[2],

          posex1: retMessage.pose.xyz[0],
          posey1: retMessage.pose.xyz[1],
          posez1: retMessage.pose.xyz[2],

          heading: retMessage.pose.heading,
          replan: retMessage.replan,
          star: retMessage.rtk.star,
          status: retMessage.rtk.status,
          run_state: retMessage.run_state,
          score: retMessage.score,
          sequence: retMessage.sequence,
          enu: retMessage.vel.enu,
          velheading: retMessage.vel.heading,
        },
        timestamp,
      }
      current = {
        lon: retMessage.pose.blh[1],
        lat: retMessage.pose.blh[0],
        x: retMessage.pose.xyz[0],
        y: retMessage.pose.xyz[1],
      }
      // client.publish("/pose", JSON.stringify(postMessagr), {
      //   qos: 1,
      //   retain: true,
      // })

    }, 100, { leading: true })
    const PlanThrottle = _.throttle(function (message) {
      let strJson = JSON.parse(message.toString()).data
      let taskName = JSON.parse(message.toString()).handlerId
      let mapName = JSON.parse(message.toString()).mapName
      console.log(6666, strJson, taskName, mapName)
      let params = {
        map: mapName || "test",
        path: taskName || "task1",
        repeate: _.get(strJson, ["repeate"], 1) || 1,
        coord_type: "local",
        cron: _.get(strJson, ["cron"], "") || "",
        coord: [],
        plan: [],
        "action": [
          []
        ],
      }
      let coord = []
      let plan = []
      if (_.get(strJson, ["type"], "xy") === "xy") {
        if (strJson.start_lon && strJson.start_lat) {
          coord = [[strJson.start_lon, strJson.start_lat]]
          plan = ["route"]
        }
        let pointSort = _.sortBy(strJson.way_points, "id")
        for (let index = 0; index < pointSort.length; index++) {
          let element = pointSort[index];
          coord.push([element.lon, element.lat])
          plan.push("route")
        }
        if (strJson.end_lon && strJson.end_lat) {
          coord.push([strJson.end_lon, strJson.end_lat])
          plan.push("route")
        }
      } else if (_.get(strJson, ["type"], "xy") === "lonlat") {
        if (strJson.start_lon && strJson.start_lat) {
          coord = [lonlatToxy(strJson.start_lon, strJson.start_lat)]
          plan = ["route"]
        }
        let pointSort = _.sortBy(strJson.way_points, "id")
        for (let index = 0; index < pointSort.length; index++) {
          let element = pointSort[index];
          coord.push(lonlatToxy(element.lon, element.lat))
          plan.push("route")
        }
        if (strJson.end_lon && strJson.end_lat) {
          coord.push(lonlatToxy(strJson.end_lon, strJson.end_lat))
          plan.push("route")
        }
      }
      params.coord = coord
      params.plan = plan
      agent.messenger.sendToApp('plan_action', params);
    }, 3000, { leading: true })
    client.on("message", async function (top, message) {
      if (_.endsWith(top, "/sensor/battery")) {
        batteryThrottle(message)
      } else if (_.endsWith(top, "/sensor/temperature")) {
        temperatureThrottle(message)
      } else if (_.endsWith(top, "/sensor/gps")) {
        gpsThrottle(message)
      } else if (_.endsWith(top, "/localization/pose")) {
        poseThrottle(message)
      } else if (_.endsWith(top, "/message/error")) {
        let msg = JSON.parse(message.toString());
        // client.publish("/errorMessage", JSON.stringify(msg), { qos: 1, })
      } else if (_.endsWith(top, "/message/warn")) {
        let msg = JSON.parse(message.toString());
        // client.publish("/warnMessage", JSON.stringify(msg), { qos: 1, })
      } else if (_.endsWith(top, "/message/info")) {
        let msg = JSON.parse(message.toString());
        // client.publish("/infoMessage", JSON.stringify(msg), { qos: 1, })
      } else if (_.endsWith(top, "/Plan")) {
        PlanThrottle(message)
      } else if (_.endsWith(top, "/startPlan")) {
        let strJson = JSON.parse(message.toString()).data
        let job = {
          args: [
            {
              roadmap: _.get(strJson, ["mapName"], "TestNJ1") || "TestNJ1",
              task: _.get(strJson, ["taskName"], "task1") || "task1",
            },
          ],
          timestamp: Date.now()
        }
        console.log(66661,job,strJson)
        // client.publish("/robot4inspection/fa49e558574df1ec/task/procedure/action/start", JSON.stringify(job), { qos: 2, retain: 0, })
      } else if (_.endsWith(top, "/pausePlan")) {
        let job = {
          args: null,
          timestamp: Date.now()
        }
        // client.publish("/robot4inspection/fa49e558574df1ec/task/procedure/action/pause", JSON.stringify(job), { qos: 1, retain: 0, })
      } else if (_.endsWith(top, "/overPlan")) {
        let job = {
          args: null,
          timestamp: Date.now()
        }
        // client.publish("/robot4inspection/fa49e558574df1ec/task/procedure/action/cancel", JSON.stringify(job), { qos: 2, retain: 0 })

      } else if (_.endsWith(top, "/delPlan")) {
        let strJson = JSON.parse(message.toString()).data
        let params = {
          map: _.get(strJson, ["mapName"], "TestNJ1") || "TestNJ1",
          path: _.get(strJson, ["taskName"], "task1") || "task1",
        }
        agent.messenger.sendToApp('plan_delete', params);
      } else if (_.endsWith(top, "/planning/service/plan/request")) {
        let msg = JSON.parse(message.toString());
      } else if (_.endsWith(top, "/trajectory/2d/compact")) {
        let msg = JSON.parse(message.toString());
      } else if (_.endsWith(top, "/task/target/action/goto")) {
        let msg = JSON.parse(message.toString());
      }
    });
  });
};