::tomato::mathmatrixTop, Main, Index

proc ::tomato::mathmatrix::CopySubMatrixTo {target storage sourceRowIndex targetRowIndex rowCount sourceColumnIndex targetColumnIndex columnCount} {

    # Copies the values of a given matrix into a region in this matrix.
    #
    # target             - The matrix [Matrix] to copy values into.
    # storage            - The base matrix [Matrix].
    # sourceRowIndex     - The row index of source
    # targetRowIndex     - The row index of target
    # rowCount           - The number of rows to copy
    # sourceColumnIndex  - The column index of source
    # targetColumnIndex  - The column index of source
    # columnCount        - The number of columns to copy
    #
    # Returns matrix copy [Matrix]

    if {$rowCount == 0 || $columnCount == 0} {
        return $target
    }

    if {
        ($sourceRowIndex == 0) && ($targetRowIndex == 0 ) &&
        ($rowCount == [$storage RowCount]) && ($rowCount == [$target RowCount]) &&
        ($sourceColumnIndex == 0) && ($targetColumnIndex == 0) &&
        ($columnCount == [$storage ColumnCount]) && ($columnCount == [$target ColumnCount])
    } {
        return [tomato::mathmatrix::CopyTo $target $storage]
    }

    set sourceValues [$storage Values]
    set endCol [expr {$sourceColumnIndex + $columnCount}]
    set endRow [expr {$sourceRowIndex + $rowCount}]

    for {set j $sourceColumnIndex; set jj $targetColumnIndex} {$j < $endCol} {incr j; incr jj} {
        for {set i $sourceRowIndex; set ii $targetRowIndex} {$i < $endRow} {incr i; incr ii} {
            $target _SetCell $ii $jj [lindex $sourceValues $i $j]
        }
    }

    return $target
}

proc ::tomato::mathmatrix::CopyTo {target storage} {

    # Copies the elements of this matrix to the given matrix.
    #
    # target - The matrix [Matrix] to copy values into.
    # storage - The base matrix [Matrix].
    #
    # Returns matrix [Matrix]

    if {[$storage RowCount] != [$target RowCount] || [$storage ColumnCount] != [$target ColumnCount]} {
        error "Matrix dimensions must agree: op1 is [$storage GetSize], op2 is [$target GetSize]."
    }

    set rows [$storage RowCount]
    for {set i 0} {$i < $rows} {incr i} {
        $target SetRow $i [$storage GetRow $i]
    }

    return $target
}

proc ::tomato::mathmatrix::CreateIdentity {size} {

    # Create a new identity matrix where each diagonal value is set to one.
    #
    # size - dimension of matrix
    #
    # Returns a new base matrix identity [Matrix]
    set mat [tomato::mathmatrix::Matrix new $size $size]

    for {set i 0} {$i < $size} {incr i} {
        $mat _SetCell $i $i 1.0
    }

    return $mat
}

proc ::tomato::mathmatrix::Equals {mat other tolerance} {

    # Indicate if this matrix is equivalent to a another matrix
    #
    # mat   - First input matrix  [Matrix]
    # other - Second input matrix [Matrix]
    # tolerance - A tolerance (epsilon) to adjust for floating point error
    #
    # Returns `True` if matrixes are equal, otherwise false.
    #
    # See : methods == !=
    if {$tolerance < 0} {
        #ruff
        # An error exception is raised if tolerance (epsilon) < 0.
        error "epsilon < 0"
    }

    if {
        ([$mat RowCount] != [$other RowCount]) ||
        ([$mat ColumnCount] != [$other ColumnCount])
    } {
        return 0
    }

    set values1 [$mat Values]
    set values2 [$other Values]

    foreach row1 $values1 row2 $values2 {
        foreach cell1 $row1 cell2 $row2 {
            if {abs($cell1 - $cell2) > $tolerance} {
                return 0
            }
        }
    }
    return 1
}

proc ::tomato::mathmatrix::MatrixDecompose {storage} {

    # `Crout's LU` decomposition for matrix determinant and inverse
    # [docs.microsoft.com](https://docs.microsoft.com/fr-fr/archive/msdn-magazine/2016/july/test-run-matrix-inversion-using-csharp)
    #
    # storage - [Matrix]
    #
    # Returns list with : +1 or -1 according to even or odd number of row permutations,<br>
    #                     combined lower & upper in lummat [Matrix] & row permutations into permmat [Matrix].

    set toggle 1 ; # even (+1) or odd (-1) row permutations
    set n [$storage RowCount]

    # Copy matrix
    set lummat [tomato::mathmatrix::Matrix new $n $n]
    set lummat [tomato::mathmatrix::CopyTo $lummat $storage]

    set permmat [tomato::mathmatrix::Matrix new 1 $n]
    for {set i 0} {$i < $n} {incr i} {
        $permmat _SetCell 0 $i [expr {double($i)}]
    }

    set lumValues [$lummat Values]
    set nMinus1 [expr {$n - 1}]

    for {set j 0} {$j < $nMinus1} {incr j} {
        set max [expr {abs([lindex $lumValues $j $j])}]
        set piv $j

        for {set i [expr {$j + 1}]} {$i < $n} {incr i} {
            set xij [expr {abs([lindex $lumValues $i $j])}]
            if {$xij > $max} {
                set max $xij
                set piv $i
            }
        }

        if {$max < $::tomato::helper::TolGeom} {
            throw {singular} "Matrix is singular: pivot at column $j is too small ($max)"
        }

        if {$piv != $j} {
            set tmp [lindex $lumValues $piv]
            lset lumValues $piv [lindex $lumValues $j]
            lset lumValues $j $tmp

            set t [$permmat _GetCell 0 $piv]
            $permmat _SetCell 0 $piv [$permmat _GetCell 0 $j]
            $permmat _SetCell 0 $j $t

            set toggle [expr {Inv($toggle)}]
        }

        set xjj [lindex $lumValues $j $j]

        if {$xjj != 0.0} {
            set jPlus1 [expr {$j + 1}]

            for {set i $jPlus1} {$i < $n} {incr i} {
                set xij [expr {[lindex $lumValues $i $j] / double($xjj)}]
                lset lumValues $i $j $xij
                set rowI [lindex $lumValues $i]
                set rowJ [lindex $lumValues $j]

                for {set k $jPlus1} {$k < $n} {incr k} {
                    set val [expr {[lindex $rowI $k] - $xij * [lindex $rowJ $k]}]
                    lset rowI $k $val
                }
                lset lumValues $i $rowI
            }
        }
    }

    set lastPivot [expr {abs([lindex $lumValues $nMinus1 $nMinus1])}]
    if {$lastPivot < $::tomato::helper::TolGeom} {
        throw {singular} "Matrix is singular: last pivot is too small ($lastPivot)"
    }

    for {set i 0} {$i < $n} {incr i} {
        $lummat SetRow $i [lindex $lumValues $i]
    }

    return [list $toggle $lummat $permmat]
}

proc ::tomato::mathmatrix::MatrixInverse {storage} {

    # `Crout's LU` decomposition for matrix inverse.
    #
    # storage - A Matrix [Matrix]
    #
    # Returns A Matrix Inverse

    set n [$storage RowCount]
    set result [tomato::mathmatrix::Matrix new $n $n]
    set result [tomato::mathmatrix::CopyTo $result $storage]

    lassign [$storage Decompose] toggle lum perm

    set b [tomato::mathmatrix::Matrix new 1 $n]

    for {set i 0} {$i < $n} {incr i} {
        for {set j 0} {$j < $n} {incr j} {
            if {$i == [$perm _GetCell 0 $j]} {
                $b _SetCell 0 $j 1.0
            } else {
                $b _SetCell 0 $j 0.0
            }
        }

        set x [tomato::mathmatrix::_MatrixHelper $lum $b]
        for {set j 0} {$j < $n} {incr j} {
            $result _SetCell $j $i [$x _GetCell 0 $j]
        }
    }

    return $result
}

proc ::tomato::mathmatrix::MatrixMulByDouble {mat other} {

    # Multiply matrix by double value
    #
    # mat   - [Matrix]
    # other - double value
    #
    # Returns mul matrix [Matrix]
    set rows [$mat RowCount]
    set cols [$mat ColumnCount]

    for {set row 0} {$row < $rows} {incr row} {
        set currentRow [$mat GetRow $row]
        set newRow {}
        foreach cell $currentRow {
            lappend newRow [expr {$cell * $other}]
        }
        $mat SetRow $row $newRow
    }

    return $mat
}

proc ::tomato::mathmatrix::MatrixMulColumnVector {mat other} {

    # Multiply matrix by column vector.<br>
    # From package tcllib [math::linearalgebra](https://core.tcl-lang.org/tcllib/doc/trunk/embedded/md/tcllib/files/modules/math/linalg.md)
    # *(Modify according to my needs)*.
    #
    # mat   - [Matrix]
    # other - [Matrix]
    #
    # Returns Tcl list (mul matrix)
    set newvect {}
    set mat1 [$mat Values]
    set vec1 {}
    foreach row [$other Values] {
        lappend vec1 [lindex $row 0]
    }

    foreach row $mat1 {
        set sum 0.0
        foreach v $vec1 c $row {
            set sum [expr {$sum + $v * $c}]
        }
        lappend newvect $sum
    }

    return $newvect
}

proc ::tomato::mathmatrix::MatrixMulMatrix {mat other} {

    # Multiply matrix by matrix.<br>
    # From package tcllib [math::linearalgebra](https://core.tcl-lang.org/tcllib/doc/trunk/embedded/md/tcllib/files/modules/math/linalg.md)
    # *(Modify according to my needs)*.
    #
    # mat   - base matrix [Matrix]
    # other - Second matrix [Matrix]
    #
    # Returns mul matrix [Matrix]

    set resultRows [$mat RowCount]
    set resultCols [$other ColumnCount]
    set result     [tomato::mathmatrix::Matrix new $resultRows $resultCols]

    # Transposer 'other' et récupérer les valeurs
    set tmat [[$other Transpose] Values]
    set mat1 [$mat Values]
    set r 0

    foreach row1 $mat1 {
        set newrow {}
        foreach row2 $tmat {
            set sum 0.0
            foreach c1 $row1 c2 $row2 {
                set sum [expr {$sum + $c1 * $c2}]
            }
            lappend newrow $sum
        }
        $result SetRow $r $newrow
        incr r
    }

    return $result
}

proc ::tomato::mathmatrix::MatrixMulVector {mat other} {

    # Multiply matrix by vector.<br>
    # From package tcllib [math::linearalgebra](https://core.tcl-lang.org/tcllib/doc/trunk/embedded/md/tcllib/files/modules/math/linalg.md)
    # *(Modify according to my needs)*.
    #
    # mat   - base matrix [Matrix]
    # other - vector [mathvec3d::Vector3d]
    #
    # Returns Tcl list (mul matrix)
    set newvect {}
    set mat1 [$mat Values]
    set vec1 [$other Get]

    foreach row $mat1 {
        set sum 0.0
        foreach v $vec1 c $row {
            set sum [expr {$sum + $v * $c}]
        }
        lappend newvect $sum

    }

    return $newvect
}

proc ::tomato::mathmatrix::SetNewDataListMatrix {data} {

    # Create a new base matrix [Matrix] based on a list
    #
    # data - list
    #
    # Returns A new [Matrix]

    set row    [llength $data]
    set column [llength [lindex $data 0]]

    set mat [tomato::mathmatrix::Matrix new $row $column]
    set r 0

    foreach row $data {
        $mat SetRow $r $row
        incr r
    }

    return $mat
}

method constructor {row col {value 0.0}} {

    set _rowCount    $row
    set _columnCount $col

    if {[llength [info level 0]] == 5} {
        set value [expr {double($value)}]
    }

    set _values [lrepeat $row [lrepeat $col $value]]
}

method != {other {tolerance {$::tomato::helper::TolEquals}}} {

    # Gets value that indicates whether any pair of elements in two specified matrixes is not equal.
    #
    # other - The second matrix [Matrix] to compare.
    # tolerance - A tolerance (epsilon) to adjust for floating point error.
    #
    # Returns `True` if the matrixes are different. Otherwise `False`.
    if {[llength [info level 0]] < 4} {
        set tolerance $::tomato::helper::TolEquals
    }

    return [expr {![tomato::mathmatrix::Equals [self] $other $tolerance]}]
}

method == {other {tolerance {$::tomato::helper::TolEquals}}} {

    # Gets value that indicates whether each pair of elements in two specified matrixes is equal.
    #
    # other     - The second matrix [Matrix] to compare.
    # tolerance - A tolerance (epsilon) to adjust for floating point error.
    #
    # Returns `True` if the matrixes are the same. Otherwise `False`.
    if {[llength [info level 0]] < 4} {
        set tolerance $::tomato::helper::TolEquals
    }

    return [expr {[tomato::mathmatrix::Equals [self] $other $tolerance]}]
}

method _GetCell {rowIndex colIndex} {

    # Gets the cell value of base matrix (internal use only)
    # There is no error checking
    #
    # rowIndex - index of row
    # colIndex - index of column
    #
    # Returns value of row/column
    return [lindex $_values $rowIndex $colIndex]
}

method _SetCell {rowIndex colIndex value} {

    # Set the cell value of base matrix (internal use only)
    # There is no error checking
    #
    # rowIndex - index of row
    # colIndex - index of column
    # value    - value of row/column
    #
    # Returns nothing
    lset _values $rowIndex $colIndex $value
    return {}
}

method Add {other} {

    # Add matrix to another matrix
    #
    # other - [Matrix]
    #
    # Return a new base matrix [Matrix]
    if {($_rowCount != [$other RowCount]) || ($_columnCount != [$other ColumnCount])} {
        error "DimensionsDontMatch..."
    }

    set result [tomato::mathmatrix::Matrix new $_rowCount $_columnCount]
    set otherValues [$other Values]

    set i 0
    foreach row1 $_values row2 $otherValues {
        set newRow {}
        foreach c1 $row1 c2 $row2 {
            lappend newRow [expr {$c1 + $c2}]
        }
        $result SetRow $i $newRow
        incr i
    }

    return $result
}

method ColumnCount {} {

    # Gets the number of columns.
    return $_columnCount
}

method Decompose {} {

    # The combined lower-upper decomposition of matrix
    #
    # See also: MatrixDecompose
    return [tomato::mathmatrix::MatrixDecompose [self]]
}

method Determinant {} {

    # Gets the determinant of the matrix for which the `Crout's LU`
    # decomposition was computed.
    #
    # Returns determinant
    lassign [my Decompose] toggle lum perm
    set result $toggle

    set lumValues [$lum Values]
    for {set i 0} {$i < $_rowCount} {incr i} {
        set result [expr {$result * [lindex $lumValues $i $i]}]
    }

    return $result
}

method GetCell {rowIndex colIndex} {

    # Gets the cell value of base matrix
    #
    # rowIndex - index of row
    # colIndex - index of column
    #
    # Returns value of row/column
    if {$colIndex >= $_columnCount || $rowIndex >= $_rowCount} {
        error "Index out of bounds"
    }
    return [lindex $_values $rowIndex $colIndex]
}

method GetColumn {colIndex} {

    # Gets Column of base matrix.
    #
    # colIndex  - index of column
    #
    # Returns values of column
    if {$colIndex >= $_columnCount} {
        error "Index Column out of bounds"
    }

    set valcolumn {}
    foreach row $_values {
        lappend valcolumn [lindex $row $colIndex]
    }

    return $valcolumn
}

method GetRow {rowIndex} {

    # Get Row of base matrix.
    #
    # rowIndex  - index of row
    #
    # Returns values of row
    if {$rowIndex >= $_rowCount} {
        error "Index Row out of bounds"
    }

    return [lindex $_values $rowIndex]
}

method GetSize {} {

    # Returns the size of this object.
    return [format {%sx%s} $_rowCount $_columnCount]
}

method GetType {} {

    # Gets the name of class.
    return [tomato::helper::TypeClass [self]]
}

method Inverse {} {

    # Matrix inverse using `Crout's LU` decomposition
    return [tomato::mathmatrix::MatrixInverse [self]]
}

method IsOrthogonal {} {

    # Gets if matrix is orthogonal
    if {$_columnCount != $_rowCount} {
        # error "Matrix is not square."
        return 0
    }

    return [[[my Transpose] Multiply [self]] == [tomato::mathmatrix::CreateIdentity $_rowCount]]
}

method Multiply {entity} {

    # Multiply matrix.
    #
    # entity - Options described below.
    #
    # value  - A double value
    # Matrix - [Matrix]
    # Vector - [mathvec3d::Vector3d]
    #
    # Returns a new base matrix [Matrix] if double value , a Tcl list for other types
    if {[string is double $entity]} {
        set mat [oo::copy [self]]
        return [tomato::mathmatrix::MatrixMulByDouble $mat $entity]
    }

    switch -exact -- [$entity GetType] {
        "::tomato::mathmatrix::Matrix" {
            if {$_columnCount != [$entity RowCount]} {
                error "DimensionsDontMatch..."
            }

            set mat [oo::copy [self]]
            if {[$entity ColumnCount] == 1} {
                return [tomato::mathmatrix::MatrixMulColumnVector $mat $entity]
            }
            return [tomato::mathmatrix::MatrixMulMatrix $mat $entity]
        }
        "::tomato::mathvec3d::Vector3d" {
            if {$_columnCount != 3 || $_rowCount != 3} {
                error "Matrix must be 3x3"
            }
            set mat [oo::copy [self]]
            return [tomato::mathmatrix::MatrixMulVector $mat $entity]
        }
        default {
            error "Entity must be Matrix, Vector3d or Double Value."
        }
    }
}

method RowCount {} {

    # Gets the number of rows.
    return $_rowCount
}

method SetCell {rowIndex colIndex value} {

    # Set the cell value of base matrix
    #
    # rowIndex - index of row
    # colIndex - index of column
    # value    - value of row/column
    #
    # Returns nothing
    if {$colIndex >= $_columnCount || $rowIndex >= $_rowCount} {
        error "Index out of bounds"
    }
    lset _values $rowIndex $colIndex $value

    return {}
}

method SetColumn {colIndex columnlistvalues} {

    # Set Column of base matrix.
    #
    # colIndex  - index of column
    # columnlistvalues - values of column
    #
    # Returns nothing
    if {$colIndex >= $_columnCount} {
        error "Index Column out of bounds"
    }
    if {[llength $columnlistvalues] != $_rowCount} {
        error "All vectors must have the same dimensionality."
    }

    set i 0
    foreach val $columnlistvalues {
        lset _values $i $colIndex $val
        incr i
    }

    return {}
}

method SetRow {rowIndex rowlistvalues} {

    # Set Row of base matrix.
    #
    # rowIndex  - index of row
    # rowlistvalues - values of row
    #
    # Returns nothing
    if {$rowIndex >= $_rowCount} {
        error "Index Row out of bounds"
    }
    if {[llength $rowlistvalues] != $_columnCount} {
        error "All vectors must have the same dimensionality."
    }

    lset _values $rowIndex $rowlistvalues

    return {}
}

method SetSubMatrix {args} {

    # Copies the values of a given matrix into a region in this matrix.
    #
    # args - Options described below.
    #
    # rowIndex    - The row to start copying to.
    # rowCount    - The number of rows to copy. Must be positive.
    # subMatrix   - The sub-matrix to copy from.
    # Or          -
    # rowIndex    - The row to start copying to.
    # rowCount    - The number of rows to copy. Must be positive.
    # columnIndex - The column to start copying to.
    # columnCount - The number of columns to copy. Must be positive.
    # subMatrix   - The sub-matrix to copy from.
    #
    # Returns base matrix [Matrix]
    set target [self]

    if {[llength $args] == 3} {

        lassign $args rowIndex columnIndex subMatrix
        return [tomato::mathmatrix::CopySubMatrixTo $target $subMatrix 0 $rowIndex [$subMatrix RowCount] 0 $columnIndex [$subMatrix ColumnCount]]

    } elseif {[llength $args] == 5} {

        lassign $args rowIndex rowCount columnIndex columnCount subMatrix
        return [tomato::mathmatrix::CopySubMatrixTo $target $subMatrix 0 $rowIndex $rowCount 0 $columnIndex $columnCount]

    } else {
        error "Wrong number of arguments, expected 3 or 5 for SetSubMatrix method."
    }
}

method SubMatrix {rowIndex rowCount columnIndex columnCount} {

    # Creates a matrix that contains the values from the requested sub-matrix.
    #
    # rowIndex    - The row to start copying from.
    # rowCount    - The number of rows to copy. Must be positive.
    # columnIndex - The column to start copying from.
    # columnCount - The number of columns to copy. Must be positive.
    #
    # Returns The requested sub-matrix [Matrix].
    set target [self]

    set storage [tomato::mathmatrix::Matrix new $rowCount $columnCount]
    return [tomato::mathmatrix::CopySubMatrixTo $storage $target $rowIndex 0 $rowCount $columnIndex 0 $columnCount]
}

method ToCsys {} {

    # Returns a csys from Matrix.
    if {($_rowCount != 4) && ($_columnCount != 4)} {
        error "Must be a Matrix 4x4."
    }
    return [tomato::mathcsys::Csys new [self]]
}

method ToMatrixString {} {

    # Returns a string representation of this object.
    return [join [my Values] "\n"]
}

method Transpose {} {

    # Transpose a matrix.
    # [mathematica Transpose](http://reference.wolfram.com/mathematica/ref/Transpose.html)
    #
    # Returns a new transposed matrix [Matrix]

    set transpose [tomato::mathmatrix::Matrix new $_columnCount $_rowCount]
    for {set c 0} {$c < $_columnCount} {incr c} {
        set newRow {}
        foreach row $_values {
            lappend newRow [lindex $row $c]
        }
        $transpose SetRow $c $newRow
    }

    return $transpose
}

method Values {} {

    # Gets the matrix's data.
    return $_values
}