/*
 * SPDX-License-Identifier: BSD-3-Clause
 * Copyright (c) 1996-2025, The SLICOT Team (original Fortran77 code)
 * Copyright (c) 2025, slicot.c contributors (C11 translation)
 */

#include "slicot.h"
#include "slicot_blas.h"
#include <math.h>

void mb03bf(i32 k, const i32 *amap, const i32 *s, i32 sinv, f64 *a,
            i32 lda1, i32 lda2, f64 ulp) {
    const f64 ZERO = 0.0;

    i32 ldas = lda1 * lda2;

    for (i32 iter = 0; iter < 20; iter++) {
        f64 cs, sn, ct, st;
        mb03af("Single", k, 2, amap, s, sinv, a, lda1, lda2, &cs, &sn, &ct, &st);

        i32 ai = amap[k - 1] - 1;
        f64 *a_slice = a + ai * ldas;
        f64 a11 = a_slice[0];
        f64 a21 = a_slice[1];
        f64 a12 = a_slice[lda1];
        f64 a22 = a_slice[1 + lda1];
        a_slice[0] = cs * a11 + sn * a12;
        a_slice[1] = cs * a21 + sn * a22;
        a_slice[lda1] = cs * a12 - sn * a11;
        a_slice[1 + lda1] = cs * a22 - sn * a21;

        for (i32 l = 0; l < k - 1; l++) {
            ai = amap[l] - 1;
            a_slice = a + ai * ldas;
            a11 = a_slice[0];
            a21 = a_slice[1];
            a12 = a_slice[lda1];
            a22 = a_slice[1 + lda1];

            if (s[ai] == sinv) {
                a_slice[0] = cs * a11 + sn * a21;
                a_slice[1] = cs * a21 - sn * a11;
                a_slice[lda1] = cs * a12 + sn * a22;
                a_slice[1 + lda1] = cs * a22 - sn * a12;

                f64 temp = a_slice[1 + lda1];
                f64 arg = -a_slice[1];
                SLC_DLARTG(&temp, &arg, &cs, &sn, &a_slice[1 + lda1]);
                a_slice[1] = ZERO;
                temp = cs * a_slice[0] + sn * a_slice[lda1];
                a_slice[lda1] = cs * a_slice[lda1] - sn * a_slice[0];
                a_slice[0] = temp;
            } else {
                a_slice[0] = cs * a11 + sn * a12;
                a_slice[lda1] = cs * a12 - sn * a11;
                a_slice[1] = cs * a21 + sn * a22;
                a_slice[1 + lda1] = cs * a22 - sn * a21;

                f64 temp = a_slice[0];
                SLC_DLARTG(&temp, &a_slice[1], &cs, &sn, &a_slice[0]);
                a_slice[1] = ZERO;
                temp = cs * a_slice[lda1] + sn * a_slice[1 + lda1];
                a_slice[1 + lda1] = cs * a_slice[1 + lda1] - sn * a_slice[lda1];
                a_slice[lda1] = temp;
            }
        }

        ai = amap[k - 1] - 1;
        a_slice = a + ai * ldas;
        a11 = a_slice[0];
        a21 = a_slice[1];
        a12 = a_slice[lda1];
        a22 = a_slice[1 + lda1];
        a_slice[0] = cs * a11 + sn * a21;
        a_slice[1] = cs * a21 - sn * a11;
        a_slice[lda1] = cs * a12 + sn * a22;
        a_slice[1 + lda1] = cs * a22 - sn * a12;

        if (fabs(a_slice[1]) < ulp * fmax(fmax(fabs(a_slice[0]), fabs(a_slice[lda1])),
                                          fabs(a_slice[1 + lda1]))) {
            break;
        }
    }
}
