import {mat3, vec3, vec2} from 'gl-matrix'
import satellite_geo_calc from 'satellite-geo-calc'
import { transformCoordinateToLngLat } from './transform'
import * as math from 'mathjs'

// 刺点预测
/**
 * 
 * @param {*} cameraInfo.images 照片列表，包含{img_id, img_path, camera_id, image_rect, projection_matrix}
 * @param {*} cameraInfo.cameras 相机列表，包含{camera_id， camera_params, width, height}
 * @param {*} controlPoint 当前控制点，包含{id, coordinate, usage, observations}
 */
export function predictObservations (cameraInfo, controlPoint, control_coordinate_system) {
  let {observations} = controlPoint
  if (observations.length > 1) {
    return predictOptimize(cameraInfo, controlPoint)
  } else {
    return predictBeforeAT(cameraInfo, controlPoint, control_coordinate_system)
  }
}

function predictBeforeAT (cameraInfo, controlPoint, control_coordinate_system) {
  let {images, cameras, coordinateSystem} = cameraInfo
  let camersMap = {}
  cameras.forEach(c => camersMap[c.camera_id] = c)
  let {coordinate} = controlPoint
  let Xw = vec3.fromValues(0, 0, 0)
  if (control_coordinate_system.type === 1) {
    Xw = vec3.fromValues(coordinate[1], coordinate[1], coordinate[2])
  } else {
    let lngLat = transformCoordinateToLngLat(coordinate, control_coordinate_system)
    let originalPoint = coordinateSystem.origin_point // enu的坐标原点
    let Xenu = satellite_geo_calc.coordinateTransfer.lla_to_enu({ lon:lngLat[0], lat:lngLat[1], alt: lngLat[2] || 0}, {lon: originalPoint[0], lat: originalPoint[1], alt: originalPoint[2]})
    Xw = vec3.fromValues(Xenu.x, Xenu.y, Xenu.z)
  }
  let predicts = {}
  images.forEach(image => {
    let {img_id, camera_id, projection_matrix, image_rect} = image
    let parameters = camersMap[camera_id].camera_params
    let {width, height} = image_rect
    let P = projection_matrix
    let R = mat3.fromValues(P[0], P[4], P[8], P[1], P[5], P[9], P[2], P[6], P[10]) // 旋转矩阵， 列主序
    let T = vec3.fromValues(P[3], P[7], P[11]) // 平移向量
    let K = mat3.fromValues(parameters[0], 0, 0, 0, parameters[1], 0, parameters[2], parameters[3], 1)
    let ph = vec3.transformMat3(vec3.create(), vec3.add(vec3.create(), vec3.transformMat3(vec3.create(), Xw, R), T), K)
    let pixelCoord = vec3.scale(vec3.create, ph, 1 / ph[2])
    if (pixelCoord[0] > 0 && pixelCoord[0] < width && pixelCoord[1] > 0 && pixelCoord[1] < height) {
      // 加上相机畸变
      let p_vec = vec3.fromValues(pixelCoord[0], pixelCoord[1], 1)
      let x_n = vec3.transformMat3(vec3.create(), p_vec, mat3.invert(mat3.create(), K))
      let dist = distortPoint(x_n, parameters)
      pixelCoord = vec3.transformMat3(vec3.create(), vec3.fromValues(...dist, 1), K)
      predicts[img_id] = [pixelCoord[0], pixelCoord[1]]
      image.predict = {
        uv: [pixelCoord[0], pixelCoord[1]],
        position: {left: pixelCoord[0] * 100 / width, top: pixelCoord[1] * 100 / height},
        distance: Math.hypot(pixelCoord[0] - width / 2, pixelCoord[1] - height / 2)
      }
    } else {
      image.predict = undefined
    }
  })
  return predicts
}

function predictOptimize (cameraInfo, controlPoint) {
  let {images, cameras} = cameraInfo
  let {observations} = controlPoint
  let camersMap = {}
  cameras.forEach(c => camersMap[c.camera_id] = c)
  let predicts = {}, observedImages = [] // 已经刺点的图像
  let obsevedMap = new Map()
  observations.forEach((d) => obsevedMap.set(d.id, d.uv))
  images.forEach(image => { // 第一次循环，找出已经完成刺点的图像，后面需要用这些刺点坐标列方程
    if (!obsevedMap.has(image.img_id)) {
      image.observation = undefined
      return
    }
    image.observation = {
      id: image.img_id,
      uv: obsevedMap.get(image.img_id)
    }
    observedImages.push(image)
  })
  // 根据已经刺的点计算控制点的空间坐标
  let a = [], b = [] // A矩阵和B矩阵的数组，满足Ax + B = 0，其中x未待求的坐标值，行优先
  observedImages.forEach(observedImage => {
    let P_m = observedImage.projection_matrix
    let parameters_m = camersMap[observedImage.camera_id].camera_params
    let undistort_uv = undistortPoint(observedImage.observation.uv, parameters_m)
    let R_m = mat3.fromValues(P_m[0], P_m[4], P_m[8], P_m[1], P_m[5], P_m[9], P_m[2], P_m[6], P_m[10]) // 旋转矩阵， 列主序
    let t_m = vec3.fromValues(P_m[3], P_m[7], P_m[11]) // 平移向量
    let K_m = mat3.fromValues(parameters_m[0], 0, 0, 0, parameters_m[1], 0, parameters_m[2], parameters_m[3], 1)
    let K_m_invert = mat3.invert(mat3.create(), K_m) // K_m的逆矩阵
    // 满足rX = l
    let undistort_uv_norm = vec3.transformMat3(vec3.create(), undistort_uv, K_m_invert) 
    a.push([R_m[0] - R_m[2] * undistort_uv_norm[0], R_m[3] - R_m[5] * undistort_uv_norm[0], R_m[6] - R_m[8] * undistort_uv_norm[0]])
    a.push([R_m[1] - R_m[2] * undistort_uv_norm[1], R_m[4] - R_m[5] * undistort_uv_norm[1], R_m[7] - R_m[8] * undistort_uv_norm[1]])
    b.push(t_m[2] * undistort_uv_norm[0] - t_m[0], t_m[2] * undistort_uv_norm[1] - t_m[1])
  })
  let A = math.matrix(a)
  let B = math.matrix(b)
  let A_t = math.transpose(A)
  let result = math.multiply(math.multiply(math.inv(math.multiply(A_t, A)), A_t), B)
  let X = vec3.fromValues(result.get([0]), result.get([1]), result.get([2]))

  images.forEach(image => { // 第二次循环，计算未刺点图像上的预测坐标
    let {img_id, camera_id, projection_matrix, image_rect} = image
    let parameters = camersMap[camera_id].camera_params
    let {width, height} = image_rect
    if (image.observation) { // 已经刺点，不需要预测
      return image.predict = {
        uv: image.observation.uv,
        position: {left: image.observation.uv[0] * 100 / width, top: image.observation.uv[1] * 100 / height},
        distance: Math.hypot(image.observation.uv[0] - width / 2, image.observation.uv[1] - height / 2)
      }
    }
    let P_n = projection_matrix
    let R_n = mat3.fromValues(P_n[0], P_n[4], P_n[8], P_n[1], P_n[5], P_n[9], P_n[2], P_n[6], P_n[10]) // 旋转矩阵， 列主序
    let t_n = vec3.fromValues(P_n[3], P_n[7], P_n[11]) // 平移向量
    let K_n = mat3.fromValues(parameters[0], 0, 0, 0, parameters[1], 0, parameters[2], parameters[3], 1)
    let p = vec3.add(vec3.create(), vec3.transformMat3(vec3.create(), X, R_n), t_n)
    let pixelCoord = vec3.transformMat3(vec3.create(), vec3.fromValues(p[0] / p[2], p[1] / p[2], 1), K_n)
    if (pixelCoord[0] > 0 && pixelCoord[0] < width && pixelCoord[1] > 0 && pixelCoord[1] < height) {
      let p_vec = vec3.fromValues(...pixelCoord, 1)
      let x_n = vec3.transformMat3(vec3.create(), p_vec, mat3.invert(mat3.create(), K_n))
      let dist = distortPoint(x_n, parameters)
      pixelCoord = vec3.transformMat3(vec3.create(), vec3.fromValues(...dist, 1), K_n)
      predicts[img_id] = [pixelCoord[0], pixelCoord[1]]
      image.predict = {
        uv: [pixelCoord[0], pixelCoord[1]],
        position: {left: pixelCoord[0] * 100 / width, top: pixelCoord[1] * 100 / height},
        distance: Math.hypot(pixelCoord[0] - width / 2, pixelCoord[1] - height / 2)
      }
    } else {
      image.predict = undefined
    }
  })
  return predicts
}

function undistortPoint (p, parameters) {
  let K = mat3.fromValues(parameters[0], 0, 0, 0, parameters[1], 0, parameters[2], parameters[3], 1)
  let p_vec = vec3.transformMat3(vec3.create(), vec3.fromValues(...p, 1), mat3.invert(mat3.create(), K)) // 归一化以后的坐标
  let iterators = 100
  let epsilon = 1e-10
  let undistort = vec2.fromValues(p_vec[0], p_vec[1])
  let dist = vec2.subtract(vec2.create(), distortPoint(undistort, parameters), undistort)
  let diff = vec2.subtract(vec2.create(), vec2.add(vec2.create(), undistort, dist), p_vec)
  while((Math.abs(diff[0]) + Math.abs(diff[1])) > epsilon && iterators-- > 0) {
    undistort = vec2.subtract(vec2.create(), p_vec, dist)
    dist = vec2.subtract(vec2.create(), distortPoint(undistort, parameters), undistort)
    diff = vec2.subtract(vec2.create(), vec2.add(vec2.create(), undistort, dist), p_vec)
  }
  return vec3.transformMat3(vec3.create(), vec3.fromValues(...undistort, 1), K)
}

function distortPoint (p, parameters) {
  let x3 = [p[0], p[1]]
  let r2 = x3[0] * x3[0] + x3[1] * x3[1]
  let r4 = Math.pow(r2, 2)
  let r6 = Math.pow(r2, 3)
  let x3_ = [
    x3[0] * (1 + parameters[4] * r2 + parameters[5] * r4 + parameters[6] * r6) + 2 * parameters[7] * x3[0] * x3[1] + parameters[8] * (r2 + 2 * x3[0] * x3[0]),
    x3[1] * (1 + parameters[4] * r2 + parameters[5] * r4 + parameters[6] * r6) + 2 * parameters[8] * x3[0] * x3[1] + parameters[7] * (r2 + 2 * x3[1] * x3[1])
  ]
  return vec2.fromValues(...x3_)
}