/*
 * rt_invd4x4_snf.cpp
 *
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 *
 * Code generation for model "VFControl".
 *
 * Model version              : 1.1
 * Simulink Coder version : 24.1 (R2024a) 19-Nov-2023
 * C++ source code generated on : Wed Oct 16 10:45:53 2024
 * Created for block: VFControl
 */

#include "rtwtypes.h"
#include "rt_invd4x4_snf.h"
#include <cmath>

void rt_invd4x4_snf(const real_T u[16], real_T y[16])
{
  real_T A[16];
  real_T x;
  int32_T Aoffset;
  int32_T ipk;
  int32_T ix;
  int32_T jm1;
  int32_T jpiv_offset;
  int32_T mmj;
  int32_T pipk;
  int8_T ipiv[4];
  int8_T p[4];
  for (Aoffset = 0; Aoffset < 16; Aoffset++) {
    A[Aoffset] = u[Aoffset];
    y[Aoffset] = 0.0;
  }

  ipiv[0] = 1;
  ipiv[1] = 2;
  ipiv[2] = 3;
  for (ipk = 0; ipk < 3; ipk++) {
    real_T s;
    real_T smax;
    int32_T ONE;
    int32_T jj;
    int32_T jp1j;
    pipk = ipk + 1;
    jm1 = pipk - 1;
    mmj = 4 - pipk;
    Aoffset = jm1 * 5;
    ONE = 1;
    jj = Aoffset + 1;
    jp1j = jj + 1;
    Aoffset = mmj + 1;
    ix = jj - 1;
    x = A[jj - 1];
    s = std::abs(x);
    smax = s;
    for (jpiv_offset = 2; jpiv_offset <= Aoffset; jpiv_offset++) {
      ix++;
      x = A[ix];
      s = std::abs(x);
      if (s > smax) {
        ONE = jpiv_offset;
        smax = s;
      }
    }

    jpiv_offset = ONE - 1;
    ONE = (jj + jpiv_offset) - 1;
    if (A[ONE] != 0.0) {
      if (jpiv_offset != 0) {
        Aoffset = pipk + jpiv_offset;
        ipiv[pipk - 1] = static_cast<int8_T>(Aoffset);
        ONE = jm1 + jpiv_offset;
        s = A[jm1];
        A[jm1] = A[ONE];
        A[ONE] = s;
        jm1 += 4;
        ONE += 4;
        s = A[jm1];
        A[jm1] = A[ONE];
        A[ONE] = s;
        jm1 += 4;
        ONE += 4;
        s = A[jm1];
        A[jm1] = A[ONE];
        A[ONE] = s;
        jm1 += 4;
        ONE += 4;
        s = A[jm1];
        A[jm1] = A[ONE];
        A[ONE] = s;
      }

      Aoffset = mmj - 1;
      jm1 = jp1j + Aoffset;
      for (int32_T b = jp1j; b <= jm1; b++) {
        x = A[b - 1];
        s = A[jj - 1];
        smax = x / s;
        A[b - 1] = smax;
      }
    }

    Aoffset = 3 - pipk;
    jpiv_offset = jj + 3;
    pipk = jj + 5;
    ONE = pipk - 1;
    for (pipk = 0; pipk <= Aoffset; pipk++) {
      s = A[jpiv_offset];
      if (s != 0.0) {
        s = -s;
        ix = jp1j - 1;
        jm1 = ONE + 1;
        jj = mmj + ONE;
        for (int32_T b = jm1; b <= jj; b++) {
          A[b - 1] += A[ix] * s;
          ix++;
        }
      }

      jpiv_offset += 4;
      ONE += 4;
    }
  }

  p[0] = 1;
  p[1] = 2;
  p[2] = 3;
  p[3] = 4;
  ipk = ipiv[0] - 1;
  if (ipk + 1 > 1) {
    pipk = p[ipk];
    p[ipk] = 1;
    p[0] = static_cast<int8_T>(pipk);
  }

  ipk = ipiv[1] - 1;
  if (ipk + 1 > 2) {
    pipk = p[ipk];
    p[ipk] = p[1];
    p[1] = static_cast<int8_T>(pipk);
  }

  ipk = ipiv[2] - 1;
  if (ipk + 1 > 3) {
    pipk = p[ipk];
    p[ipk] = p[2];
    p[2] = static_cast<int8_T>(pipk);
  }

  for (jpiv_offset = 0; jpiv_offset < 4; jpiv_offset++) {
    pipk = jpiv_offset;
    Aoffset = p[pipk] - 1;
    mmj = Aoffset << 2;
    y[pipk + mmj] = 1.0;
    for (ipk = pipk + 1; ipk < 5; ipk++) {
      Aoffset = (mmj + ipk) - 1;
      if (y[Aoffset] != 0.0) {
        jm1 = ipk + 1;
        for (int32_T b = jm1; b < 5; b++) {
          ix = (mmj + b) - 1;
          y[ix] -= A[(((ipk - 1) << 2) + b) - 1] * y[Aoffset];
        }
      }
    }
  }

  for (ipk = 0; ipk < 4; ipk++) {
    pipk = ipk;
    pipk <<= 2;
    for (jpiv_offset = 3; jpiv_offset >= 0; jpiv_offset--) {
      ix = jpiv_offset << 2;
      Aoffset = jpiv_offset + pipk;
      x = y[Aoffset];
      if (x != 0.0) {
        y[Aoffset] = x / A[jpiv_offset + ix];
        jm1 = jpiv_offset - 1;
        for (int32_T b = 0; b <= jm1; b++) {
          mmj = b + pipk;
          y[mmj] -= A[b + ix] * y[Aoffset];
        }
      }
    }
  }
}
