/*
 * Copyright 2018 Google Inc.
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "../skcms.h"
#include "LinearAlgebra.h"
#include "PortableMath.h"
#include <float.h>

bool skcms_Matrix4x4_invert(const skcms_Matrix4x4* src, skcms_Matrix4x4* dst) {
    double a00 = src->vals[0][0],
           a01 = src->vals[1][0],
           a02 = src->vals[2][0],
           a03 = src->vals[3][0],
           a10 = src->vals[0][1],
           a11 = src->vals[1][1],
           a12 = src->vals[2][1],
           a13 = src->vals[3][1],
           a20 = src->vals[0][2],
           a21 = src->vals[1][2],
           a22 = src->vals[2][2],
           a23 = src->vals[3][2],
           a30 = src->vals[0][3],
           a31 = src->vals[1][3],
           a32 = src->vals[2][3],
           a33 = src->vals[3][3];

    double b00 = a00*a11 - a01*a10,
           b01 = a00*a12 - a02*a10,
           b02 = a00*a13 - a03*a10,
           b03 = a01*a12 - a02*a11,
           b04 = a01*a13 - a03*a11,
           b05 = a02*a13 - a03*a12,
           b06 = a20*a31 - a21*a30,
           b07 = a20*a32 - a22*a30,
           b08 = a20*a33 - a23*a30,
           b09 = a21*a32 - a22*a31,
           b10 = a21*a33 - a23*a31,
           b11 = a22*a33 - a23*a32;

    double determinant = b00*b11
                       - b01*b10
                       + b02*b09
                       + b03*b08
                       - b04*b07
                       + b05*b06;

    if (determinant == 0) {
        return false;
    }

    double invdet = 1.0 / determinant;
    if (invdet > +FLT_MAX || invdet < -FLT_MAX || !isfinitef_((float)invdet)) {
        return false;
    }

    b00 *= invdet;
    b01 *= invdet;
    b02 *= invdet;
    b03 *= invdet;
    b04 *= invdet;
    b05 *= invdet;
    b06 *= invdet;
    b07 *= invdet;
    b08 *= invdet;
    b09 *= invdet;
    b10 *= invdet;
    b11 *= invdet;

    dst->vals[0][0] = (float)( a11*b11 - a12*b10 + a13*b09 );
    dst->vals[1][0] = (float)( a02*b10 - a01*b11 - a03*b09 );
    dst->vals[2][0] = (float)( a31*b05 - a32*b04 + a33*b03 );
    dst->vals[3][0] = (float)( a22*b04 - a21*b05 - a23*b03 );
    dst->vals[0][1] = (float)( a12*b08 - a10*b11 - a13*b07 );
    dst->vals[1][1] = (float)( a00*b11 - a02*b08 + a03*b07 );
    dst->vals[2][1] = (float)( a32*b02 - a30*b05 - a33*b01 );
    dst->vals[3][1] = (float)( a20*b05 - a22*b02 + a23*b01 );
    dst->vals[0][2] = (float)( a10*b10 - a11*b08 + a13*b06 );
    dst->vals[1][2] = (float)( a01*b08 - a00*b10 - a03*b06 );
    dst->vals[2][2] = (float)( a30*b04 - a31*b02 + a33*b00 );
    dst->vals[3][2] = (float)( a21*b02 - a20*b04 - a23*b00 );
    dst->vals[0][3] = (float)( a11*b07 - a10*b09 - a12*b06 );
    dst->vals[1][3] = (float)( a00*b09 - a01*b07 + a02*b06 );
    dst->vals[2][3] = (float)( a31*b01 - a30*b03 - a32*b00 );
    dst->vals[3][3] = (float)( a20*b03 - a21*b01 + a22*b00 );

    for (int r = 0; r < 4; ++r)
    for (int c = 0; c < 4; ++c) {
        if (!isfinitef_(dst->vals[r][c])) {
            return false;
        }
    }
    return true;
}

bool skcms_Matrix3x3_invert(const skcms_Matrix3x3* src, skcms_Matrix3x3* dst) {
    double a00 = src->vals[0][0],
           a01 = src->vals[1][0],
           a02 = src->vals[2][0],
           a10 = src->vals[0][1],
           a11 = src->vals[1][1],
           a12 = src->vals[2][1],
           a20 = src->vals[0][2],
           a21 = src->vals[1][2],
           a22 = src->vals[2][2];

    double b0 = a00*a11 - a01*a10,
           b1 = a00*a12 - a02*a10,
           b2 = a01*a12 - a02*a11,
           b3 = a20,
           b4 = a21,
           b5 = a22;

    double determinant = b0*b5
                       - b1*b4
                       + b2*b3;

    if (determinant == 0) {
        return false;
    }

    double invdet = 1.0 / determinant;
    if (!isfinitef_((float)invdet)) {
        return false;
    }

    b0 *= invdet;
    b1 *= invdet;
    b2 *= invdet;
    b3 *= invdet;
    b4 *= invdet;
    b5 *= invdet;

    dst->vals[0][0] = (float)( a11*b5 - a12*b4 );
    dst->vals[1][0] = (float)( a02*b4 - a01*b5 );
    dst->vals[2][0] = (float)(        +     b2 );
    dst->vals[0][1] = (float)( a12*b3 - a10*b5 );
    dst->vals[1][1] = (float)( a00*b5 - a02*b3 );
    dst->vals[2][1] = (float)(        -     b1 );
    dst->vals[0][2] = (float)( a10*b4 - a11*b3 );
    dst->vals[1][2] = (float)( a01*b3 - a00*b4 );
    dst->vals[2][2] = (float)(        +     b0 );

    for (int r = 0; r < 3; ++r)
    for (int c = 0; c < 3; ++c) {
        if (!isfinitef_(dst->vals[r][c])) {
            return false;
        }
    }
    return true;
}

skcms_Vector4 skcms_Matrix4x4_Vector4_mul(const skcms_Matrix4x4* m, const skcms_Vector4* v) {
    skcms_Vector4 dst = {{0,0,0,0}};
    for (int row = 0; row < 4; ++row) {
        dst.vals[row] = m->vals[row][0] * v->vals[0]
                      + m->vals[row][1] * v->vals[1]
                      + m->vals[row][2] * v->vals[2]
                      + m->vals[row][3] * v->vals[3];
    }
    return dst;
}