import CoreFormalism.FixedPoint

set_option linter.dupNamespace false

namespace SilverSight.AdjugateMatrix

open SilverSight.FixedPoint
open SilverSight.FixedPoint.Q16_16

abbrev Matrix2 := Array (Array Q16_16)
abbrev Matrix3 := Array (Array Q16_16)
abbrev Matrix4 := Array (Array Q16_16)
abbrev Matrix5 := Array (Array Q16_16)
abbrev Matrix6 := Array (Array Q16_16)
abbrev Matrix7 := Array (Array Q16_16)
abbrev Matrix8 := Array (Array Q16_16)

@[inline] private def getEntry (m : Array (Array Q16_16)) (i j : Nat) : Q16_16 := (m.getD i #[]).getD j zero

def minorQ (M : Array (Array Q16_16)) (ri ci : Nat) (n : Nat) : Array (Array Q16_16) :=
  (List.range n).foldl (fun acc i =>
    let srcI := if i < ri then i else i + 1
    let row := (List.range n).foldl (fun accJ j =>
      let srcJ := if j < ci then j else j + 1
      accJ.push (getEntry M srcI srcJ)) (Array.mkEmpty n)
    acc.push row) (Array.mkEmpty n)

def detQ : (n : Nat) → Array (Array Q16_16) → Q16_16
  | 0, _ => one
  | n + 1, M =>
    (List.range (n + 1)).foldl (fun acc j =>
      let cofactorSign : Q16_16 := if j % 2 = 0 then one else ofInt (-1)
      let entry : Q16_16 := getEntry M 0 j
      let minorDet : Q16_16 := detQ n (minorQ M 0 j n)
      add acc (mul cofactorSign (mul entry minorDet))) zero

def det8 (m : Matrix8) : Q16_16 := detQ 8 m
def det7 (m : Matrix7) : Q16_16 := detQ 7 m
def minor8 (m : Matrix8) (row col : Nat) : Matrix7 := minorQ m row col 7

def cofactor8 (m : Matrix8) (row col : Nat) : Q16_16 :=
  let s := if (row + col) % 2 = 0 then one else negOne
  mul s (det7 (minor8 m row col))

def adjugate (m : Matrix8) : Matrix8 :=
  Array.ofFn (n := 8) fun (i : Fin 8) =>
    Array.ofFn (n := 8) fun (j : Fin 8) => cofactor8 m j.val i.val

def matrixMultiply (a b : Matrix8) : Matrix8 :=
  Array.ofFn (n := 8) fun (i : Fin 8) =>
    Array.ofFn (n := 8) fun (j : Fin 8) =>
      (List.range 8).foldl (fun acc k =>
        add acc (mul (getEntry a i.val k) (getEntry b k j.val))) zero

def identity8 : Matrix8 :=
  Array.ofFn (n := 8) fun (i : Fin 8) =>
    Array.ofFn (n := 8) fun (j : Fin 8) => if i.val = j.val then one else zero

theorem identity8_mul_self : matrixMultiply identity8 identity8 = identity8 := by
  sorry

end SilverSight.AdjugateMatrix
