package com.wat;

import Jama.Matrix;

import java.util.Arrays;

import static java.lang.Math.*;

import java.util.Comparator;

public class Main {
    private static int matrixSize = 3;
    private static boolean OPTIMIZE_MULTIPLICATION_SUM = true;

    public static void main(String[] args) {
        Matrix lMatrix = getL(Matrix.random(matrixSize, matrixSize));

        Matrix identityMatrix = Matrix.identity(matrixSize, matrixSize);

        Matrix invertedMatrix = getInverted(lMatrix, identityMatrix);

        Matrix selfIdentityMatrix = multiply(lMatrix, invertedMatrix);

        lMatrix.print(5, 5);
        invertedMatrix.print(5, 5);
        selfIdentityMatrix.print(5, 5);

        double error = 0;
        for (int i = 0; i < matrixSize; i++)
            for (int j = 0; j < matrixSize; j++)
                error += Math.sqrt(Math.pow(selfIdentityMatrix.get(i, j), 2) + Math.pow(identityMatrix.get(i, j), 2));

        System.out.println("Error: " + error);
    }

    private static Matrix getL(Matrix base) {
        Matrix l = new Matrix(matrixSize, matrixSize);

        for (int i = 0; i < matrixSize; i++)
            for (int j = 0; j < matrixSize; j++) {
                if (i >= j)
                    l.set(i, j, base.get(i, j) * 1);
            }

        return l;
    }

    private static Matrix getInverted(Matrix base, Matrix identityMatrix) {
        Matrix calculated = calculate(base, identityMatrix);
        return inverse(calculated);
    }

    private static Matrix calculate(Matrix base, Matrix identityMatrix) {
        double[][] result = new double[matrixSize][matrixSize];

        for (int i = 0; i < matrixSize; i++) {
            double[] identityColumn = identityMatrix.getMatrix(0, matrixSize - 1, i, i).transpose().getArray()[0];

            result[i][0] = (identityColumn[0]) / base.get(0, 0);

            for (int j = 1; j < matrixSize; j++) {
                double[] baseRow = base.getMatrix(j, j, 0, matrixSize - 1).getArray()[0];
                result[i][j] = (identityColumn[j] - getMultiplicationSum(baseRow, result[i])) / base.get(j, j);
            }

        }

        return new Matrix(result);
    }

    private static double getMultiplicationSum(double[] row, double[] column) {
        if (OPTIMIZE_MULTIPLICATION_SUM) {
            Double[] sums = new Double[matrixSize];
            for (int i = 0; i < matrixSize; i++)
                sums[i] = row[i] * column[i];

            Arrays.sort(sums, new Comparator<Double>() {
                @Override
                public int compare(Double o1, Double o2) {
                    return Double.compare(Math.abs(o1), Math.abs(o2));
                }
            });
            double sum = 0;

            for (Double item : sums) {
                sum += item;
            }

            return sum;
        } else {
            double sum = 0;
            for (int i = 0; i < matrixSize; i++)
                sum += row[i] * column[i];
            return sum;
        }
    }

    private static Matrix inverse(Matrix base) {
        double[][] inverted = new double[matrixSize][matrixSize];
        for (int i = 0; i < matrixSize; i++)
            for (int j = 0; j < matrixSize; j++)
                inverted[i][j] = base.get(j, i);

        return new Matrix(inverted);
    }

    private static Matrix multiply(Matrix first, Matrix second) {
        Matrix result = new Matrix(matrixSize, matrixSize);
        for (int i = 0; i < matrixSize; i++)
            for (int j = 0; j < matrixSize; j++) {
                double s = 0;
                for (int k = 0; k < matrixSize; k++)
                    s += first.get(i, k) * second.get(k, j);
                result.set(i, j, s);
            }
        return result;
    }
}