util.math = {}

--- calculates the determinant of a 2x2 matrix
--- @param M table input matrix
--- @return number
function util.math.det2x2(M)
    if type(M) ~= "table" then error(util.error.argErr(M, "table", 1)) end

    return M.x1*M.y2 - M.y1*M.x2
end

--- attemts to solve a generic 2x2 linear system
---
--- INFO: this is an implementation of Cramers Rule.
---
--- WARNING: if your system is either unsolvable or implements redundant equations this function will return nil
--- @param M table input matrix ({ x1 = <>, x2 = <>, y1 = <>, y2 = <> })
--- @param s table input solution vector ({ x1 = <>, y1 = <> })
--- @return table ({ x1 = <>, y1 = <> })
function util.math.solve2x2(M, s)
    if type(M) ~= "table" then error(util.error.argErr(M, "table", 1)) end
    if type(s) ~= "table" then error(util.error.argErr(s, "table", 2)) end

    -- combinatori matrix of vector M1 and s
    local M1 = { x1 = s.x1, x2 = M.x2,
                 y1 = s.y1, y2 = M.y2, }

    -- combinatori matrix of vector M2 and s
    local M2 = { x1 = M.x1, x2 = s.x1,
                 y1 = M.y1, y2 = s.y1,  }

    -- calculate determinant of M if 0 return nil (not solvable / infinit solutions)
    local detM = util.math.det2x2(M)

    -- someone claimed e-52 is the maximum float precision
    if math.abs(detM) <= 1e-52 then error("system not solvable (determine 0)") end

    -- calculate determinants of M1 s and M2 s combinations 
    local detM1 = util.math.det2x2(M1)
    local detM2 = util.math.det2x2(M2)

    -- return the value according to Cramers rule
    return { x1 = detM1 / detM, y1 = detM2 / detM }
end

--- transforms polar coordinates to cartesian (complex algebra)
--- @param phi number angle in radian
--- @param r number radius / distance to origin
--- @return number, number @x, y
function util.math.polToCar(phi, r)
    if not tonumber(phi) then error(util.error.argErr(phi, "number", 1)) end
    if not tonumber(r) then error(util.error.argErr(r, "number", 2)) end

    return r * math.cos(phi), r * math.sin(phi)
end

--- transforms cartesian to polar (complex algebra)
--- @param x number
--- @param y number
--- @param mCorrect? boolean if true the function will return mathematical correct ranges [-pi, pi] instead of [0, 2pi]
--- @return number, number @(angle, radius)
function util.math.carToPol(x, y, mCorrect)
    if not tonumber(x) then error(util.error.argErr(x, "number", 1)) end
    if not tonumber(y) then error(util.error.argErr(y, "number", 2)) end
    mCorrect = mCorrect or false

    -- atan2 tends to be very fishy in lua. Seems to be fine in DCS tho
    local r = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
    local phi = math.atan2(y, x)

    -- transform [-pi, pi] to [0, 2pi] if the mathematical correctness is not needed (easier for headings)
    if not mCorrect then phi = math.fmod(phi + 2*math.pi , 2*math.pi) end

    return phi, r
end

--- transforms radians to degrees
--- @param phi number input radians
--- @return number @degree
function util.math.radToDeg(phi)
    if not tonumber(phi) then error(util.error.argErr(phi, "number", 1)) end

    return phi * 180/math.pi
end

--- transforms degrees to radian
--- @param alpha number input degrees
--- @return number|nil, nil|string @radian
function util.math.degToRad(alpha)
    if not tonumber(alpha) then error(util.error.argErr(alpha, "number", 1)) end

    return alpha * math.pi/180
end

--- subracts v2 from v1 and returns the result in a new array (v1 and v2 unmodiefied)
--- @param v1 table vector 1 in form {a,b,c,...} or {x=a,y=b,z=c,...}
--- @param v2 table vector 2 in form {a,b,c,...} or {x=a,y=b,z=c,...}
--- @return table result
function util.math.vecSub(v1, v2)
    if type(v1) ~= "table" then error(util.error.argErr(v1, "table", 1)) end
    if type(v2) ~= "table" then error(util.error.argErr(v2, "table", 2)) end

    local dimV1, dimV2 = #util.misc.getKeys(v1), #util.misc.getKeys(v2)
    if dimV1 ~= dimV2 then
        error(util.error.preconditionFailedErr("dimensions differ (v1: "..tostring(dimV1)..", v2: "..tostring(dimV2)..")")) 
    end

    local res = {}

    -- go through all dimensions and subtract pairwise (order does not matter only dimensions)
    for k, iv1 in pairs(v1) do
        local iv2 = v2[k]
        if iv2 == nil then error(util.error.preconditionFailedErr("Dimension '"..tostring(k).."' present in v1 but not v2")) end

        res[k] = iv1 - iv2
    end

    return res
end

--- scales a vector with provided factor (does not modify v1)
--- @param v1 table vector 1 in form {a,b,c,...} or {x=a,y=b,z=c,...}
--- @param factor number scale factor
--- @return table result
function util.math.vecScale(v1, factor)
    if type(v1) ~= "table" then error(util.error.argErr(v1, "table", 1)) end
    if not tonumber(factor) then error(util.error.argErr(factor, "number", 2)) end

    -- scale every individual element
    local res = {}
    for k, v in pairs(v1) do
        res[k] = factor * v
    end
    return res
end

--- creates the dot product of v1 and v2
--- @param v1 table vector 1 in form {a,b,c,...} or {x=a,y=b,z=c,...}
--- @param v2 table vector 2 in form {a,b,c,...} or {x=a,y=b,z=c,...}
--- @return table result
function util.math.dotPro(v1, v2)
    if type(v1) ~= "table" then error(util.error.argErr(v1, "table", 1)) end
    if type(v2) ~= "table" then error(util.error.argErr(v2, "table", 2)) end

    local dimV1, dimV2 = #util.misc.getKeys(v1), #util.misc.getKeys(v2)
    if dimV1 ~= dimV2 then
        error(util.error.preconditionFailedErr("dimensions differ (v1: "..tostring(dimV1)..", v2: "..tostring(dimV2)..")")) 
    end

    local res = 0

    -- go through all dimensions and add multiplication pairwise (order does not matter because distributive property in R)
    for k, iv1 in pairs(v1) do
        local iv2 = v2[k]
        if iv2 == nil then error(util.error.preconditionFailedErr("Dimension '"..tostring(k).."' present in v1 but not v2")) end

        res = res + (iv1 * iv2)
    end

    return res
end

--- normalizes a vector (unit vector)
--- @param v1 table input vector in form {a,b,c,...} or {x=a,y=b,z=c,...}
--- @return table result #vector with same direction but abs(1)
function util.math.vecNormalize(v1)
    if type(v1) ~= "table" then error(util.error.argErr(v1, "table", 1)) end

    -- calculate absolute of vector
    local abs = util.math.vecAbs(v1)

    -- return normalized vector
    return util.math.vecScale(v1, 1/abs)
end

--- calculates the absolute of a given vector
--- @param v1 table input vector
--- @return number abs
function util.math.vecAbs(v1)
    if type(v1) ~= "table" then error(util.error.argErr(v1, "table", 1)) end

    -- calculate absolute of vector
    local abs = 0
    for k, v in pairs(v1) do
        abs = abs + tonumber(v)^2
    end
    return math.sqrt(abs)
end

--- generates a random value. Probability is exponentially distributed
--- @param a number sets the exponent of the distribution. Defaults to 2
--- @param i number lower border. Defaults to 0
--- @param j number upper border. Defaults to 1
--- @return number rand
function util.math.randExp(a, i, j)
    a, i, j = tonumber(a) or 2, tonumber(i) or 0, tonumber(j) or 1

    local b = (j-i)
    local x = math.random()

    return (x^a * b) + i
end