assistedDrive/entry/src/main/ets/utils/wireControl/WireControl.ets

import socket from '@ohos.net.socket'
import Queue from '@ohos.util.Queue'
import { LogHelper } from '../LogHelper'

interface ApaListener {
  onApaStart(): void

  onApaStop(): void
}

interface AutomaticListener {
  onPathTrackingStart(): void

  onPathTrackingStop(): void
}

class ApaListener implements ApaListener {
  private listener: AutomaticListener

  constructor(listener: AutomaticListener) {
    this.listener = listener
  }

  onApaStart() {
    if (this.listener) {
      this.listener.onPathTrackingStart()
    }
  }

  onApaStop() {
    if (this.listener) {
      this.listener.onPathTrackingStop()
    }
  }
}

function sleep(time: number) {
  return new Promise<void>((resolve, reject) => {
    setTimeout(() => {
      resolve()
    }, time)
  })
}

class TrackInfo {
  public static CAR_IN_LAND = 1; //车在平地
  public static CAR_IN_RAMP_UP = 2; //车在上坡
  public static CAR_IN_RAMP_DOWN = 3; //车在下坡
  // private static  TYPE_1_LIB = 1;                        //库位
  // private static  TYPE_2_UPRAMP = 2;                     //上坡模式
  // private static  TYPE_3_DOWNRAMP = 3;
}


class TrackMode {
  public static MODE_TEACH = "teach";
  public static MODE_LIB = "lib";
  public static MODE_PATH = "path";
}


class CtrlParam {
  public static k = 0.1; // look forward gain
  public static Lfc = 6.5; // [m] look-ahead distance
  public static Kp = 1.0; // speed proportional gain
  public static dt = 0.1; // [s] time tick
  public static WB = 2.42; // [m] wheel base of vehicle
  public static STEERING_RATIO = 16; //哪吒V型车转向比（哪吒底盘人员提供，与速度关系不大，与方向盘关系大，一般为抛物线关系，已获取到文档）
}

class Message {
  deadline: number;
  angle = 0;

  constructor(deadline: number, angle: number) {
    this.deadline = deadline
    this.angle = angle
  }
}


export class WireControl {
  private static FXP_GAP_TIME: number = 180;
  private static FXP_GAP_DEGREE: number = 40;
  private TAG: string = "automatic";
  private mAutomaticListener: AutomaticListener;
  private mApaListener: ApaListener;
  private socket: socket.UDPSocket
  private apaIp: string;
  private apaPort: number;
  private APA2_HEAD: number[] = [0x08, 0x00, 0x00, 0x02, 0xba]; //  8 0 0 2 186
  private APA3_HEAD: number[] = [0x08, 0x00, 0x00, 0x02, 0x7b]; // 8 0 0 2
  private APA4_HEAD: number[] = [0x08, 0x00, 0x00, 0x02, 0x53];
  private APA_WORKING: number = 0;
  private APA_BRAKE_STATUS: number = 0;
  private APA_InfoDisplayReq: number = 0; // Advanced Parking Information display request
  private APA_EPSAngleReq: number = 1; // APA是否请求转向
  private APA_EPSAngleReqValidity: number = 1; // APA请求转向有效位
  private APA_EPSAngleValueReq: number = 0; //@@@ APA请求转向角度（-540~+540）
  private APA_WorkSts_CH: number = 0; // APA状态
  private APA_TurnLightReq: number = 0; // APA请求左/右转向闪灯
  private APA3_GearPosReq: number = 0; //@@@ 目标档位信号  0X1:P 0X2:R 0X3:N 0X4:D
  private APA3_GearPosReqValidity: number = 1; // 目标档位信号的有效性
  private APA3_TorqReq: number = 0; // 请求VCU目标扭矩
  private APA3_TorqReqValue: number = 512; //@@@ 请求VCU目标扭矩值
  private APA3_TorqReqValidity: number = 1; // 请求VCU目标扭矩状态
  private APA4_ApaMod: number = 0; // APA工作状态
  private APA4_ApaBrkMod: number = 0; // APA制动控制模式
  private APA4_ApaStopReq: number = 0; //@@@ APA请求Stop  0是松  1是拉
  private APA4_ApaEpbReq: number = 0; // APA请求EPB协助
  private APA4_ApaMaxSpd: number = 0; //@@@ APA最大允许车速
  private APA4_ApaTarDecel: number = 0; // APA请求目标减速度
  private APA4_APASwFd: number = 0; // APA设置开关反馈信息
  private APA_Fr02_MsgCounter: number = 0; // 校验位
  private APA3_Fr03_MsgCounter: number = 0; // 校验位
  private APA4_Fr04_MsgCounter: number = 0; // 校验位
  // APA2
  private APA2_APA_HornReq: number = 0; // 喇叭  0x0——关 0x1——开
  //0:unInit; 1:initing; 2:init fail; 3:init success
  private initStatus: number;
  private apaWorking: boolean;
  private isInit: boolean;
  private lastAngle: number = 0;
  private angleHandle: number = 0;
  private lastItem: string = "";
  private lastDoor: number = -1;
  protected cruiseModeSwitch: boolean = true;
  protected rampUpModeSwitch: boolean = true;
  protected rampDownModeSwitch: boolean = true;

  constructor() {
    this.mApaListener = new ApaListener(this.mAutomaticListener)
  }

  public setMAutomaticListener(mAutomaticListener: AutomaticListener) {
    this.mAutomaticListener = mAutomaticListener;
  }

  public getMAutomaticListener() {
    return this.mAutomaticListener;
  }

  public init(ip: string, port: number) {
    this.apaIp = ip;
    this.apaPort = port;
    this.socket = socket.constructUDPSocketInstance()
    this.socket.setExtraOptions({ reuseAddress: true });
    return this.socket.bind({ address: "0.0.0.0", port: 31021 }).then(() => {
      this.isInit = true;
    })
  }

  public apaStart() {
    if (!this.isInit) {
      return
    }
    this.APA_EPSAngleValueReq = 0;
    this.lastAngle = 0;
    if (this.apaWorking) return;
    this.apaWorking = true;
    setInterval(() => {
      if (this.apaWorking) {
        this.sendAutomaticMsg()
      }
    }, 20)
    sleep(3000).then(() => {
      this.initStatus = 1;
      this.APA_EPSAngleValueReq = 0;
      this.APA_WorkSts_CH = 0x2;
      return sleep(2000)
    }).then(() => {
      this.APA_WorkSts_CH = 0x3;
      this.APA3_TorqReq = 1;
      this.APA4_ApaBrkMod = 1;
      this.APA4_ApaMod = 1;
      this.APA4_APASwFd = 1;
      return sleep(60)
    }).then(() => {
      this.APA_WorkSts_CH = 0x4;
      this.APA4_ApaBrkMod = 2;
      this.APA4_ApaStopReq = 1;
      this.APA4_ApaEpbReq = 1;
      this.APA4_ApaTarDecel = 0xc8;
      return sleep(2000)
    }).then(() => {
      this.APA4_ApaMaxSpd = 0x80;
      this.APA4_ApaTarDecel = 0;
      this.initStatus = 3;
      return sleep(2000)
    });
    this.mApaListener.onApaStart();
  }

  public apaStop() {
    if (!this.isInit) return;
    this.mApaListener.onApaStop();
    this.apaOff();
    sleep(3000).then(() => {
      this.apaWorking = false
    })
  }

  public apaOff() {
    this.initStatus = 0;
    this.brakeOn();
    this.APA_WorkSts_CH = 0x0;
    this.APA3_TorqReq = 0;
    this.APA4_ApaBrkMod = 0;
    this.APA4_ApaMod = 0;
    this.APA4_ApaStopReq = 0;
    this.APA4_ApaEpbReq = 0;
    this.APA4_ApaMaxSpd = 0;
    this.APA4_ApaTarDecel = 0;
    this.APA4_APASwFd = 0;
    this.APA_EPSAngleValueReq = 0;
  }

  public apaInit() {
    if (this.initStatus != 0) return;
    this.initStatus = 1;
    this.APA_EPSAngleValueReq = 0;
    this.APA_WorkSts_CH = 0x2;
    sleep(2000).then(() => {
      this.APA_WorkSts_CH = 0x3;
      this.APA3_TorqReq = 1;
      this.APA4_ApaBrkMod = 1;
      this.APA4_ApaMod = 1;
      this.APA4_APASwFd = 1;
      return sleep(60)
    }).then(() => {
      this.APA_WorkSts_CH = 0x4;
      this.APA4_ApaBrkMod = 2;
      this.APA4_ApaStopReq = 1;
      this.APA4_ApaEpbReq = 1;
      this.APA4_ApaTarDecel = 0xc8;
      return sleep(2000)
    }).then(() => {
      this.APA4_ApaStopReq = 0;
      this.APA4_ApaMaxSpd = 0x80; //0xc0;
      this.APA4_ApaTarDecel = 0;
      this.initStatus = 3;
      return sleep(2000)
    }).then(() => {
      this.moveUp();
    })

  }

  // 设定控制模式
  private setControlMode(carLocate: number, mode: string) {
    switch (carLocate) {
      case -1:
      case TrackInfo.CAR_IN_LAND:
        this.cruiseMode();
        break;
      case TrackInfo.CAR_IN_RAMP_UP:
        this.rampUpMode();
        break;
      case TrackInfo.CAR_IN_RAMP_DOWN:
        this.rampDownMode();
        break;
    }

    if (mode === TrackMode.MODE_LIB) {
      this.lowSpeedCruiseMode();
    }
  }

  /**
   * apa指令接口
   * @param cmd
   */

  public apaCmd(cmd: string, mode: string, carLocate: number, isPathTrackEnd: boolean, doorSwitch: number) {
    if (doorSwitch == 1 && this.lastDoor != 1) {
      this.brakeOn();
    } else if (doorSwitch == 0 && this.lastDoor != 0) {
      this.moveUp();
    }
    this.lastDoor = doorSwitch;

    if (isPathTrackEnd) {
      this.brakeOn();
      LogHelper.I(LogHelper.RESULT_PURPERSUIT, "导航到最后点位，停车拉手刹！");
      return;
    }

    if (cmd === "") {
      return;
    }
    let params = cmd.split(";");
    let fxp = parseFloat(params[0]);
    let move = parseInt(params[1]);

    if (fxp != -1000) {
      this.fxpCtrl(fxp, mode);
    }

    this.setControlMode(carLocate, mode);

    if (move != -1000) {
      if (move == 1) {
        this.moveUp();
        LogHelper.I(LogHelper.RESULT_TRACKING, "前进");
      } else if (move == -1) {
        this.moveDown();
        LogHelper.I(LogHelper.RESULT_TRACKING, "后退");
      } else if (move == 0) {
        this.moveStop();
        LogHelper.I(LogHelper.RESULT_TRACKING, "停止");
      } else if (move == 10) {
        this.brakeOn();
        LogHelper.I(LogHelper.RESULT_TRACKING, "刹车");
      }
    }
  }


  private handleAngle(di: number) {

    let angle = di * 180 / Math.PI;
    if (angle > 45) {
      angle = 45;
    } else if (angle < -45) {
      angle = -45;
    }
    let fxp: number = angle * CtrlParam.STEERING_RATIO;

    return fxp;
  }

  public fxpCtrl(angle: number, mode: string) {
    let angle2 = 0;
    if (mode === TrackMode.MODE_PATH) {
      LogHelper.I(LogHelper.RESULT_TRACKING, "输出方向盘PATH：" + this.handleAngle(angle));
      this.APA4_ApaMaxSpd = 0xFF; //0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码  0x13F-5码  0x17F_6码  0x1BF_7码  0x200_8码
      // 0x300——12码   0x3c0——15码  0x500——20码   0x640——25码  0x780——30码
      angle2 = this.handleAngle(angle);
    } else if (mode === TrackMode.MODE_TEACH) {
      LogHelper.I(LogHelper.RESULT_TRACKING, "输出方向盘：" + angle);
      this.APA4_ApaMaxSpd = 0x80; //0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码  0x13F-5码  0x17F_6码  0x1BF_7码  0x200_8码
      angle2 = angle;
    } else {
      LogHelper.I(LogHelper.RESULT_TRACKING, "输出方向盘：" + angle);
      this.APA4_ApaMaxSpd = 0x80; //0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码  0x13F-5码  0x17F_6码  0x1BF_7码  0x200_8码
      angle2 = angle;
    }

    if (this.initStatus != 3) return;
    if (angle2 > 540) {
      angle2 = 540;
    } else if (angle < -540) {
      angle2 = -540;
    }
    if (mode === TrackMode.MODE_PATH) {
      this.fxp4(angle2);
    } else {
      this.fxp4(angle2);
    }

  }

  private fxp2(angle: number) {
    // 要实现方向盘的逐步转向，哪吒控制协议不支持单步过大的操作，之前的转向
    // 策略为：每次转30度，300毫秒完成一次转向
    if (Math.abs(this.lastAngle - angle) < 10) {
      return;
    }

    if (Math.abs(angle - this.APA_EPSAngleValueReq) < 50) {
      this.APA_EPSAngleValueReq = angle;
      LogHelper.I(LogHelper.RESULT_TRACKING, "-----方向盘变化小，直接执行：" + this.APA_EPSAngleValueReq);
      return;
    }

    this.angleHandle = angle - this.lastAngle;

    let queue = new Queue<Message>()
    let now = new Date().getUTCMilliseconds();
    let angleHandleAbs = Math.abs(this.angleHandle);

    for (let i = 0; i < 30; i++) {
      if (angleHandleAbs > 0) {
        if (this.angleHandle > 0) {
          queue.add(new Message(now + i * WireControl.FXP_GAP_TIME, Math.min(WireControl.FXP_GAP_DEGREE, angleHandleAbs)));
        } else {
          queue.add(new Message(now + i * WireControl.FXP_GAP_TIME, -Math.min(WireControl.FXP_GAP_DEGREE, angleHandleAbs)));
        }
        angleHandleAbs -= WireControl.FXP_GAP_DEGREE;
      } else {
        break;
      }
    }

    let temp = "";
    for (let ms of queue) {
      temp = temp + "&" + ms.angle;
    }

    LogHelper.I(LogHelper.RESULT_TRACKING, "queue：" + temp)

    setInterval(() => {
      if (queue.length > 0) {
        this.APA_EPSAngleValueReq += queue.pop().angle;
      }
    }, WireControl.FXP_GAP_TIME)

    this.lastAngle = angle;
  }

  private fxp3(fxpAngle: number) {
    let gap = Math.abs(fxpAngle - this.APA_EPSAngleValueReq);
    if (gap <= 10) {
    } else {
      let step = gap * 45 / 540;
      step = Math.min(45, step);
      step = Math.max(2, step);
      LogHelper.I(this.TAG, "AAA step:" + step);
      if (fxpAngle > this.APA_EPSAngleValueReq) {
        this.APA_EPSAngleValueReq += step;
      } else if (fxpAngle < this.APA_EPSAngleValueReq) {
        this.APA_EPSAngleValueReq -= step;
      }
    }
  }

  public fxp4(fxpAngle: number) {
    LogHelper.I("xwq22", "angle:" + fxpAngle + " APA_EPSAngleValueReq:" + this.APA_EPSAngleValueReq);
    let gap = Math.abs(fxpAngle - this.APA_EPSAngleValueReq);
    if (gap <= 10) {
      // do nothing
    } else if (gap < 40) {
      this.APA_EPSAngleValueReq = fxpAngle;
    } else {
      if (fxpAngle > this.APA_EPSAngleValueReq) {
        this.APA_EPSAngleValueReq += 40;
      } else if (fxpAngle < this.APA_EPSAngleValueReq) {
        this.APA_EPSAngleValueReq -= 40;
      }
    }
  }

  private fxp(angle: number) {
    LogHelper.I(this.TAG, "angle:" + angle + " APA_EPSAngleValueReq:" + this.APA_EPSAngleValueReq);
    let gap = Math.abs(angle - this.APA_EPSAngleValueReq);
    if (gap <= 10) {
      this.APA_EPSAngleValueReq = angle;
    } else {
      let step = gap * 45 / 540;
      step = Math.min(45, step);
      step = Math.max(2, step);
      if (angle > this.APA_EPSAngleValueReq) {
        this.APA_EPSAngleValueReq += step;
      } else if (angle < this.APA_EPSAngleValueReq) {
        this.APA_EPSAngleValueReq -= step;
      }
    }
  }

  private lowSpeedCruiseMode() {
    this.APA3_TorqReqValue = 536; // 正常536  坡道610
    this.APA4_ApaMaxSpd = 0x40; //0xc0;0x80 //0x40为1码、0x80为2码、c0为3码 0xFF-4码  0x13F-5码  0x17F_6码  0x1BF_7码  0x200_8码
  }

  //正常巡航模式
  private cruiseMode() {
    if (this.cruiseModeSwitch) {
      this.APA3_TorqReqValue = 536; // 正常536  坡道610
      //            APA4_ApaMaxSpd = 0xc0;//0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码  0x13F-5码  0x17F_6码  0x1BF_7码  0x200_8码

      this.cruiseModeSwitch = false;
      this.rampUpModeSwitch = true;
      this.rampDownModeSwitch = true;
    }
  }

  // 坡道上坡模式
  private rampUpMode() {
    if (this.rampUpModeSwitch) {
      this.APA3_TorqReqValue = 610; // 正常536  坡道610
      //            APA4_ApaMaxSpd = 0xc0;//0xc0;0x80 // 0x80为2码、c0为3码 0xFF-4码  0x13F-5码 0x200_8码

      this.rampUpModeSwitch = false;
      this.cruiseModeSwitch = true;
      this.rampDownModeSwitch = true;
    }

  }

  // 坡道下坡模式
  private rampDownMode() {
    if (this.rampDownModeSwitch) {
      this.APA3_TorqReqValue = 488; // 正常536  坡道610
      this.APA4_ApaTarDecel = 0x76C; //0x12C:300   0xc8:200(当前刹车力度)  0x190:400  0x1F4:500  0x258:600  0x320:800  0x3E8:1000  0x5DC:1500  0x76C:1900
      this.rampDownModeSwitch = false;
      this.cruiseModeSwitch = true;
      this.rampUpModeSwitch = true;
    }

  }

  // 起步
  public moveUp() {
    if (this.initStatus != 3) return;
    this.brakeOff();
    this.APA3_GearPosReq = 0x4;
    this.APA3_TorqReqValue = 536; // 正常536  坡道610
    // this.carDir = "车辆状态：前进";
  }

  // 停车
  public moveStop() {
    if (this.initStatus != 3) return;
    this.APA3_GearPosReq = 0x3;
    this.APA3_TorqReqValue = 512;
    // carDir = "车辆状态：停车";
  }

  // 后退
  public moveDown() {
    if (this.initStatus != 3) return;
    this.brakeOff();
    this.APA3_GearPosReq = 0x2;
    this.APA3_TorqReqValue = 488;
    // carDir = "车辆状态：后退";
  }

  // 拉手刹
  public brakeOn() {
    if (this.initStatus != 3) return;
    this.moveStop();
    this.APA4_ApaStopReq = 0x1;
    this.APA4_ApaTarDecel = 0xc8;
  }

  // 松手刹
  public brakeOff() {
    if (this.initStatus != 3) return;
    this.APA4_ApaStopReq = 0;
    this.APA4_ApaTarDecel = 0;
  }

  // 左转向灯
  public leftLight() {
    this.APA_TurnLightReq = 0x1;
  }

  // 右转向灯
  public rightLight() {
    this.APA_TurnLightReq = 0x2;
  }

  // 双跳灯
  public doubleLight() {
    this.APA_TurnLightReq = 0x3;
  }

  // 关闭转向灯
  public shutLight() {
    this.APA_TurnLightReq = 0x0;
  }

  // 开喇叭
  public onHorn() {
    this.APA2_APA_HornReq = 0x1;
  }

  // 关喇叭
  public offHorn() {
    this.APA2_APA_HornReq = 0x0;
  }

  // 增加扭矩
  public addTorq() {
    this.APA3_TorqReqValue += 5;
  }

  // 减少扭矩
  public redTorq() {
    this.APA3_TorqReqValue -= 5;
  }

  // 设置档位
  public setGear(gear: string) {
    if ("N" === gear) { //空挡N
      this.APA3_GearPosReq = 0x3;
    } else if ("P" === gear) { //驻车档P
      this.APA3_GearPosReq = 0x1;
    } else if ("R" === gear) { //倒挡R
      this.APA3_GearPosReq = 0x2;
    } else if ("D" === gear) { //前进D
      this.APA3_GearPosReq = 0x4;
    }
  }

  private sendAutomaticMsg() {
    this.sendAPA4Data();
    this.sendAPA3Data();
    this.sendAPA2Data();
  }

  private sendAPA2Data() {
    let data = new Uint8Array(13);
    data.set(this.APA2_HEAD);
    let index = 5;
    data[index++] = this.APA_InfoDisplayReq;
    let angle = (-this.APA_EPSAngleValueReq + 780) * 10;
    data[index++] = ((angle & 0xff00) >> 8);
    data[index++] = (angle & 0xff);
    // 20240527 添加喇叭信号
    data[index++] = ((((this.APA_EPSAngleReq & 0x1) << 7) | ((this.APA_EPSAngleReqValidity & 0x1) << 6) | ((this.APA2_APA_HornReq & 0x1) << 4)) | 0x22);
    data[index++] = (((this.APA_WorkSts_CH & 0x7) << 5) | (this.APA_TurnLightReq & 0x3));
    data[index++] = 0;
    this.APA_Fr02_MsgCounter += 1;
    if (this.APA_Fr02_MsgCounter > 15) {
      this.APA_Fr02_MsgCounter = 0;
    }
    data[index++] = this.APA_Fr02_MsgCounter & 0xf;
    let checkSum = 0;
    for (let i = 5; i < 12; i++) {
      checkSum ^= data[i];
    }
    data[index++] = checkSum & 0xff;
    this.sendAutomaticData(data);
  }

  private sendAPA3Data() {
    let data = new Uint8Array(13);
    data.set(this.APA3_HEAD);
    let index = 5;
    data[index++] = 0;
    data[index++] = 0;
    data[index++] = ((this.APA3_GearPosReq & 0x7) << 5) | (this.APA3_GearPosReqValidity << 4) | (this.APA3_TorqReq << 3) | ((this.APA3_TorqReqValue & 0x380) >> 7);
    data[index++] = (this.APA3_TorqReqValue & 0x7f) << 1
    data[index++] = this.APA3_TorqReqValidity << 7;
    data[index++] = 0;
    this.APA3_Fr03_MsgCounter += 1;
    if (this.APA3_Fr03_MsgCounter > 15) {
      this.APA3_Fr03_MsgCounter = 0;
    }

    data[index++] = this.APA3_Fr03_MsgCounter & 0xf;
    let checkSum = 0;
    for (let i = 5; i < 12; i++) {
      checkSum ^= data[i];
    }

    data[index++] = checkSum & 0xff;
    this.sendAutomaticData(data);
  }

  private sendAPA4Data() {
    let data = new Uint8Array(13);
    data.set(this.APA4_HEAD);
    let index = 5;
    data[index++] = 0x4 | (this.APA4_ApaEpbReq & 0x3) | (this.APA4_APASwFd << 3);
    data[index++] = 0;
    data[index++] = 0x00 | (this.APA4_ApaTarDecel & 0x700) >> 8; // 第二字节  去9 10 11 三位
    data[index++] = this.APA4_ApaTarDecel & 0xff; //  第三字节 取低八位
    data[index++] = (this.APA4_ApaMaxSpd & 0xff00) >> 8; // 第四字节 取 速度的高八位
    data[index++] = this.APA4_ApaMaxSpd & 0xff; // 第五字节 取 速度的低八位
    this.APA4_Fr04_MsgCounter += 1;
    if (this.APA4_Fr04_MsgCounter > 15) {
      this.APA4_Fr04_MsgCounter = 0;
    }
    data[index++] = (this.APA4_Fr04_MsgCounter & 0xf) | (this.APA4_ApaStopReq << 7) | (this.APA4_ApaMod << 6) | (this.APA4_ApaBrkMod << 4);
    let checkSum = 0;
    for (let i = 5; i < 12; i++) {
      checkSum ^= data[i];
    }
    data[index++] = checkSum & 0xff;
    this.sendAutomaticData(data);
  }

  private sendAutomaticData(data: Uint8Array) {
    this.socket.send({
      address: {
        address: this.apaIp,
        port: this.apaPort,
      },
      data: data.buffer
    })
  }
}