#include "FoodDeliveryBase.h"
/**
* @description: 初始化
*/
FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char* userUdpServerIP, uint32_t userUdpServerPort) {
  /*初始化*/
  ssid = userSsid;                    //wifi帐号
  password = userPassword;            //wifi密码
  mqttServer = userMqttServer;        //mqtt服务器地址
  mqttPort = userMqttPort;            //mqtt服务器端口
  mqttName = userMqttName;            //mqtt帐号
  mqttPassword = userMqttPassword;    //mqtt密码
  mavlinkSerial = userMavlinkSerial;  //mavlink用的串口
  voiceSerial = userVoiceSerial;      //声音模块用的串口
  udpServerIP = userUdpServerIP;      //云台相机ip
  udpServerPort = userUdpServerPort;  //云台相机端口
  wificonnected=false;
  _tm_mqttconnect=0;

  //初始化飞控通讯串口 波特率
  switch (mavlinkSerial) {  //初始化指定 串口号
    case 0:
      Serial.begin(57600);
      break;
    case 1:
      Serial1.begin(57600);
      break;
    case 2:
      Serial2.begin(57600);
      break;
  }
  //初始化声音模块串口 波特率
  switch (voiceSerial) {  //初始化指定 串口号
    case 0:
      Serial.begin(115200);
      break;
    case 1:
      Serial1.begin(115200);
      break;
    case 2:
      Serial2.begin(115200);
      break;
  }
  /*初始化 连接mqtt对象 赋予mqttClient指针*/
  mqttClient = new PubSubClient(mqttServer, mqttPort, wifiClient);
}
/**
* @description: 日志打印
* @param {*} val 输出的信息
*/
void FoodCube::log(String val) {
  Serial.print(val);
}
void FoodCube::log(char* val) {
  Serial.print(val);
}
void FoodCube::log(int val) {
  Serial.print(val);
}
void FoodCube::log(IPAddress val) {
  Serial.print(val);
}
void FoodCube::log(bool val) {
  Serial.print(val);
}
void FoodCube::logln(String val) {
  Serial.println(val);
}
void FoodCube::logln(char* val) {
  Serial.println(val);
}
void FoodCube::logln(int val) {
  Serial.println(val);
}
void FoodCube::logln(IPAddress val) {
  Serial.println(val);
}
void FoodCube::logln(bool val) {
  Serial.print(val);
}

/**
*@description: 取值 设置值
*/
bool FoodCube::getIsInit() {
  return isInit;
}
void FoodCube::setIsInit(bool b) {
  isInit = b;
}
String FoodCube::getMacAdd() {
  return macAdd;
}

/**
* @description: 连接wifi
*/
// 设置静态IP信息（配置信息前需要对将要接入的wifi网段有了解）
IPAddress local_IP(192, 168, 8, 201);
// 设置静态IP网关
IPAddress gateway(192, 168, 8, 1);

IPAddress subnet(255, 255, 255, 0);
IPAddress primaryDNS(8, 8, 8, 8);   //optional
IPAddress secondaryDNS(8, 8, 4, 4); //optional

void FoodCube::connectWifi() {
  
  if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) {
    Serial.println("STA Failed to configure");
  }
  
  //设置wifi帐号密码
  WiFi.begin(ssid, password);
  //连接wifi
  logln("Connecting Wifi...");
  /*
  while (WiFi.status() != WL_CONNECTED) {
    log(".");
    delay(150);
  }
  //获取局域网ip
  logln("");
  logln("WiFi connected");
  log("IP address: ");
  logln(WiFi.localIP());
  localIp = WiFi.localIP();
  //设置开发板为无线终端 获取物理mac地址
  WiFi.mode(WIFI_STA);
  macAdd = WiFi.macAddress();
  macAdd.replace(":", "");  //板子的物理地址 并且去除冒号
  log("macAdd: ");
  logln(macAdd);
  playText("[v1]网络连接成功");
  delay(500);
  */
}

bool FoodCube::checkWiFiStatus()
{
  if(!wificonnected&&(WiFi.status() == WL_CONNECTED))
  {
    wificonnected=true;
    //获取局域网ip
    logln("");
    logln("WiFi connected");
    log("IP address: ");
    logln(WiFi.localIP());
    localIp = WiFi.localIP();
    //设置开发板为无线终端 获取物理mac地址
    WiFi.mode(WIFI_STA);
    macAdd = WiFi.macAddress();
    macAdd.replace(":", "");  //板子的物理地址 并且去除冒号
    log("macAdd: ");
    logln(macAdd);
    playText("[v1]网络连接成功");
  }
  return wificonnected;
}
/**
* @description: 连接mqtt
* @param {String[]} topicSub 主题数组
* @param {int} topicSubCount 数组总数
*/
void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
  if (mqttClient->connected()) return;
  /*尝试连接mqtt*/
  if ((millis() - _tm_mqttconnect)>3000)
  {
    logln("connect_mqtt");
    if (mqttClient->connect(macAdd.c_str(), mqttName, mqttPassword)) {
      logln("MQTT Server Connected.");
      log("Server Address: ");
      logln(mqttServer);
      log("ClientId :");
      logln(macAdd);
      /*连接成功 订阅主题*/
      //订阅主题 PS:连接上mqtt服务器 订阅主题 收到信息触发回调函数 receiveCallback
      for (int i = 0; i < topicSubCount; i++) {  //遍历 订阅主题
        subscribeTopic(topicSub[i], 1);
      }
      delay(500);
      playText("[v1]指令服务器已连接");
    } else {
      //失败返回状态码
      log("MQTT Server Connect Failed. Client State:");
      logln(mqttClient->state());
      _tm_mqttconnect=millis();

      //delay(3000);
    }
  }
}

/**
* @description: 写在loop函数里 检测mqtt连接情况 并保持心跳
* @param {String[]} topicSub 主题数组
* @param {int} topicSubCount 数组总数
*/
void FoodCube::mqttLoop(String topicSub[], int topicSubCount) {
  if (mqttClient->connected()) {           //检测 如果开发板成功连接服务器
    mqttClient->loop();                    // 保持心跳
  } else {                                 // 如果开发板未能成功连接服务器
    connectMqtt(topicSub, topicSubCount);  // 则尝试连接服务器
  }
}

/**
* @description: 订阅指定主题
* @param {String} topicString 主题名称
* @param {int} Qos 1：重要 ps:响应时间快
*/
void FoodCube::subscribeTopic(String topicString, int Qos = 1) {
  //处理主题字符串
  topicString = topicString + "/" + macAdd;
  char subTopic[topicString.length() + 1];
  strcpy(subTopic, topicString.c_str());
  //订阅主题
  mqttClient->subscribe(subTopic, Qos);
}

/**
* @description: 向指定主题 发布信息
* @param {String} topicString 主题名称
* @param {String} messageString 发布的内容
*/
void FoodCube::pubMQTTmsg(String topicString, String messageString) {
  //处理主题字符串
  topicString = topicString + "/" + macAdd;
  char publishTopic[topicString.length() + 1];
  strcpy(publishTopic, topicString.c_str());
  //处理发布内容字符串
  char publishMsg[messageString.length() + 1];
  strcpy(publishMsg, messageString.c_str());
  //向指定主题 发布信息
  mqttClient->publish(publishTopic, publishMsg);
}

/**
* @description: 判断并向指定串口 发送命令帧
* @param {uint8_t[]} buf[]  命令帧 字节数组
* @param {int} len 命令帧 的长度
* @param {uint8_t} swSerial 串口选择
*/
void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) {
  //通过串口发送
  switch (swSerial) {  //初始化指定 串口号
    case 0:
      Serial.write(buf, len);
      break;
    case 1:
      Serial1.write(buf, len);
      break;
    case 2:
      Serial2.write(buf, len);
      break;
  }
}


/**
* @description: 声音播放
* @param {String} str 播放内容
*/
void FoodCube::playText(String str) {
  /*消息长度*/
  int len = str.length();  //修改信息长度 消息长度
  // if (len >= 3996) {       //限制信息长度 超长的不播放
  //   return;
  // }
  /*长度高低位*/
  int frameLength = len + 2;                  // 5 bytes for header and length bytes
  byte highByte = (frameLength >> 8) & 0xFF;  // extract high byte of length
  byte lowByte = frameLength & 0xFF;          // extract low byte of length
  /*帧命令头*/
  uint8_t command[len + 5];
  //已知 帧头0xFD z信息长度高位 x信息长度低位 0x01命令字1是开始命令 0x04utf8编码 先给值
  int index = 0;
  command[index++] = 0xFD;
  command[index++] = highByte;
  command[index++] = lowByte;
  command[index++] = 0x01;
  command[index++] = 0x04;
  /*帧命令 消息段*/
  for (int i = 0; i < str.length(); i++) {
    command[index++] = (int)str[i];
  }
  for (int i = 0; i < sizeof(command); i++) {
    log(command[i]);
    log(" ");
  }
  logln("");
  //串口发送 播放声音
  SWrite(command, sizeof(command), voiceSerial);
}

/**
* @description: 检查声音模块是否空闲
* @return {uint8_t} 0x4E 78:系统忙  0x4F 79:系统空闲
*/
uint8_t FoodCube::chekVoiceMcu() {
  uint8_t serialData;
  uint8_t check[] = { 0xFD, 0x00, 0x01, 0x21 };  //检查命令帧
  SWrite(check, sizeof(check), voiceSerial);     //发送检查指令
  switch (voiceSerial) {
    case 0:
      {
        while (Serial.available()) {   // 当串口接收到信息后
          serialData = Serial.read();  // 将接收到的信息使用read读取
        }
      }
      break;
    case 1:
      {
        while (Serial1.available()) {   // 当串口接收到信息后
          serialData = Serial1.read();  // 将接收到的信息使用read读取
        }
      }
      break;
    case 2:
      {
        while (Serial2.available()) {   // 当串口接收到信息后
          serialData = Serial2.read();  // 将接收到的信息使用read读取
        }
      }
      break;
  }

  return serialData;
}

/**
* @description: 声音模块停止合成
*/
void FoodCube::stopVoice() {
  uint8_t stop[] = { 0xFD, 0x00, 0x01, 0x22 };  //停止合成命令帧
  SWrite(stop, sizeof(stop), voiceSerial);      //发送检查指令
}

/**
* @description: 相机控制命令帧 校检码
* @param {uint8_t[]} 命令帧 数组
* @param {int} len 命令帧 的长度
*/
uint16_t FoodCube::CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init) {
  uint16_t crc, oldcrc16;
  uint8_t temp;
  crc = crc_init;
  while (len-- != 0) {
    temp = (crc >> 8) & 0xff;
    oldcrc16 = crc16_tab[*ptr ^ temp];
    crc = (crc << 8) ^ oldcrc16;
    ptr++;
  }
  return (crc);
}

void FoodCube::crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result) {
  uint16_t crc_result = 0;
  crc_result = CRC16_cal(pbuf, len, 0);
  *p_result = crc_result;
}
/**
* @description: 相机
* @param {uint8_t[]} 命令帧 数组
* @param {int} len 命令帧 的长度
*/
void FoodCube::udpSendToCamera(uint8_t* p_command, uint32_t len) {
  uint16_t result = 0;
  uint16_t* p_result = &result;
  //计算校检码
  crc_check_16bites(p_command, len, p_result);
  //加上校检码
  uint8_t bytes[2 + len];
  for (int i = 0; i < len; i++) {
    bytes[i] = p_command[i];
  }
  bytes[len] = static_cast<uint8_t>(result);           // 低位 互换
  bytes[len + 1] = static_cast<uint8_t>(result >> 8);  // 高位 互换
  
  /*  //发送日志
  printf("SendToCamera:");
  for (int i=0;i<len+2;i++)
     printf("0x%02X,", bytes[i]);
  printf("\n");
  */
  //udp发送命令帧
  udp.beginPacket(udpServerIP, udpServerPort);
  udp.write(bytes, len + 2);
  udp.endPacket();
}

/**
* @description: 按位设置 1 or 0
* @param {String} str 要设置的值
* @param {uint8_t} n 第几位 从0开始
* @param {uint8_t} isOne 1:设置成1 0:设置成0
*/
String FoodCube::setNBit(String str, uint8_t n, uint8_t i) {
  char buf[10];
  uint16_t val = (uint8_t)str.toInt();
  if (i) {
    val |= (1u << n);  //按位设置成1
  } else {
    val &= ~(1u << n);  //按位设置成0
  }
  sprintf(buf, "%d", val);
  return buf;
}



/**
 * @description: 向飞控请求 指定数据
 */
void FoodCube::mav_request_data() {
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  // 以下注释 把数据流组合到一起
  /*
   * MAV_DATA_STREAM_ALL=0, // 获取所有类别 数据流
   * MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
   * MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS
   * MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW
   * MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.
   * MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.
   * MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot
   * MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot
   * MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot
   * MAV_DATA_STREAM_ENUM_END=13,
   */
  // 根据从Pixhawk请求的所需信息进行设置
  const int maxStreams = 1;                                        // 遍历次数 （下面组合的长度）
  const uint8_t MAVStreams[maxStreams] = { MAV_DATA_STREAM_ALL };  // 请求的数据流组合 放到一个对象 后面进行遍历
  const uint16_t MAVRates[maxStreams] = { 0x01 };                  // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹  既每秒发送一次
  for (int i = 0; i < maxStreams; i++) {
    // 向飞控发送请求
    mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
    int len = mavlink_msg_to_send_buffer(buf, &msg);
    SWrite(buf, len, mavlinkSerial);
  }
 // printf("send_mav\n");    
}

/**
 * @description: 解析mavlink数据流
 * @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调
 */
void FoodCube::comm_receive(void (*pFun)( uint8_t)) {
  mavlink_message_t msg;
  mavlink_status_t status;
  switch (mavlinkSerial) {
    case 0:
      while (Serial.available() > 0) {  // 判断串口缓存 是否有数据
        uint8_t c = Serial.read();      // 从缓存拿到数据
        pFun( c);
      }
      break;
    case 1:
      while (Serial1.available() > 0) {  // 判断串口缓存 是否有数据
        uint8_t c = Serial1.read();      // 从缓存拿到数据
        pFun(c);
      }
      break;
    case 2:
      while (Serial2.available() > 0) {  // 判断串口缓存 是否有数据
        uint8_t c = Serial2.read();      // 从缓存拿到数据
        pFun(c);
      }
      break;
  }
}

/**
 * @description: 写入航点 第一步 “向飞控发送航点数量”
 * @param {uint8_t} taskcount 航点数量
 */
void FoodCube::mav_mission_count(uint8_t taskcount) {
  mavlink_message_t msg;                //mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
  //设置飞行模式的数据包
  mavlink_msg_mission_count_pack(0xFF, 0xBE, &msg, 1, 0, taskcount);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  //通过串口发送
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @description: 等待飞控反馈之后 写入指定航点
 * @param {uint8_t} seq 航点序列
 * @param {uint8_t} frame 通常第一个是home航点0 绝对海拔  后面的固定的全是 3 相对高度
 * @param {uint8_t} command 航点的类型 如：16 是waypoint模式
 * @param {uint8_t} current 固定给0
 * @param {uint8_t} autocontinue 是否自动飞下一个航点 
 * @param {double} param1
 * @param {double} param2
 * @param {double} param3
 * @param {double} param4
 * @param {double} x
 * @param {double} y
 * @param {double} z
 */
void FoodCube::mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z) {
  mavlink_message_t msg;                //mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
  //写入航点数据包
  mavlink_msg_mission_item_pack(0xFF, 0xBE, &msg, 1, 0, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  //通过串口发送
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @description: 向飞控发送 设置模式指令
 */
void FoodCube::mav_set_mode(uint8_t SysState) {
  mavlink_message_t msg;                //mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
  //设置飞行模式的数据包
  mavlink_msg_set_mode_pack(0xFF, 0xBE, &msg, 1, 209, SysState);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  //通过串口发送
  SWrite(buf, len, mavlinkSerial);
}
void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd) {
  mavlink_message_t msg;                //mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
  mavlink_msg_command_long_encode(0xFF, 0xBE, &msg, &msg_cmd);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  //通过串口发送
  SWrite(buf, len, mavlinkSerial);
}

/**
 * @description: 飞控 控制
 * @param {uint8_t} controlType 3:加解锁模式控制
 * @param {uint16_t} param[] 
 */
void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
  mavlink_message_t msg;                //mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
  //设置飞行模式的数据包
  mavlink_command_long_t cmd;
  cmd.target_system = 1;
  cmd.target_component = 1;
  cmd.confirmation = 0;
  if (controlType == 3) {  //飞机加解锁
    cmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
    cmd.param1 = param[0];  // 0:加锁 1：解锁
    cmd.param2 = param[1];  //0:符合条件加解锁  21196:强制加解锁
  }
  if (controlType == 6) {  //测试起飞
    float p = (float)param[0];
    cmd.command = MAV_CMD_NAV_TAKEOFF;
    cmd.param1 = 0;
    cmd.param2 = 0;
    cmd.param3 = 0;
    cmd.param4 = 0;
    cmd.param5 = 0;
    cmd.param6 = 0;
    cmd.param7 = p;  //起飞高度
  }
  if (controlType == 8) {  //降落
    cmd.command = MAV_CMD_NAV_LAND;
  }
  if (controlType == 11) {  //磁罗盘校准
    cmd.command = MAV_CMD_DO_START_MAG_CAL;
    cmd.param1 = 0;
    cmd.param2 = 1;
    cmd.param3 = 1;
  }
  mavlink_msg_command_long_encode(0xFF, 0xBE, &msg, &cmd);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  //通过串口发送
  SWrite(buf, len, mavlinkSerial);
}
/**
 * @description: 油门控制
 * @param {uint16_t} chan[] 四个通道
 */
void FoodCube::mav_channels_override(uint16_t chan[]) {
  mavlink_message_t msg;                //mavlink协议信息(msg)
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
  //控制油门
  mavlink_msg_rc_channels_override_pack(0xFF, 0xBE, &msg, 1, 1, chan[0], chan[1], chan[2], chan[3], 0xffff, 0xffff, 0xffff, 0xffff);
  int len = mavlink_msg_to_send_buffer(buf, &msg);
  //通过串口发送
  SWrite(buf, len, mavlinkSerial);
}