const fs = require('fs');
const util = require('util');
const exec = util.promisify(require('child_process').exec);
const {Matrix} = require('ml-matrix');

const commandI = "/Users/msladek/Documents/bakalarka/bachelor/server/output/10/reconstruction_sequential/sfm_data.bin";
const commandM = "/Users/msladek/Documents/bakalarka/bachelor/server/output/10/matches";
const commandU = "/Users/msladek/Documents/bakalarka/bachelor/server/output/10/match_out";
const commandO = "/Users/msladek/Documents/bakalarka/bachelor/server/tmp/localization_out";
// const commandQ = "/Users/msladek/Documents/bakalarka/bachelor/server/imgs/loc_camera_image2019_03_28_20_43_35_36.jpeg";
let command = `-s -i ${commandI} -m ${commandM} -u ${commandU} -o ${commandO} -q `;
const binary = "openMVG_main_SfM_Localization";

function parseHrtimeToSeconds(hrtime) {
  const seconds = (hrtime[0] + (hrtime[1] / 1e9)).toFixed(3);
  return seconds;
}

function find_extrinsics(obj, currId){
  let arr = obj.extrinsics;
  arr = arr.filter(e => parseInt(e.key) === currId);
  if(arr.length > 0) return arr[0].value;
  return null;
}

async function localizate(path, filename) {
  // return {matrix: Matrix.identity(4).to2DArray(), matrixProjection: Matrix.identity(4).to2DArray()};
  command += path;
  // ls = spawnSync( 'ls', [ '-lh', '/usr' ] );
  // const ls = spawnSync( command );
  let outputDir = commandO + "/" + filename;
  outputDir = outputDir.slice(0, outputDir.length-5);
  outputDir += "/";
  const startTime = process.hrtime();
  // const ls = spawnSync(binary, ['-s', '-i', commandI, '-m', commandM, '-u', commandU, '-o', outputDir, '-q', path]);
  const cmd = [binary, '-s', '-i', commandI, '-m', commandM, '-u', commandU, '-o', outputDir, '-q', path].join(' ');
  // const startTime = process.hrtime();
  const { stdout, stderr } = await exec(cmd);
  const elapsedSeconds = parseHrtimeToSeconds(process.hrtime(startTime));
  console.log('It takes ' + elapsedSeconds + 'seconds');
  // console.log(( err || !saved )?"Error":"Saved");
  const fileJSON = await fs.readFileSync(outputDir + "/sfm_data_expanded.json", 'utf8');
  const obj = JSON.parse(fileJSON);
  const currId = 438;
  const extrinsics = find_extrinsics(obj, currId);
  // const extrinsics = obj.extrinsics[0].value;
  // const extrinsicsKey = obj.extrinsics[0].key;
  // const intrinsics = obj.intrinsics[0].value;
  // const viewKey = obj.views[0].key;
  // const lastFilename = obj.views[0].value.ptr_wrapper.data.filename;
  // if (lastFilename === filename) {
  //   console.log("dobre");
  // } else {
  //   console.log("zle");
  // }
  // if(viewKey === extrinsicsKey){
  if(extrinsics === null){
    console.log('nelocalizovane');
    return;
  } else {
    console.log('localizovane');
  }
  // console.log('asd', extrinsics);
  try {
    const matrix = getPoseMatrix(extrinsics);
    // const matrixProjection = getProjectionMatrix(extrinsics, intrinsics);
    const matrixProjection = [];
    return {matrix, matrixProjection};
  } catch(e){
    console.log('err', e);
    return {matrix: Matrix.identity(4).to2DArray(), matrixProjection: Matrix.identity(4).to2DArray()}
  }
}

function getTranslation(extrinsics) {
  let rotationMatrix = new Matrix(extrinsics.rotation);
  const center = new Matrix(3, 1);
  center.setColumn(0, extrinsics.center);
  let out = rotationMatrix.mmul(center);
  out = Matrix.mul(out, -1);
  // out = Matrix.mul(out, 100);
  return out;
}

function scale(m) {
  // return m;
  // 0.42 dataset 7
  // const factor = 0.42;
  // 1 dataset 8
  // const factor = 1;
  const factor = 1;
  const scaleMatrix = Matrix.identity(4);
  scaleMatrix.set(0,0, factor);
  scaleMatrix.set(1,1, factor);
  scaleMatrix.set(2,2, factor);
  const out = m.mmul(scaleMatrix);
  return out;
}


function getPoseMatrix(extrinsics) {
  let out = Matrix.identity(4);
  const rotation = extrinsics.rotation;
  const t = getTranslation(extrinsics);
  // console.log('translation', t);
  out.setRow(0, [rotation[0][0], rotation[0][1], rotation[0][2], t.get(0,0)]);
  out.setRow(1, [rotation[1][0], rotation[1][1], rotation[1][2], t.get(1,0)]);
  out.setRow(2, [rotation[2][0], rotation[2][1], rotation[2][2], t.get(2,0)]);
  out.setRow(3, [0, 0, 0, 1]);
  out = scale(out);
  out = out.transpose();
  return out.to1DArray();
}

function getProjectionMatrix(extrinsics, intrinsics) {
  // const rotationMatrix = extrinsics.rotation;
  // const center = extrinsics.center;
  // const t = getTranslation(extrinsics);
  // rotationMatrix[0].push(t.get(0, 0));
  // rotationMatrix[1].push(t.get(1, 0));
  // rotationMatrix[2].push(t.get(2, 0));
  // const r = new Matrix(rotationMatrix);
  let k = Matrix.identity(4);
  const pp = intrinsics.ptr_wrapper.data.principal_point;
  const focal = intrinsics.ptr_wrapper.data.focal_length;
  // k.set(0, 2, pp[0]);
  k.set(0, 2, 0.008);
  // k.set(1, 2, pp[1]);
  k.set(1, 2, 0.008);
  // k.set(0, 0, focal);
  k.set(0, 0, 2);
  // k.set(1, 1, focal);
  k.set(1, 1, 1.4);
  // k = Matrix.mul(k, 1/100);
  k.set(2, 2, -1);
  k.set(3, 3, 0);
  k.set(3, 2, -1);
  k.set(2, 3, -0.02);
  // k = Matrix.mul(k, 100);
  // GLdouble perspMatrix[16]={2*fx/w,0,0,0,0,2*fy/h,0,0,2*(cx/w)-1,2*(cy/h)-1,-(far+near)/(far-near),-1,0,0,-2*far*near/(far-near),0};
  let out = k;
  // let out = k.mmul(r);
  // let out = k;
  // out = Matrix.from1DArray(4,4, out.to1DArray().concat([0, 0, -1, 0]));
  // out = out.addRowVector([0, 0, 0, 1]);
  out = scale(out);
  out = out.transpose();
  // console.log('asd');
  return out.to1DArray();
  // return math.matrix(math.ones([4, 4])).valueOf();
}

module.exports = {localizate};