import { Vector2 } from 'three';
import {
  getDistance,
  myTruncate,
  parsePath,
  standardKeyPointsChanger
} from './utils';

/**
 * @desc A star
 */
export default class AStar {
  sc: number;
  keyPoints: any[];
  keyPointMap: any;

  openList: any[];
  closeList: any[];
  invalidList: any[];
  searchCount: number;
  beginTime: Date;
  endTime: Date;

  beginNode: any;
  endNode: any;

  private beginVertical!: number[];
  private endVertical!: number[];

  constructor(private path: any[]) {
    // 转换 path 的字符串为数字
    this.path = parsePath(this.path);

    // 单节点预估成本
    this.sc = 1;
    this.keyPoints = standardKeyPointsChanger(this.path);
    this.keyPointMap = {};
    // 将列表转换map对象
    for (let i = 0; i < this.keyPoints.length; i++) {
      const p = this.keyPoints[i];
      this.keyPointMap[p.id] = p;
    }
    this.openList = [];
    this.closeList = [];
    this.invalidList = [];
    this.searchCount = 0;
    this.beginTime = new Date();
    this.endTime = new Date();
  }

  private shortestPath(beginNode: any, endNode: any) {
    const result: any = [];
    this.openList = []; // 已经知道，但未走过的节点
    this.closeList = []; // 走过的节点
    this.invalidList = []; // 失效的点
    this.searchCount = 0;
    this.beginTime = new Date();
    beginNode.parent = null;
    beginNode.d = 0;
    this.beginNode = beginNode;
    this.endNode = endNode;
    this.closeList.push(this.beginNode);
    while (true) {
      if (this.closeList.length === 0) {
        break;
      }
      this.searchCount++;
      let currNode = this.closeList[this.closeList.length - 1];
      if (currNode.id === endNode.id) {
        // 已经抵达目的地, 按照parent往回追溯，返回结果
        while (currNode && currNode != null) {
          result.unshift(currNode);
          currNode = currNode.parent;
        }
        break;
      }
      if (currNode.neighbours && currNode.neighbours.length > 0) {
        for (let i = 0; i < currNode.neighbours.length; i++) {
          const nb = currNode.neighbours[i];
          const tmpNode = this.keyPointMap[nb.id];
          // 周围节点必须满足三个条件：
          // 1.这个点是合法的，什么叫合法？这里简单的就是可以通行的，不能是障碍物
          if (!tmpNode || this.contain(this.invalidList, tmpNode)) {
            continue;
          }
          // 2.这个点不能在close表中
          if (this.contain(this.closeList, tmpNode)) {
            continue;
          }
          // 3.这个点不能在open表中
          if (this.contain(this.openList, tmpNode)) {
            continue;
          }
          tmpNode.d = nb.d; // 相邻点距离
          tmpNode.parent = currNode;
          tmpNode.cost = this.f(tmpNode);
          this.openList.push(tmpNode);
        }
      }
      if (this.openList.length > 0) {
        // 计算open列表cost值最低的节点minNode
        let minNode: any = null;
        for (let i = 0; i < this.openList.length; i++) {
          const tmp = this.openList[i];
          if (minNode == null || minNode.cost > tmp.cost) {
            minNode = tmp;
          }
        }
        // minNode从open表中删除加入close表
        this.remove(this.openList, minNode);
        this.closeList.push(minNode);
      } else {
        // 如果openList没有节点了，那么取上一个closeList中的节点，继续循环
        // 该节点没有有效的邻近节点，移到失效列表
        this.invalidList.push(currNode);
        this.remove(this.closeList, currNode);
      }
      if (this.searchCount > 500000) {
        // 避免死循环
        break;
      }
    }
    this.endTime = new Date();
    return result;
  }

  private contain(nodes: any, node: any) {
    let result = false;
    nodes.forEach(function (item: any) {
      if (item.id === node.id) {
        result = true;
      }
    });
    return result;
  }

  private remove(nodes: any, node: any) {
    let move = false;
    for (let i = 0; i < nodes.length; i++) {
      if (move) {
        nodes[i - 1] = nodes[i];
      }
      if (node.id === nodes[i].id) {
        move = true;
      }
    }
    if (move) {
      nodes.length -= 1;
    }
  }

  // 节点代价
  private f(n: any) {
    return this.g(n) + this.h(n);
  }

  // 初始节点到n节点的明确代价
  private g(n: any) {
    let result = 0;
    let currNode = n;
    while (currNode && currNode != null) {
      result += currNode.d;
      currNode = currNode.parent;
    }
    return result;
  }

  // 是n节点到目标点的预估代价；
  private h(n: any) {
    return this.euclidianCost(n, this.endNode);
  }

  // //麦哈顿估算法
  // manhattanCost(begin, end) {
  //   return (
  //     Math.abs(begin.x - end.x) * this.sc + Math.abs(begin.y + end.y) * this.sc
  //   );
  // }

  // 几何估价法
  private euclidianCost(begin: any, end: any) {
    const dx = begin.x - end.x;
    const dy = begin.y - end.y;
    return Math.sqrt(dx * dx + dy * dy) * this.sc;
  }

  // //对角线估价法
  // diagonalCost(begin, end) {
  //   var dx = Math.abs(begin.x - end.x);
  //   var dy = Math.abs(begin.y - end.y);
  //   var diag = Math.min(dx, dy);
  //   var straight = dx + dy;
  //   return this.sc * diag + this.sc * (straight - 2 * diag);
  // }

  private getNearest(point: number[], isBegin = true) {
    // const _name = (val: string) => `${val}_${isBegin ? 'begin' : 'end'}`;

    // removeObjectByName(_name('rnFindPointNear'), pointGroup.value.pointGroup);
    // removeObjectByName(
    //   _name('rnFindPointVertical'),
    //   pointGroup.value.pointGroup
    // );
    // removeObjectByName(_name('rnFindLineNear'));

    let nearItem;
    let tempItem;
    let nearPoint;
    let vertical;
    let dist = Infinity;
    for (let i = 0; i < this.path.length; i++) {
      tempItem = this.path[i];
      const {
        dist: tempDist,
        point: tempPoint,
        vertical: tempVertical
      } = this.nearestPointOnLine(tempItem.begin, tempItem.end, point);
      if (tempDist < dist) {
        dist = tempDist;
        nearPoint = tempPoint;
        nearItem = tempItem;
        vertical = tempVertical;
      }
    }

    // this.drawNearLine(nearItem.begin, nearItem.end, _name('rnFindLineNear'));
    // this.drawNearPoint(nearPoint as [number, number], _name('rnFindPointNear'));
    // vertical &&
    //   this.drawVertical(
    //     vertical as [number, number],
    //     _name('rnFindPointVertical')
    //   );
    return { point: nearPoint, vertical };
  }

  /**
   * @desc 最佳线路计算
   */
  calcShortestPath(begin: number[], end: number[]) {
    const { point: beginPoint, vertical: beginVertical } = this.getNearest(
      begin,
      true
    );
    const { point: endPoint, vertical: endVertical } = this.getNearest(
      end,
      false
    );

    // 更新起点/终点垂点
    this.beginVertical = beginVertical!;
    this.endVertical = endVertical!;

    const standardBeginPoint = this.keyPoints.find(
      _ => _.id === JSON.stringify(beginPoint)
    );
    const standardEndPoint = this.keyPoints.find(
      _ => _.id === JSON.stringify(endPoint)
    );

    let result = this.shortestPath(standardBeginPoint, standardEndPoint);

    // 进行线路优化
    result = this.trackOptimize(result);

    return result;
  }

  /**
   * @desc 线路优化
   */
  private trackOptimize(trackPoints: any[]) {
    if (trackPoints.length >= 2) {
      // 加工，判断头尾是否加上垂点

      // 有垂点才进行判断追加，没有垂点，就不进行添加
      if (this.beginVertical) {
        const beginV = new Vector2(
          this.beginVertical[0],
          this.beginVertical[1]
        );

        if (this.isInLine(trackPoints[0], trackPoints[1], beginV, 2)) {
          console.log(
            '起点垂点在导航线上，shift 去除连接起点，后续 unshift 起点垂点'
          );
          trackPoints.shift();
        }
        trackPoints.unshift(beginV);
      }

      if (this.endVertical) {
        const endV = new Vector2(this.endVertical[0], this.endVertical[1]);
        if (
          this.isInLine(
            trackPoints[trackPoints.length - 1],
            trackPoints[trackPoints.length - 2],
            endV,
            2
          )
        ) {
          console.log(
            '终点垂点在导航线上，pop 去除连接终点，后续 push 终点垂点'
          );
          trackPoints.pop();
        }
        trackPoints.push(endV);
      }
    }
    return trackPoints;
  }

  /**
   * @desc 获取点到各个线段上最近的一条线段和相关信息
   * 需要返回距离，来筛选最近的线段
   * dist：一般来说是点和垂点的距离 dist。但是需要判断如果垂点在直线外部，就计算点到线段两端的距离 min dist
   * 拿到 dist 循环筛选出最近的即可。
   * point：表示距离点最近的线段端点，主要用来a星算法
   * vertical：表示垂点，主要用来补充线段
   */
  private nearestPointOnLine(begin: number[], end: number[], p: number[]) {
    const _getNearPointByLineBeginEnd = () => {
      const arr = [
        {
          point: begin,
          dist: getDistance(
            new Vector2(begin[0], begin[1]),
            new Vector2(p[0], p[1])
          )
        },
        {
          point: end,
          dist: getDistance(
            new Vector2(end[0], end[1]),
            new Vector2(p[0], p[1])
          )
        }
      ];
      arr.sort((a, b) => a.dist - b.dist);
      return arr[0];
    };

    let point: number[] = [0, 0];
    let vertical: number[] | null = null;
    let dist = 0;
    // 获取垂足
    const { x, y } = this.getVerticalPoint(
      new Vector2(begin[0], begin[1]),
      new Vector2(end[0], end[1]),
      new Vector2(p[0], p[1])
    );

    // 判断垂足是否在线上，在线上直接返回，不在线上，返回距离垂足最近的点
    const isInline = this.isInLine(
      new Vector2(begin[0], begin[1]),
      new Vector2(end[0], end[1]),
      new Vector2(x, y),
      2
    );
    const { point: _point, dist: _dist } = _getNearPointByLineBeginEnd();

    point = _point;

    if (isInline) {
      vertical = [x, y];
      dist = getDistance(new Vector2(p[0], p[1]), new Vector2(x, y));
    } else {
      vertical = null;
      point = _point;
      dist = _dist;
    }

    return {
      vertical,
      point,
      dist
    };
  }

  /**
   * @desc 求点C在由点A、点B组成的直线的垂点
   */
  private getVerticalPoint(p1: Vector2, p2: Vector2, p: Vector2) {
    // 直线方程：Ax + By + C = 0
    const A = p2.y - p1.y;
    const B = p1.x - p2.x;
    const C = p2.x * p1.y - p1.x * p2.y;
    const tmp = A * A + B * B;
    let x = (B * B * p.x - A * B * p.y - A * C) / tmp; // 垂点的X坐标
    let y = (A * A * p.y - A * B * p.x - B * C) / tmp; // 垂点的Y坐标

    // 缩小精度
    const [x1, y1] = myTruncate([x, y]);
    x = x1;
    y = y1;

    return { x, y };
  }

  /**
   * @desc 计算两个坐标连线后的倾斜角度
   */
  private getAngle(p1: Vector2, p2: Vector2) {
    const radian = Math.atan2(p2.y - p1.y, p2.x - p1.x);
    const angle = radian * (180 / Math.PI);
    return {
      radian,
      angle
    };
  }

  /**
   * @desc 判断p是否在p1和p2的线段内
   */
  private isInLine(p1: Vector2, p2: Vector2, p: Vector2, offsetAngle = 0) {
    if (!p1 || !p2 || !p) {
      return false;
    }
    // 允许一定的上下偏差，由于一些精度计算原因，可能点无法被识别在线段上
    const diffAngle =
      Math.floor(this.getAngle(p1, p).angle) -
      Math.floor(this.getAngle(p, p2).angle);

    if (offsetAngle === 0) {
      return diffAngle === 0;
    } else {
      // [-offsetAngle,offsetAngle]
      return diffAngle >= -offsetAngle && diffAngle <= offsetAngle;
    }
  }
}
