/*
Copyright (c) 2012 Juan Mellado

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

import { type CVPoint } from './cv';
import { svdcmp } from './svd';

/*
References:
- "Iterative Pose Estimation using Coplanar Feature Points"
  Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis
  http://www.cfar.umd.edu/~daniel/daniel_papersfordownload/CoplanarPts.pdf
*/

class Pose {
  bestError: any;
  bestRotation: any;
  bestTranslation: any;
  alternativeError: any;
  alternativeRotation: any;
  alternativeTranslation: any;

  constructor(error1: number, rotation1: number[][], translation1: number[], error2: number, rotation2: number[][], translation2: number[]) {
    this.bestError = error1;
    this.bestRotation = rotation1;
    this.bestTranslation = translation1;
    this.alternativeError = error2;
    this.alternativeRotation = rotation2;
    this.alternativeTranslation = translation2;
  }
}

export class Posit {
  objectPoints: number[][];
  focalLength: number;
  objectVectors: number[][];
  objectNormal: number[];
  objectMatrix: number[][];

  constructor(modelSize: number, focalLength: number) {
    this.objectPoints = this.buildModel(modelSize);
    this.focalLength = focalLength;

    this.objectVectors = [];
    this.objectNormal = [];
    this.objectMatrix = [[], [], []];

    this.init();
  };

  buildModel(modelSize: number): number[][] {
    const half = modelSize / 2.0;

    return [
      [-half, half, 0.0],
      [half, half, 0.0],
      [half, -half, 0.0],
      [-half, -half, 0.0]];
  }

  init(): void {
    const np = this.objectPoints.length;
    const vectors: number[][] = []; const n: any[] = []; let len = 0.0; let row = 2;

    for (let i = 0; i < np; ++i) {
      this.objectVectors[i] = [this.objectPoints[i][0] - this.objectPoints[0][0],
      this.objectPoints[i][1] - this.objectPoints[0][1],
      this.objectPoints[i][2] - this.objectPoints[0][2]];

      vectors[i] = [this.objectVectors[i][0],
      this.objectVectors[i][1],
      this.objectVectors[i][2]];
    }

    while (len === 0.0) {
      n[0] = this.objectVectors[1][1] * this.objectVectors[row][2] -
        this.objectVectors[1][2] * this.objectVectors[row][1];
      n[1] = this.objectVectors[1][2] * this.objectVectors[row][0] -
        this.objectVectors[1][0] * this.objectVectors[row][2];
      n[2] = this.objectVectors[1][0] * this.objectVectors[row][1] -
        this.objectVectors[1][1] * this.objectVectors[row][0];

      len = Math.sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]);

      ++row;
    }

    for (let i = 0; i < 3; ++i) {
      this.objectNormal[i] = n[i] / len;
    }

    this.pseudoInverse(vectors, np, this.objectMatrix);
  }

  pose(imagePoints: CVPoint[]): Pose {
    const posRotation1 = [[], [], []]; const posRotation2 = [[], [], []];
    const posTranslation: number[] = [];
    const rotation1 = [[], [], []]; const rotation2 = [[], [], []];
    const translation1: number[] = [];
    const translation2: number[] = [];
    let error1; let error2; let i; let j;

    this.pos(imagePoints, posRotation1, posRotation2, posTranslation);

    const valid1 = this.isValid(posRotation1, posTranslation);
    if (valid1) {
      error1 = this.iterate(imagePoints, posRotation1, posTranslation, rotation1, translation1);
    } else {
      error1 = { euclidean: -1.0, pixels: -1, maximum: -1.0 };
    }

    const valid2 = this.isValid(posRotation2, posTranslation);
    if (valid2) {
      error2 = this.iterate(imagePoints, posRotation2, posTranslation, rotation2, translation2);
    } else {
      error2 = { euclidean: -1.0, pixels: -1, maximum: -1.0 };
    }

    for (i = 0; i < 3; ++i) {
      for (j = 0; j < 3; ++j) {
        if (valid1) {
          translation1[i] -= rotation1[i][j] * this.objectPoints[0][j];
        }
        if (valid2) {
          translation2[i] -= rotation2[i][j] * this.objectPoints[0][j];
        }
      }
    }

    return error1.euclidean < error2.euclidean
      ? new Pose(error1.pixels, rotation1, translation1, error2.pixels, rotation2, translation2)
      : new Pose(error2.pixels, rotation2, translation2, error1.pixels, rotation1, translation1);
  };

  pos(imagePoints: CVPoint[], rotation1: number[][], rotation2: number[][], translation: number[]): void {
    const np = this.objectPoints.length; const imageVectors: any[] = [];
    const i0: number[] = []; const j0: number[] = []; const ivec: number[] = []; const jvec: number[] = []; const row1: number[] = []; const row2: number[] = []; const row3: number[] = [];
    let i0i0; let j0j0; let i0j0; let delta; let q; let lambda; let mu; let scale; let i; let j;

    for (i = 0; i < np; ++i) {
      imageVectors[i] = [imagePoints[i].x - imagePoints[0].x,
      imagePoints[i].y - imagePoints[0].y];
    }

    // i0 and j0
    for (i = 0; i < 3; ++i) {
      i0[i] = 0.0;
      j0[i] = 0.0;
      for (j = 0; j < np; ++j) {
        i0[i] += this.objectMatrix[i][j] * imageVectors[j][0];
        j0[i] += this.objectMatrix[i][j] * imageVectors[j][1];
      }
    }

    i0i0 = i0[0] * i0[0] + i0[1] * i0[1] + i0[2] * i0[2];
    j0j0 = j0[0] * j0[0] + j0[1] * j0[1] + j0[2] * j0[2];
    i0j0 = i0[0] * j0[0] + i0[1] * j0[1] + i0[2] * j0[2];

    // Lambda and mu
    delta = (j0j0 - i0i0) * (j0j0 - i0i0) + 4.0 * (i0j0 * i0j0);

    if (j0j0 - i0i0 >= 0.0) {
      q = (j0j0 - i0i0 + Math.sqrt(delta)) / 2.0;
    } else {
      q = (j0j0 - i0i0 - Math.sqrt(delta)) / 2.0;
    }

    if (q >= 0.0) {
      lambda = Math.sqrt(q);
      if (lambda === 0.0) {
        mu = 0.0;
      } else {
        mu = -i0j0 / lambda;
      }
    } else {
      lambda = Math.sqrt(-(i0j0 * i0j0) / q);
      if (lambda === 0.0) {
        mu = Math.sqrt(i0i0 - j0j0);
      } else {
        mu = -i0j0 / lambda;
      }
    }

    // First rotation
    for (i = 0; i < 3; ++i) {
      ivec[i] = i0[i] + lambda * this.objectNormal[i];
      jvec[i] = j0[i] + mu * this.objectNormal[i];
    }

    scale = Math.sqrt(ivec[0] * ivec[0] + ivec[1] * ivec[1] + ivec[2] * ivec[2]);

    for (i = 0; i < 3; ++i) {
      row1[i] = ivec[i] / scale;
      row2[i] = jvec[i] / scale;
    }

    row3[0] = row1[1] * row2[2] - row1[2] * row2[1];
    row3[1] = row1[2] * row2[0] - row1[0] * row2[2];
    row3[2] = row1[0] * row2[1] - row1[1] * row2[0];

    for (i = 0; i < 3; ++i) {
      rotation1[0][i] = row1[i];
      rotation1[1][i] = row2[i];
      rotation1[2][i] = row3[i];
    }

    // Second rotation
    for (i = 0; i < 3; ++i) {
      ivec[i] = i0[i] - lambda * this.objectNormal[i];
      jvec[i] = j0[i] - mu * this.objectNormal[i];
    }

    for (i = 0; i < 3; ++i) {
      row1[i] = ivec[i] / scale;
      row2[i] = jvec[i] / scale;
    }

    row3[0] = row1[1] * row2[2] - row1[2] * row2[1];
    row3[1] = row1[2] * row2[0] - row1[0] * row2[2];
    row3[2] = row1[0] * row2[1] - row1[1] * row2[0];

    for (i = 0; i < 3; ++i) {
      rotation2[0][i] = row1[i];
      rotation2[1][i] = row2[i];
      rotation2[2][i] = row3[i];
    }

    // Translation
    translation[0] = imagePoints[0].x / scale;
    translation[1] = imagePoints[0].y / scale;
    translation[2] = this.focalLength / scale;
  }

  isValid(rotation: number[][], translation: number[]): boolean {
    const np = this.objectPoints.length; let zmin = Infinity; let i = 0; let zi;

    for (; i < np; ++i) {
      zi = translation[2] +
        (rotation[2][0] * this.objectVectors[i][0] +
          rotation[2][1] * this.objectVectors[i][1] +
          rotation[2][2] * this.objectVectors[i][2]);
      if (zi < zmin) {
        zmin = zi;
      }
    }

    return zmin >= 0.0;
  }

  iterate(imagePoints: CVPoint[], posRotation: any[][], posTranslation: any[], rotation: number[][], translation: number[]) {
    const np = this.objectPoints.length;
    const oldSopImagePoints: CVPoint[] = []; const sopImagePoints: CVPoint[] = [];
    const rotation1 = [[], [], []]; const rotation2 = [[], [], []];
    const translation1: number[] = []; const translation2: number[] = [];
    let converged = false; let iteration = 0;
    let oldImageDifference; let imageDifference; let factor;
    let error; let error1; let error2; let delta; let i; let j;

    for (i = 0; i < np; ++i) {
      oldSopImagePoints[i] = {
        x: imagePoints[i].x,
        y: imagePoints[i].y
      };
    }

    for (i = 0; i < 3; ++i) {
      for (j = 0; j < 3; ++j) {
        rotation[i][j] = posRotation[i][j];
      }
      translation[i] = posTranslation[i];
    }

    for (i = 0; i < np; ++i) {
      factor = 0.0;
      for (j = 0; j < 3; ++j) {
        factor += this.objectVectors[i][j] * rotation[2][j] / translation[2];
      }
      sopImagePoints[i] = {
        x: (1.0 + factor) * imagePoints[i].x,
        y: (1.0 + factor) * imagePoints[i].y
      };
    }

    imageDifference = 0.0;

    for (i = 0; i < np; ++i) {
      imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x);
      imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y);
    }

    for (i = 0; i < 3; ++i) {
      translation1[i] = translation[i] -
        (rotation[i][0] * this.objectPoints[0][0] +
          rotation[i][1] * this.objectPoints[0][1] +
          rotation[i][2] * this.objectPoints[0][2]);
    }

    error = error1 = this.error(imagePoints, rotation, translation1);

    // Convergence
    converged = (error1.pixels === 0.0) || (imageDifference < 0.01);

    while (iteration++ < 100 && !converged) {
      for (i = 0; i < np; ++i) {
        oldSopImagePoints[i].x = sopImagePoints[i].x;
        oldSopImagePoints[i].y = sopImagePoints[i].y;
      }

      this.pos(sopImagePoints, rotation1, rotation2, translation);

      for (i = 0; i < 3; ++i) {
        translation1[i] = translation[i] -
          (rotation1[i][0] * this.objectPoints[0][0] +
            rotation1[i][1] * this.objectPoints[0][1] +
            rotation1[i][2] * this.objectPoints[0][2]);

        translation2[i] = translation[i] -
          (rotation2[i][0] * this.objectPoints[0][0] +
            rotation2[i][1] * this.objectPoints[0][1] +
            rotation2[i][2] * this.objectPoints[0][2]);
      }

      error1 = this.error(imagePoints, rotation1, translation1);
      error2 = this.error(imagePoints, rotation2, translation2);

      if ((error1.euclidean >= 0.0) && (error2.euclidean >= 0.0)) {
        if (error2.euclidean < error1.euclidean) {
          error = error2;
          for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
              rotation[i][j] = rotation2[i][j];
            }
          }
        } else {
          error = error1;
          for (i = 0; i < 3; ++i) {
            for (j = 0; j < 3; ++j) {
              rotation[i][j] = rotation1[i][j];
            }
          }
        }
      }

      if ((error1.euclidean < 0.0) && (error2.euclidean >= 0.0)) {
        error = error2;
        for (i = 0; i < 3; ++i) {
          for (j = 0; j < 3; ++j) {
            rotation[i][j] = rotation2[i][j];
          }
        }
      }

      if ((error2.euclidean < 0.0) && (error1.euclidean >= 0.0)) {
        error = error1;
        for (i = 0; i < 3; ++i) {
          for (j = 0; j < 3; ++j) {
            rotation[i][j] = rotation1[i][j];
          }
        }
      }

      for (i = 0; i < np; ++i) {
        factor = 0.0;
        for (j = 0; j < 3; ++j) {
          factor += this.objectVectors[i][j] * rotation[2][j] / translation[2];
        }
        sopImagePoints[i].x = (1.0 + factor) * imagePoints[i].x;
        sopImagePoints[i].y = (1.0 + factor) * imagePoints[i].y;
      }

      oldImageDifference = imageDifference;
      imageDifference = 0.0;

      for (i = 0; i < np; ++i) {
        imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x);
        imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y);
      }

      delta = Math.abs(imageDifference - oldImageDifference);

      converged = (error.pixels === 0.0) || (delta < 0.01);
    }

    return error;
  }

  error(imagePoints: CVPoint[], rotation: number[][], translation: number[]) {
    const np = this.objectPoints.length;
    const move: number[][] = []; const projection: number[][] = []; const errorvec: number[][] = [];
    let euclidean = 0.0; let pixels = 0.0; let maximum = 0.0;
    let i; let j; let k;

    if (!this.isValid(rotation, translation)) {
      return { euclidean: -1.0, pixels: -1, maximum: -1.0 };
    }

    for (i = 0; i < np; ++i) {
      move[i] = [];
      for (j = 0; j < 3; ++j) {
        move[i][j] = translation[j];
      }
    }

    for (i = 0; i < np; ++i) {
      for (j = 0; j < 3; ++j) {
        for (k = 0; k < 3; ++k) {
          move[i][j] += rotation[j][k] * this.objectPoints[i][k];
        }
      }
    }

    for (i = 0; i < np; ++i) {
      projection[i] = [];
      for (j = 0; j < 2; ++j) {
        projection[i][j] = this.focalLength * move[i][j] / move[i][2];
      }
    }

    for (i = 0; i < np; ++i) {
      errorvec[i] = [projection[i][0] - imagePoints[i].x,
      projection[i][1] - imagePoints[i].y];
    }

    for (i = 0; i < np; ++i) {
      euclidean += Math.sqrt(errorvec[i][0] * errorvec[i][0] +
        errorvec[i][1] * errorvec[i][1]);

      pixels += Math.abs(Math.round(projection[i][0]) - Math.round(imagePoints[i].x)) +
        Math.abs(Math.round(projection[i][1]) - Math.round(imagePoints[i].y));

      if (Math.abs(errorvec[i][0]) > maximum) {
        maximum = Math.abs(errorvec[i][0]);
      }
      if (Math.abs(errorvec[i][1]) > maximum) {
        maximum = Math.abs(errorvec[i][1]);
      }
    }

    return { euclidean: euclidean / np, pixels, maximum };
  }

  pseudoInverse(a: number[][], n: number, b: number[][]): void {
    const w: number[] = []; const v = [[], [], []]; const s: number[][] = [[], [], []];
    let wmax = 0.0; let cn = 0;
    let i: number; let j: number; let k: number;

    svdcmp(a, n, 3, w, v);

    for (i = 0; i < 3; ++i) {
      if (w[i] > wmax) {
        wmax = w[i];
      }
    }

    wmax *= 0.01;

    for (i = 0; i < 3; ++i) {
      if (w[i] < wmax) {
        w[i] = 0.0;
      }
    }

    for (j = 0; j < 3; ++j) {
      if (w[j] === 0.0) {
        ++cn;
        for (k = j; k < 2; ++k) {
          for (i = 0; i < n; ++i) {
            a[i][k] = a[i][k + 1];
          }
          for (i = 0; i < 3; ++i) {
            v[i][k] = v[i][k + 1];
          }
        }
      }
    }

    for (j = 0; j < 2; ++j) {
      if (w[j] === 0.0) {
        w[j] = w[j + 1];
      }
    }

    for (i = 0; i < 3; ++i) {
      for (j = 0; j < 3 - cn; ++j) {
        s[i][j] = v[i][j] / w[j];
      }
    }

    for (i = 0; i < 3; ++i) {
      for (j = 0; j < n; ++j) {
        b[i][j] = 0.0;
        for (k = 0; k < 3 - cn; ++k) {
          b[i][j] += s[i][k] * a[j][k];
        }
      }
    }
  }
}