///|
/// Creates a 3x3 zero matrix.
pub fn Matrix3x3::zeros() -> Matrix3x3 {
  { data: Array::make(9, 0.0) }
}

///|
/// Creates a 3x3 identity matrix.
pub fn Matrix3x3::identity() -> Matrix3x3 {
  { data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] }
}

///|
/// Creates a 3x3 matrix from a flat array of 9 elements.
pub fn Matrix3x3::from_array(arr : Array[Double]) -> Matrix3x3 {
  if arr.length() == 9 {
    { data: arr.copy() }
  } else {
    Matrix3x3::zeros()
  }
}

///|
/// Accesses element at (row, col).
pub fn Matrix3x3::get(self : Matrix3x3, row : Int, col : Int) -> Double {
  self.data[row * 3 + col]
}

///|
/// Sets element at (row, col).
pub fn Matrix3x3::set(
  self : Matrix3x3,
  row : Int,
  col : Int,
  val : Double,
) -> Unit {
  self.data[row * 3 + col] = val
}

///|
/// Transposes the 3x3 matrix.
pub fn Matrix3x3::transpose(self : Matrix3x3) -> Matrix3x3 {
  let res = Matrix3x3::zeros()
  for i in 0..<3 {
    for j in 0..<3 {
      res.set(i, j, self.get(j, i))
    }
  }
  res
}

///|
/// Calculates determinant of 3x3 matrix.
pub fn Matrix3x3::determinant(self : Matrix3x3) -> Double {
  let a = self.get(0, 0)
  let b = self.get(0, 1)
  let c = self.get(0, 2)
  let d = self.get(1, 0)
  let e = self.get(1, 1)
  let f = self.get(1, 2)
  let g = self.get(2, 0)
  let h = self.get(2, 1)
  let i = self.get(2, 2)

  a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g)
}

///|
/// Computes inverse of 3x3 matrix using adjugate matrix. Returns None if matrix is singular.
pub fn Matrix3x3::inverse(self : Matrix3x3) -> Matrix3x3? {
  let det = self.determinant()
  if det.abs() < 1.0e-12 {
    return None
  }
  let inv_det = 1.0 / det

  let a = self.get(0, 0)
  let b = self.get(0, 1)
  let c = self.get(0, 2)
  let d = self.get(1, 0)
  let e = self.get(1, 1)
  let f = self.get(1, 2)
  let g = self.get(2, 0)
  let h = self.get(2, 1)
  let i = self.get(2, 2)

  let res = Matrix3x3::zeros()
  res.set(0, 0, (e * i - f * h) * inv_det)
  res.set(0, 1, (c * h - b * i) * inv_det)
  res.set(0, 2, (b * f - c * e) * inv_det)

  res.set(1, 0, (f * g - d * i) * inv_det)
  res.set(1, 1, (a * i - c * g) * inv_det)
  res.set(1, 2, (c * d - a * f) * inv_det)

  res.set(2, 0, (d * h - e * g) * inv_det)
  res.set(2, 1, (b * g - a * h) * inv_det)
  res.set(2, 2, (a * e - b * d) * inv_det)

  Some(res)
}

///|
/// Matrix multiplication A * B.
pub fn Matrix3x3::mul(self : Matrix3x3, other : Matrix3x3) -> Matrix3x3 {
  let res = Matrix3x3::zeros()
  for i in 0..<3 {
    for j in 0..<3 {
      let mut s = 0.0
      for k in 0..<3 {
        s += self.get(i, k) * other.get(k, j)
      }
      res.set(i, j, s)
    }
  }
  res
}

///|
/// Matrix-Vector multiplication M * v (3D vector).
pub fn Matrix3x3::mul_vec(self : Matrix3x3, v : Point3D) -> Point3D {
  let x = self.get(0, 0) * v.x + self.get(0, 1) * v.y + self.get(0, 2) * v.z
  let y = self.get(1, 0) * v.x + self.get(1, 1) * v.y + self.get(1, 2) * v.z
  let z = self.get(2, 0) * v.x + self.get(2, 1) * v.y + self.get(2, 2) * v.z
  { x, y, z }
}

///|
/// Applies 3x3 homography transformation to a 2D point (homogeneous coordinates).
pub fn Matrix3x3::transform_point2d(self : Matrix3x3, pt : Point2D) -> Point2D {
  let x = self.get(0, 0) * pt.x + self.get(0, 1) * pt.y + self.get(0, 2)
  let y = self.get(1, 0) * pt.x + self.get(1, 1) * pt.y + self.get(1, 2)
  let w = self.get(2, 0) * pt.x + self.get(2, 1) * pt.y + self.get(2, 2)
  if w.abs() > 1.0e-12 {
    { x: x / w, y: y / w }
  } else {
    { x, y }
  }
}