import { Plane, Point } from './Plane.js';

declare const numeric: any;

export interface PointPair {
  source: Point;
  target: Point;
}

export class PlaneTransformer {
  private homographyMatrix: number[][] | null = null;

  constructor(
    private readonly sourcePlane: Plane,
    private readonly targetPlane: Plane,
    private readonly correspondingPoints: PointPair[]
  ) {
    if (correspondingPoints.length < 4) {
      throw new Error('At least 4 point pairs are required for homography transformation');
    }

    this.validatePoints();
    this.computeHomography();
  }

  private validatePoints(): void {
    for (const pair of this.correspondingPoints) {
      if (!this.sourcePlane.isPointInBounds(pair.source)) {
        throw new Error(`Source point ${JSON.stringify(pair.source)} is out of bounds`);
      }
      if (!this.targetPlane.isPointInBounds(pair.target)) {
        throw new Error(`Target point ${JSON.stringify(pair.target)} is out of bounds`);
      }
    }
  }

  private computeHomography(): void {
    const numPoints = this.correspondingPoints.length;
    const A: number[][] = [];
    const b: number[] = [];

    // Build the system of equations Ah = b
    for (let i = 0; i < numPoints; i++) {
      const { source, target } = this.correspondingPoints[i];
      const { x, y } = source;
      const { x: X, y: Y } = target;

      // Add equations for x coordinate
      A.push([x, y, 1, 0, 0, 0, -x*X, -y*X]);
      b.push(X);

      // Add equations for y coordinate
      A.push([0, 0, 0, x, y, 1, -x*Y, -y*Y]);
      b.push(Y);
    }

    try {
      // Solve the system using least squares
      const h = numeric.solve(A, b);

      // Reshape the solution into a 3x3 matrix
      this.homographyMatrix = [
        [h[0], h[1], h[2]],
        [h[3], h[4], h[5]],
        [h[6], h[7], 1.0]
      ];
    } catch (error: any) {
      const errorMessage = error instanceof Error ? error.message : String(error);
      throw new Error('Failed to compute homography matrix: ' + errorMessage);
    }
  }

  public transform(point: Point): Point {
    if (!this.homographyMatrix) {
      throw new Error('Homography matrix has not been computed');
    }

    if (!this.sourcePlane.isPointInBounds(point)) {
      throw new Error('Source point is out of bounds');
    }

    const H = this.homographyMatrix;
    const { x, y } = point;

    // Apply homography transformation
    const w = H[2][0] * x + H[2][1] * y + 1.0;
    const transformedX = (H[0][0] * x + H[0][1] * y + H[0][2]) / w;
    const transformedY = (H[1][0] * x + H[1][1] * y + H[1][2]) / w;

    return { x: transformedX, y: transformedY };
  }

  public getTransformationError(): number {
    if (!this.homographyMatrix) {
      throw new Error('Homography matrix has not been computed');
    }

    let totalError = 0;
    for (const pair of this.correspondingPoints) {
      const transformed = this.transform(pair.source);
      const dx = transformed.x - pair.target.x;
      const dy = transformed.y - pair.target.y;
      totalError += Math.sqrt(dx * dx + dy * dy);
    }

    return totalError / this.correspondingPoints.length;
  }
}