#include "../include/paralleltt.h"
#include <math.h>

#include <stdlib.h>
#include <stdio.h>
#include <mpi.h>
#include <cblas.h>
#include <string.h>
#include <lapacke.h>

#define min(a,b) ((a)>(b)?(b):(a))
#define abs(a) ((a)>(0)?(a):(-a))

#ifndef HEAD
#define HEAD (int) 0
#endif

matrix_tt* matrix_tt_init(const int m, const int n)
{
    matrix_tt* A = (matrix_tt*) malloc(sizeof(matrix_tt));
    A->m = m;
    A->n = n;
    A->transpose = 0;
    A->offset = 0;
    A->lda = m;
    A->X_size = (long) m*n;
    A->X = (double*) malloc(A->X_size*sizeof(double));

    return A;
}

void matrix_tt_wrap_update(matrix_tt* A, int m, int n, double* X)
{
    A->m = m;
    A->n = n;
    A->transpose = 0;
    A->offset = 0;
    A->lda = m;
    A->X_size = (long) m*n;
    A->X = X;
}

matrix_tt* matrix_tt_wrap(int m, int n, double* X)
{
    matrix_tt* A = (matrix_tt*) malloc(sizeof(matrix_tt));
    A->m = m;
    A->n = n;
    A->transpose = 0;
    A->offset = 0;
    A->lda = m;
    A->X_size = (long) m*n;
    A->X = X;
    return A;
}

matrix_tt* matrix_tt_copy(const matrix_tt* A)
{
    matrix_tt* B = (matrix_tt*) malloc(sizeof(matrix_tt));
    int m = A->m; int n = A->n; int X_size = A->X_size;
    B->m = m;
    B->n = n;
    B->transpose = A->transpose;
    B->offset = A->offset;
    B->lda = A->lda;
    B->X_size = X_size;
    B->X = (double*) malloc(X_size * sizeof(double));
    memcpy(B->X, A->X, X_size * sizeof(double));

    return B;
}

// Copy from matrix A to matrix B
void matrix_tt_copy_data(matrix_tt* B, const matrix_tt* A)
{
    int BT = (B->transpose == 0) ? 0 : 1;
    int AT = (A->transpose == 0) ? 0 : 1;
    if (BT == AT){
        if ((B->m != A->m) || (B->n != A->n)){
            printf("matrix_tt_copy_data: B (%d x %d) is not the same size as A (%d x %d)\n", B->n, B->m, A->n, A->m);
            return;
        }
        for (int jj = 0; jj < B->n; ++jj){
            memcpy((B->X) + (B->offset) + (B->lda)*jj, (A->X) + (A->offset) + jj*(A->lda), (B->m)*sizeof(double));
        }
    }
    else{
        if ((B->m != A->n) || (B->n != A->m)){
            printf("matrix_tt_copy_data: B (%d x %d, transpose = %d) is not the same size as A (%d x %d, transpose = %d)\n",
                    B->n, B->m, B->transpose, A->n, A->m, A->transpose);
            return;
        }
        for (int jj = 0; jj < B->n; ++jj){
            double* BX = (B->X) + (B->offset) + (B->lda)*jj;
            double* AX = (A->X) + (A->offset) + jj;
            for (int ii = 0; ii < B->m; ++ii){
                BX[ii] = AX[ii*(A->lda)];
            }
        }
    }
}

matrix_tt* submatrix_copy(const matrix_tt* A)
{
    matrix_tt* B = (matrix_tt*) malloc(sizeof(matrix_tt));
    int m = A->m; int n = A->n; int X_size = m*n;
    B->m = m;
    B->n = n;
    B->transpose = A->transpose;
    B->offset = 0;
    B->lda = m;
    B->X_size = X_size;
    B->X = (double*) malloc(X_size * sizeof(double));
    matrix_tt_copy_data(B, A);

    return(B);
}

// NOTE: reshape and submatrix will not work well together.
void matrix_tt_reshape(int m, int n, matrix_tt* A)
{
    long new_size = (long) m*n;
    if (new_size > A->X_size){
        printf("Reshape failed: m*n=%ld is too large for X_size=%ld\n", (long) new_size, A->X_size);
    }
    else{
        A->m = m;
        A->n = n;
        A->offset = 0;
        A->lda = m;
    }
}

// In matlab notation, returns A[ii1:ii2-1, jj1:jj2-1]
matrix_tt* submatrix(const matrix_tt* A, int ii1, int ii2, int jj1, int jj2)
{
    int mA = A->m; int nA = A->n; int lda = A->lda;
    if ((ii1 < 0) || (ii2 < ii1) || (mA < ii2) || (jj1 < 0) || (jj2 < jj1) || (nA < jj2)){
        printf("Cannot take the A[%d:%d,%d:%d] of a %d by %d matrix (Matlab notation)\n", ii1, ii2-1, jj1, jj2-1, mA, nA);
    }
    matrix_tt* B = (matrix_tt*) malloc(sizeof(matrix_tt));

    B->m = ii2 - ii1;
    B->n = jj2 - jj1;
    B->transpose = A->transpose;
    B->offset = ii1 + jj1*lda;
    B->lda = lda;
    B->X_size = A->X_size;
    B->X = A->X + A->offset;

    return B;
}

// Updates the indices of the submatrix A
void submatrix_update(matrix_tt* A, int ii1, int ii2, int jj1, int jj2)
{
    A->m = ii2 - ii1;
    A->n = jj2 - jj1;
    A->offset = ii1 + jj1*(A->lda);
}

// Get an element of the matrix
double matrix_tt_element(const matrix_tt* A, int ii, int jj)
{
    return A->X[A->offset + ii + (A->lda)*jj];
}

void matrix_tt_free(matrix_tt* A)
{
    free(A->X); A->X = NULL;
    free(A);
    return;
}

void matrix_tt_print(const matrix_tt* A, int just_the_matrix)
{
    int m = A->m; int n = A->n; int T = A->transpose;
    int offset = A->offset; int lda = A->lda;

    if (!just_the_matrix){
        printf("Size of matrix: m=%d, n=%d\n", m, n);
        printf("Transpose=%d, offset=%d, lda=%d\n", T, offset, lda);
        printf("X_size=%ld\n", A->X_size);
        printf("X");
        if (T){ printf("^T"); }
        printf(" is \n");
    }

    int mloop = T ? m : n;
    int nloop = T ? n : m;
    printf("[");
    for (int jj = 0; jj < nloop; ++jj){
        for (int ii = 0; ii < mloop; ++ii){
            if (!T){ printf("%f",A->X[lda*ii + jj + offset]); }
            else{    printf("%f",A->X[lda*jj + ii + offset]); }
            if (ii < mloop-1){ printf(", "); }
        }

        if (jj == nloop - 1){
            printf("]\n");
        }
        else{
            printf(",\n ");
        }
    }
}

double frobenius_norm(matrix_tt* A){
    double norm = 0;
    if (A->m == A->lda){
        norm = cblas_dnrm2(A->m * A->n, A->offset + A->X, 1);
    }
    else{
        for (int ii = 0; ii < A->n; ++ii){
            double tmp = cblas_dnrm2(A->m, A->offset + (A->lda)*ii + A->X, 1);
            norm = sqrt(norm*norm + tmp*tmp);
        }
    }
    return norm;
}

// Performs the operation C = alpha*A*B + beta*C, where A and B are appropriately transposed
int matrix_tt_dgemm(const matrix_tt* A, const matrix_tt* B, matrix_tt* C, const double alpha, const double beta)
{
    int mA = A->m; int nA = A->n;
    int mB = B->m; int nB = B->n;
    int m_check = C->m; int n_check = C->n; int k_check;

    int m; int n; int k;
    CBLAS_TRANSPOSE TransA;
    CBLAS_TRANSPOSE TransB;
    if (A->transpose == 0){
        TransA = CblasNoTrans;
        m = mA;
        k = nA;
    }
    else{
        TransA = CblasTrans;
        m = nA;
        k = mA;
    }

    if (B->transpose == 0){
        TransB = CblasNoTrans;
        k_check = mB;
        n = nB;
    }
    else{
        TransB = CblasTrans;
        k_check = nB;
        n = mB;
    }

    if ((m != m_check) || (n != n_check) || (k != k_check)){
        printf("Dimensions of input arrays do not match:\n");
        printf("A: m=%d, n=%d, tranpose=%d\n",mA,nA,A->transpose);
        printf("B: m=%d, n=%d, tranpose=%d\n",mB,nB,B->transpose);
        printf("C: m=%d, n=%d, tranpose=%d\n",m_check,n_check,C->transpose);
        return 1;
    }

    C->transpose = 0;

    cblas_dgemm(CblasColMajor, TransA, TransB,
                m, n, k,
                alpha,
                A->X + A->offset, A->lda,
                B->X + B->offset, B->lda,
                beta,
                C->X + C->offset, C->lda);

    return 0;
}

// Solves the least squares problem Ax = b
// At the end, it just copies the result into x. This probably isn't the most efficient way to do this
void matrix_tt_dgels(matrix_tt* x, matrix_tt* A, const matrix_tt* b)
{
    if (x->transpose != 0){
        printf("matrix_tt_least_squares: x cannot be transposed for dgels\n");
        return;
    }

    if (b->transpose != 0){
        printf("matrix_tt_least_squares: b cannot be transposed for dgels\n");
        return;
    }

    int nrhs = x->n;
    char TransA;
    int m_check;
    int n_check;

    if (A->transpose == 0){
        TransA = 'N';
        m_check = A->m;
        n_check = A->n;
    }
    else{
        TransA = 'T';
        m_check = A->n;
        n_check = A->m;
    }

    if (n_check != x->m){
        printf("right index of A (%d) does not match left index of x (%d)\n", n_check, x->m);
    }
    if (m_check != b->m){
        printf("left index of A (%d) does not match left index of b (%d)\n", m_check, b->m);
    }
    if (x->n != b->n){
        printf("right index of x (%d) does not match right index of b (%d)\n", x->n, b->n);
    }

    int m = A->m;
    int n = A->n;

    int info = LAPACKE_dgels(LAPACK_COL_MAJOR, TransA, m, n, nrhs, A->X + A->offset, A->lda, b->X + b->offset, b->lda);

    matrix_tt* result = submatrix(b, 0, x->m, 0, x->n);
    matrix_tt_copy_data(x, result);
    free(result);
}

// Input:
//   Q - the matrix you want to take the QR
// Output:
//   Q - the Q matrix of QR
//   R - (input is NULL) ? nothing : the R matrix of QR

// NOTE: Currently assumes R and Q are not transposed
int matrix_tt_truncated_qr(matrix_tt* Q, matrix_tt* R, int r)
{
    int Qm = Q->m; int Qn = Q->n; double* QX = Q->X;
    int Qtranspose = Q->transpose; long Qoffset = Q->offset; int Qlda = Q->lda;

    lapack_int info;

    if (Qtranspose == 0){
        if (r > Qn)
        {
            printf("ERROR: (matrix_tt_truncated_qr) r = %d is too large for Q->n = %d \n", r, Qn);
            return 1;
        }
        // The QR step
        double* tau = (double*) malloc(sizeof(double)*min(Qm,Qn));
        info = LAPACKE_dgeqrf(LAPACK_COL_MAJOR, Qm, Qn, QX + Qoffset, Qlda, tau);

        if (R != NULL){
            int Rm = R->m; int Rn = R->n; double* RX = R->X;
            int Rtranspose = R->transpose; long Roffset = R->offset; int Rlda = R->lda;

            if ((Rm != r) || (Rn != r)){
                printf("ERROR: (matrix_tt_truncated_qr) R->m = %d or R->n = %d is not equal to r = %d\n", Rm, Rn, r);
                return 1;
            }
            if (Rtranspose != 0){
                printf("ERROR: (matrix_tt_truncated_qr) Only defined for R->transpose = 0\n");
                return 1;
            }

            for (int jj = 0; jj < r; ++jj){
                int r_col_length = (Qm < jj+1) ? (Qm) : jj+1;
                memcpy(RX + Roffset + jj*Rlda, QX + Qoffset + jj*Qlda, r_col_length*sizeof(double));
                memset(RX + Roffset + jj*Rlda + r_col_length, 0, (r-r_col_length)*sizeof(double));
            }
        }

        if (r > Qm){
            LAPACKE_dorgqr(LAPACK_COL_MAJOR, Qm, Qm, Qm, QX + Qoffset, Qlda, tau);
            matrix_tt* Q_sub = submatrix(Q, 0, Qm, Qm, r);
            matrix_tt_fill_zeros(Q_sub);
            free(Q_sub);
        }
        else{
            LAPACKE_dorgqr(LAPACK_COL_MAJOR, Qm, r, r, QX + Qoffset, Qlda, tau);
        }

        Q->n = r;

        free(tau);       tau = NULL;
    }
    else{
        if (r > Qm)
        {
            printf("ERROR: (matrix_tt_truncated_qr) r = %d is too large for Q->n = %d \n", r, Qn);
            return 1;
        }
        // The LQ step
        printf("Performing LQ\n");
        double* tau = (double*) malloc(sizeof(double)*min(Qm,Qn));
        info = LAPACKE_dgelqf(LAPACK_COL_MAJOR, Qm, Qn, QX + Qoffset, Qlda, tau);
        printf("\nRight after LQ, Q = \n");
        matrix_tt_print(Q, 1);

        if (R != NULL){
            int Rm = R->m; int Rn = R->n; double* RX = R->X;
            int Rtranspose = R->transpose; long Roffset = R->offset; int Rlda = R->lda;

            if ((Rm != r) || (Rn != r)){
                printf("ERROR: (matrix_tt_truncated_qr) R->m = %d or R->n = %d is not equal to r = %d\n", Rm, Rn, r);
                return 1;
            }
            if (Rtranspose == 0){
                printf("ERROR: (matrix_tt_truncated_qr) Only defined for R->transpose = Q->transpose\n");
                return 1;
            }

            int ncols = (r > Qm) ? Qm : r;
            for (int jj = 0; jj < r; ++jj){
                if (jj < Qm){
                    memcpy(RX + Roffset + jj*Rlda + jj, QX + Qoffset + jj*Qlda + jj, (Qm - jj)*sizeof(double));
                    memset(RX + Roffset + jj*Rlda, 0, jj*sizeof(double));
                }
            }

        }

        if (r > Qn){
            printf("Doing first dorglq\n");
            LAPACKE_dorglq(LAPACK_COL_MAJOR, Qn, Qn, Qn, QX + Qoffset, Qlda, tau);
            matrix_tt* Q_sub = submatrix(Q, Qn, r, 0, Qn);
            matrix_tt_fill_zeros(Q_sub);
            free(Q_sub);
        }
        else{
            printf("Doing second dorglq\n");
            LAPACKE_dorglq(LAPACK_COL_MAJOR, Qm, Qn, Qm, QX + Qoffset, Qlda, tau);
        }

        Q->m = r;

        free(tau);       tau = NULL;
    }

    return 0;
}/**/

void matrix_tt_group_reduce(MPI_Comm comm, int rank, matrix_tt* A, matrix_tt* buf, int head, int* group_ranks, int nranks)
{
    int in_the_group = 0;
    for (int ii = 0; ii < nranks; ++ii){
        if (rank == group_ranks[ii]){
            in_the_group = 1;
        }
    }

    if (in_the_group){
        int buf_assigned = 0;
        if (!buf){
            buf_assigned = 1;
            buf = matrix_tt_init(A->m, A->n);
        }

        if ((buf->m < A->m) || (buf->n < A->n)){
            printf("buf (%d x %d) must be larger than A (%d x %d)\n", buf->m, buf->n, A->m, A->n);
        }

        int send = -1;
        int recv = -1;
        while (nranks > 1){
            int half_nranks = (nranks + 1) / 2;
            for (int ii = 0; ii < half_nranks; ++ii){
                if (ii + half_nranks >= nranks){
                    recv = group_ranks[ii];
                    send = -1;
                }
                else if (group_ranks[ii + half_nranks] == head){
                    recv = head;
                    send = group_ranks[ii];
                }
                else{
                    recv = group_ranks[ii];
                    send = group_ranks[ii + half_nranks];
                }

                if (rank == send){
                    matrix_tt_send(comm, A, recv);
                }
                else if ((rank == recv) && (send != -1)){
                    matrix_tt_recv(comm, buf, send);
                    for (int jj = 0; jj < A->n; ++jj){
//                        MPI_Recv(buf->X + buf->offset, A->m, MPI_DOUBLE, send, 0, comm, MPI_STATUS_IGNORE);
                        long A_offset = A->offset + jj * (A->lda);
                        long buf_offset = buf->offset +  jj * (buf->lda);
                        for (int kk = 0; kk < A->m; ++kk){
                            A->X[A_offset + kk] = A->X[A_offset + kk] + buf->X[buf_offset + kk];
                        }
                    }
                }
                group_ranks[ii] = recv;
            }
            nranks = half_nranks;
        }

        if (buf_assigned){
            matrix_tt_free(buf);
        }
    }

}

// buf just has to be size A->lda x 1 or more
void matrix_tt_reduce(MPI_Comm comm, int rank, matrix_tt* A, matrix_tt* buf, int head)
{
    if (buf == NULL){
        matrix_tt* buf = matrix_tt_init(A->m, 1);
        matrix_tt_reduce(comm, rank, A, buf, head);
        matrix_tt_free(buf);
    }
    else{
        int size;
        MPI_Comm_size(comm, &size);

        if ((head < 0) || (head >= size)){
            printf("matrix_tt_reduce: head = %d is not a valid rank for size = %d\n", head, size);
        }

        if (buf->lda < A->m){
            printf("buf->lda = %d must be larger than A->m = %d\n", buf->lda, A->m);
        }

        int* activated_ranks = (int*) calloc(size, sizeof(int));
        for (int ii = 0; ii < size; ++ii){
            activated_ranks[ii] = ii;
        }
        int send = -1;
        int recv = -1;
        while (size > 1){
            int half_size = (size + 1) / 2;
            for (int ii = 0; ii < half_size; ++ii){
                if (ii + half_size >= size){
                    recv = activated_ranks[ii];
                    send = -1;
                }
                else if (activated_ranks[ii + half_size] == head){
                    recv = head;
                    send = activated_ranks[ii];
                }
                else{
                    recv = activated_ranks[ii];
                    send = activated_ranks[ii + half_size];
                }

                if (rank == send){
                    for (int jj = 0; jj < A->n; ++jj){
                        MPI_Send(A->X + A->offset + A->lda * jj, A->m, MPI_DOUBLE, recv, 0, comm);
                    }
                }
                else if ((rank == recv) && (send != -1)){
                    for (int jj = 0; jj < A->n; ++jj){
                        MPI_Recv(buf->X + buf->offset, A->m, MPI_DOUBLE, send, 0, comm, MPI_STATUS_IGNORE);
                        long A_offset = A->offset + jj * (A->lda);
                        for (int kk = 0; kk < A->m; ++kk){
                            A->X[A_offset + kk] = A->X[A_offset + kk] + buf->X[buf->offset + kk];
                        }
                    }
                }
                activated_ranks[ii] = recv;
            }
            size = half_size;
        }
        free(activated_ranks);
    }
}

void matrix_tt_allreduce(MPI_Comm comm, matrix_tt* A)
{
    for (int jj = 0; jj < A->n; ++jj){
        MPI_Allreduce(MPI_IN_PLACE, A->X + A->offset + jj*(A->lda), A->m, MPI_DOUBLE, MPI_SUM, comm);
    }
}

void matrix_tt_broadcast(MPI_Comm comm, matrix_tt* buf)
{
    int count = buf->m * buf->n;
    MPI_Bcast(buf->X, count, MPI_DOUBLE, HEAD, comm);
}

void matrix_tt_send(MPI_Comm comm, matrix_tt* buf, int dest){
    MPI_Datatype buf_type;
    MPI_Type_vector(buf->n, buf->m, buf->lda, MPI_DOUBLE, &buf_type);
    MPI_Type_commit(&buf_type);
    MPI_Send(buf->X + buf->offset, 1, buf_type, dest, 0, comm);
    MPI_Type_free(&buf_type);
}

void matrix_tt_recv(MPI_Comm comm, matrix_tt* buf, int source){
    MPI_Datatype buf_type;
    MPI_Type_vector(buf->n, buf->m, buf->lda, MPI_DOUBLE, &buf_type);
    MPI_Type_commit(&buf_type);
    MPI_Recv(buf->X + buf->offset, 1, buf_type, source, 0, comm, MPI_STATUS_IGNORE);
    MPI_Type_free(&buf_type);
}

// Column khatri-rao product C = A x B
void khatri_rao(const matrix_tt* restrict A, const matrix_tt* restrict B, matrix_tt* restrict C){
    int n = A->n;
    int m_A = A->m;

    int n_B = B->n;
    int m_B = B->m;

    int n_C = C->n;
    int m_C = C->m;

    if ((n != n_B) || (n != n_C) || (m_A*m_B != m_C)){
        printf("khatri_rao: the sizes dont work!\n");
        printf("A is %d x %d\n", m_A, n);
        printf("B is %d x %d\n", m_B, n_B);
        printf("C is %d x %d\n", m_C, n_C);
    }

    for (int kk = 0; kk < n; ++kk){
        for (int ii = 0; ii < m_B; ++ii){
            double Bii = B->X[kk*m_B + ii];
            for (int jj = 0; jj < m_A; ++jj){
                C->X[kk*m_C + ii*m_B + jj] = Bii * A->X[kk*m_A + jj];
            }
        }
    }
}

// Recursively KR multiply the list of matrices A
void list_khatri_rao(int d_kr, matrix_tt** A, matrix_tt* Omega){
    if (d_kr == 1){
        for (int ii = 0; ii < A[0]->X_size; ++ii){
            Omega->X[ii] = A[0]->X[ii];
        }
        return;
    }

    if (d_kr == 2){
        khatri_rao(A[0], A[1], Omega);
    }
    else{
        int m_B = 1;
        int n_B = Omega->n;
        for (int ii = 0; ii < d_kr-1; ++ii){
            m_B = m_B * A[ii]->m;
        }

        matrix_tt* B = matrix_tt_init(m_B, n_B);
        list_khatri_rao(d_kr - 1, A, B);
        khatri_rao(B, A[d_kr - 1], Omega);
        matrix_tt_free(B); B = NULL;
    }

}

void matrix_tt_dlarnv(matrix_tt* A){
    int r1 = rand()%4096, r2 = rand()%4096, r3 = rand()%4096, r4 = rand()%4096;
    int iseed[4] = {r1, r2, r3, r4+(r4%2 == 0?1:0)};
    LAPACKE_dlarnv(3, iseed, (A->n) * (A->m), A->X);
}

void matrix_tt_fill_zeros(matrix_tt* A)
{
    for (int jj = 0; jj < A->n; ++jj){
        memset(A->X + A->offset + jj*A->lda, 0, A->m * sizeof(double));
    }
}

// Takes a matrix in row major and transforms it to column major
void row_to_col_major(const matrix_tt* mat_row, matrix_tt* mat_col)
{
    int m = mat_col->m;
    int n = mat_col->n;
    if ((m != mat_row->n) || (n != mat_row->m)){
        printf("The dimensions don't work out! mat_row should have the dimensions of mat_col transposed\n");
        printf("mat_row->m = %d, mat_row->n = %d\n", mat_row->m, mat_row->n);
        printf("mat_col->m = %d, mat_col->n = %d\n", mat_col->m, mat_col->n);
    }

    for (int ii = 0; ii < m; ++ii){
        for (int jj = 0; jj < n; ++jj){
            mat_col->X[jj * (mat_col->lda) + ii + mat_col->offset] = mat_row->X[ii * (mat_row->lda) + jj + mat_row->offset];
        }
    }
}


/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2013, Georgia Tech Research Corporation
 * Copyright (c) 2015, Rice University
 * Copyright (c) 2018-2019, Colorado School of Mines
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ntd@gatech.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   * Neither the name of the Rice University nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <nlopt.h>
#include "amino.h"
#include "amino/diffeq.h"

/* static double */
/* s_nlobj_jpinv(unsigned n, const double *q, double *dq, void *vcx) */
/* { */
/*     struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx; */
/*     void *ptrtop = aa_mem_region_ptr(cx->reg); */

/*     struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1); */
/*     aa_rx_fk_sub(cx->fk, cx->ssg, &vq); */
/*     double *E_act = aa_rx_fk_ref(cx->fk, cx->frame); */

/*     if( dq ) { */
/*         struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1); */
/*         s_ksol_jpinv(cx,q, &v_dq); */
/*         aa_dvec_scal(-1,&v_dq); */
/*     } */

/*     double x = s_serr( E_act, cx->TF_ref->data ); */

/*     aa_mem_region_pop(cx->reg, ptrtop); */
/*     return x; */

/* } */

static double s_nlobj_dq_fd_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);

    double S_act[8], S_ref[8], S_err[8], S_ln[8];
    aa_tf_qutr2duqu(E_act, S_act);
    aa_tf_qutr2duqu(cx->TF_ref->data, S_ref);
    aa_tf_duqu_cmul(S_act,S_ref,S_err);
    aa_tf_duqu_minimize(S_err);
    aa_tf_duqu_ln(S_err, S_ln);
    double result = aa_la_dot(8,S_ln,S_ln);

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

AA_API double
aa_rx_ik_opt_err_dqln_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_dq_fd_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_dq_fd_helper, vcx,
                       &vq, eps, &vdq );
    }

    return x;
}

static void
mv_block_helper (const double *A, const double *x, double *y)
{
    cblas_dgemv(CblasColMajor, CblasTrans, 8, 4,
                1, A, 8, x, 1,
                0, y, 1);
    cblas_dgemv(CblasColMajor, CblasTrans, 4, 4,
                1, A, 8, x+4, 1,
                0, y+4, 1);
}

static void
duqu_rmul_helper( const double *Sr, const double *x, double *y )
{
    double M[8*8];
    aa_tf_duqu_matrix_r(Sr,M,8);
    mv_block_helper(M, x, y);
}

AA_API double
aa_rx_ik_opt_err_dqln( void *vcx, double *q, double *dq ) {
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);

    double S_act[8], S_ref[8], S_err[8], S_ln[8];
    aa_tf_qutr2duqu(E_act, S_act);
    aa_tf_qutr2duqu(cx->TF_ref->data, S_ref);
    aa_tf_duqu_cmul(S_act,S_ref,S_err);
    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion
    int needs_min = (S_err[AA_TF_DUQU_REAL_W] < 0 );
    if( needs_min ) {
        for( size_t i = 0; i < 8; i ++ ) S_err[i] *= -1;
    } else {
    }
    aa_tf_duqu_ln(S_err, S_ln);
    double result = aa_la_dot(8,S_ln,S_ln);

    if( dq ) {
        if( needs_min ) {
            for( size_t i = 0; i < 8; i ++ ) S_ln[i] *= -1;
        }
        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        /*
         * g_sumsq * J_ln*[S_ref]_r*J_conj*J_S
         * -> J_ln^T g_sumsq * [S_ref]_r*J_conj*J_S
         * -> [S_ref]_r^T (J_ln^T g_sumsq) *J_conj*J_S
         * -> J_conj^T*[S_ref]_r^T (J_ln^T g_sumsq) * J_S
         * -> J_S^T J_conj^T * [S_ref]_r^T * J_ln^T g_sumsq
         */

        double a[8], b[8], dJ[8*8];
        struct aa_dmat vJ_8x8 = AA_DMAT_INIT(8,8,dJ,8);
        struct aa_dmat *J_8x8 = &vJ_8x8;

        {
            struct aa_dmat *J_ln = J_8x8;
            aa_tf_duqu_ln_jac(S_err,J_ln);
            /*
             *  struct aa_dvec v_S_ln = AA_DVEC_INIT(8,S_ln,1);
             * //aa_dmat_gemv( CblasTrans, 2, J_8x8, &v_S_ln, 0, &va );
             *
             * Avoid multiplying by the zero block:
             *
             * J_ln = [J_r   0]     J_ln^T = [J_r^T J_d^T]
             *        [J_d J_r]              [  0   J_r^T]
             *
             * J_ln^T * del(S_ln^T*S_ln)^T = 2*J_ln^T S_ln
             *
             * 2*J_ln^T S_ln = 2*[J_r^T  J_d^T] (S_ln_r)
             *                   [   0   J_r^T] (S_ln_d)
             *
             */
            mv_block_helper(dJ, S_ln, a);
        }

        /*
         * dS/dphi = [S]_R V J
         * g [S]_R V J
         * -> [S]_R^T g V J
         * -> V^T [S]_R^T g J
         * -> J^T V [S]_R^T g
         */

        duqu_rmul_helper( S_ref, a, b );
        aa_tf_duqu_conj1(b);

        duqu_rmul_helper( S_act, b, a );  /* TODO: Pure result, some
                                           * extra multiplies here. */
        // a is now a pure dual quaternion
        {
            double c[6];
            struct aa_dvec vc = AA_DVEC_INIT(6,c,1);
            // Using Twist Jacobian
            aa_tf_duqu2pure(a,c);
            struct aa_dmat *Jtw = aa_rx_sg_sub_jac_twist_get(cx->ssg, cx->reg, cx->fk);
            aa_dmat_gemv( CblasTrans, 1, Jtw, &vc, 0, &v_dq );

            // Using Velocity Jacobian
            /* aa_tf_cross_a(a+AA_TF_DUQU_DUAL_XYZ,E_act+AA_TF_QUTR_V,a); */
            /* aa_tf_duqu2pure(a,c); */
            /* struct aa_dmat *J_vel = aa_rx_sg_sub_get_jacobian(cx->ssg,cx->reg,cx->TF); */
            /* aa_dmat_gemv( CblasTrans, 1, J_vel, &vc, 0, &v_dq ); */

        }

        /* { */
        /*     struct aa_dvec *v_dq_fd = aa_dvec_alloc(cx->reg,n); */
        /*     double eps = 1e-6; */
        /*     aa_de_grad_fd( s_nlobj_dq_fd_helper, vcx, */
        /*                    &vq, eps, v_dq_fd ); */

        /*     printf("--\n"); */
        /*     printf("needs min: %d\n", needs_min); */
        /*     printf("ad: "); */
        /*     aa_dump_vec(stdout,v_dq_fd->data,n); */
        /*     printf("an: "); */
        /*     aa_dump_vec(stdout,dq,n); */

        /*     assert( aa_dvec_ssd(v_dq_fd, &v_dq) < 1e-3 ); */
        /* } */
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

static double s_nlobj_qv_fd_trans_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = aa_tf_vdot(v_err,v_err);

    return result;
}

AA_API double
aa_rx_ik_opt_err_trans_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_qv_fd_trans_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_qv_fd_trans_helper, vcx,
                       &vq, eps, &vdq );
    }
    /* fprintf(stdout, "fd error: %2.2f\n", x);  */
    return x;
}

static double s_nlobj_qv_fd_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;
    double *q_act = E_act + AA_TF_QUTR_Q;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;
    double *q_ref = E_ref + AA_TF_QUTR_Q;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;
    double *q_err = E_err + AA_TF_QUTR_Q;

    aa_tf_qcmul(q_act,q_ref,q_err);

    double q_ln[4];
    aa_tf_qminimize(q_err);
    aa_tf_duqu_ln(q_err, q_ln);

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = ( aa_tf_qdot(q_ln,q_ln)
                      +
                      aa_tf_vdot(v_err,v_err) );

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

AA_API double
aa_rx_ik_opt_err_qlnpv_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_qv_fd_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_qv_fd_helper, vcx,
                       &vq, eps, &vdq );
    }

    return x;
}

AA_API double
aa_rx_ik_opt_err_trans( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );

    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;

    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = aa_tf_vdot(v_err,v_err);

    if( dq ) {

        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        struct aa_dmat *Jvel = aa_rx_sg_sub_jac_vel_get(cx->ssg, cx->reg, cx->fk);
        struct aa_dmat Jr, Jv;
        aa_dmat_view_block(&Jv, Jvel, AA_TF_DX_V, 0, 3, Jvel->cols);
        aa_dmat_view_block(&Jr, Jvel, AA_TF_DX_W, 0, 3, Jvel->cols);

        // Translational Part
        struct aa_dvec v_v_err = AA_DVEC_INIT(3,v_err,1);
        aa_dmat_gemv(CblasTrans, 2, &Jv, &v_v_err, 0, &v_dq);
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    /* printf("err: %f\n", result); */
    return result;
}

static void
q_rmul_helper( const double *q, const struct aa_dvec *x, struct aa_dvec *y )
{
    double dM[4*4];
    struct aa_dmat M = AA_DMAT_INIT(4,4,dM,4);
    aa_tf_qmat_r(q,&M);
    aa_dmat_gemv(CblasTrans, 1, &M, x, 0, y);
}

AA_API double
aa_rx_ik_opt_err_qlnpv( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );

    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;
    double *q_act = E_act + AA_TF_QUTR_Q;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;
    double *q_ref = E_ref + AA_TF_QUTR_Q;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;
    double *q_err = E_err + AA_TF_QUTR_Q;

    aa_tf_qcmul(q_act,q_ref,q_err);

    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion
    int needs_min = (E_err[AA_TF_QUAT_W] < 0 );
    if( needs_min ) {
        for( size_t i = 0; i < 4; i ++ ) E_err[i] *= -1;
    }

    double q_ln[4];
    aa_tf_qln(E_err, q_ln);

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = ( aa_tf_qdot(q_ln,q_ln) +
                      + aa_tf_vdot(v_err,v_err) );

    if( dq ) {

        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        struct aa_dmat *Jvel = aa_rx_sg_sub_jac_vel_get(cx->ssg, cx->reg, cx->fk);
        struct aa_dmat Jr, Jv;
        aa_dmat_view_block(&Jv, Jvel, AA_TF_DX_V, 0, 3, Jvel->cols);
        aa_dmat_view_block(&Jr, Jvel, AA_TF_DX_W, 0, 3, Jvel->cols);

        // Translational Part
        struct aa_dvec v_v_err = AA_DVEC_INIT(3,v_err,1);
        aa_dmat_gemv(CblasTrans, 2, &Jv, &v_v_err, 0, &v_dq);

        // Rotational Part
        if( needs_min ) {
            for( size_t i = 0; i < 4; i ++ ) q_ln[i] *= -1;
        }

        double a[4], b[4], dJ[4*4];
        struct aa_dvec va = AA_DVEC_INIT(4,a,1);
        struct aa_dvec vb = AA_DVEC_INIT(4,b,1);
        struct aa_dvec vln = AA_DVEC_INIT(4,q_ln,1);
        struct aa_dmat vJ_4x4 = AA_DMAT_INIT(4,4,dJ,4);
        struct aa_dmat *J_4x4 = &vJ_4x4;

        {
            struct aa_dmat *J_ln = J_4x4;
            aa_tf_qln_jac(q_err,J_ln);
            aa_dmat_gemv(CblasTrans, 1, J_ln, &vln, 0, &va);
        }

        q_rmul_helper( q_ref, &va, &vb );
        aa_tf_qconj1(b);

        q_rmul_helper( q_act, &vb, &va ); /* TODO: Pure result, some
                                           * extra multiplies here. */

        // a is now a pure dual quaternion */
        {
            struct aa_dvec va3 = AA_DVEC_INIT(3,a,1);
            aa_dmat_gemv( CblasTrans, 1, &Jr, &va3, 1, &v_dq );
        }

        /* { */
        /*     struct aa_dvec *v_dq_fd = aa_dvec_alloc(cx->reg,n); */
        /*     double eps = 1e-6; */
        /*     aa_de_grad_fd( s_nlobj_qv_fd_helper, vcx, */
        /*                    &vq, eps, v_dq_fd ); */

        /*     printf("--\n"); */
        /*     printf("needs min: %d\n", needs_min); */
        /*     printf("ad: "); */
        /*     aa_dump_vec(stdout,v_dq_fd->data,n); */
        /*     printf("an: "); */
        /*     aa_dump_vec(stdout,dq,n); */

        /*     assert( aa_dvec_ssd(v_dq_fd, &v_dq) < 1e-3 ); */
        /* } */
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    //printf("err: %f\n", result);
    return result;
}

// TODO: weighted error

AA_API double
aa_rx_ik_opt_err_jcenter( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    struct aa_mem_region *reg = cx->reg;
    void *ptrtop = aa_mem_region_ptr(reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec *q_center = aa_dvec_alloc(reg,n);
    aa_rx_sg_sub_center_configv(cx->ssg,q_center);

    double result = 0;

    if( dq ) { // error and gradient
        for( size_t i = 0; i < n; i ++ ) {
            double d = q[i] - AA_DVEC_REF(q_center,i);
            result += (d*d);
            dq[i] = d;
        }
        //printf("--\n");
        //printf("q:  "); aa_dump_vec(stdout,q,n);
        //printf("qc: "); aa_dump_vec(stdout,q_center->data,n);
        //printf("dq: "); aa_dump_vec(stdout,dq,n);
    } else { // error only
        for( size_t i = 0; i < n; i ++ ) {
            double d = q[i] - AA_DVEC_REF(q_center,i);
            result += (d*d);
        }
    }

    aa_mem_region_pop(cx->reg, ptrtop);

    return result / 2;
}

struct err_cx {
    void *cx;
    aa_rx_ik_opt_fun *fun;
};

static double
s_err_cx_dispatch(unsigned n, const double *q, double *dq, void *vcx)
{
    (void)n;
    struct err_cx *cx = (struct err_cx *)vcx;
    return cx->fun(cx->cx, q, dq);
}

static int
s_ik_nlopt( struct kin_solve_cx *cx,
            struct aa_dvec *q )
{
    struct aa_mem_region *reg = cx->reg;
    void *ptrtop = aa_mem_region_ptr(reg);

    const struct aa_rx_sg_sub *ssg = cx->ssg;
    const struct aa_rx_sg *sg = cx->ssg->scenegraph;

    if( 1 != cx->q_sub->inc || 1 != cx->q_all->inc ) {
        return AA_RX_INVALID_PARAMETER;
    }

    size_t n_sub = cx->q_sub->len;
    nlopt_algorithm alg = NLOPT_LD_SLSQP;
    nlopt_opt opt = nlopt_create(alg, (unsigned)n_sub); /* algorithm and dimensionality */

    struct err_cx ocx, eqctcx;
    ocx.cx = cx;
    ocx.fun = cx->ik_cx->opts->obj_fun;
    nlopt_set_min_objective(opt, s_err_cx_dispatch, &ocx);

    if( cx->ik_cx->opts->eqct_fun ) {
        eqctcx.cx = cx;
        eqctcx.fun = cx->ik_cx->opts->eqct_fun;
        nlopt_add_equality_constraint(opt, s_err_cx_dispatch, &eqctcx,
                                      cx->ik_cx->opts->eqct_tol);
    }

    //nlopt_set_xtol_rel(opt, 1e-4); // TODO: make a parameter
    if( cx->opts->tol_obj_abs >= 0 )
        nlopt_set_ftol_abs(opt, cx->opts->tol_obj_abs );
    if( cx->opts->tol_obj_rel >= 0 )
        nlopt_set_ftol_rel(opt, cx->opts->tol_obj_rel );
    if( cx->opts->tol_dq >= 0 )
        nlopt_set_xtol_rel(opt, cx->opts->tol_dq );

    double *lb = AA_MEM_REGION_NEW_N(reg,double,n_sub);
    double *ub = AA_MEM_REGION_NEW_N(reg,double,n_sub);
    { // bounds
        for( size_t i = 0; i < n_sub; i ++ ) {
            aa_rx_config_id id = aa_rx_sg_sub_config(ssg,i);
            if( aa_rx_sg_get_limit_pos(sg, id, lb+i, ub+i) )
            {
                lb[i] = -DBL_MAX;
                ub[i] = DBL_MAX;
            }
        }
    }

    nlopt_set_lower_bounds(opt, lb);
    nlopt_set_upper_bounds(opt, ub);
    double minf;
    nlopt_result ores = nlopt_optimize(opt, cx->q_sub->data, &minf);
    if( cx->opts->debug ) {
        /* fprintf("AMINO IK NLOPT RESULT: %s (%d)", */
        /*         nlopt_result_to_string(ores), ores); */

        fprintf(stderr, "AMINO IK NLOPT RESULT: %d\n", ores);
    }
    aa_dvec_copy( cx->q_sub, q );

    aa_rx_fk_sub(cx->fk, cx->ssg, q);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    int result = s_check(cx->ik_cx, cx->TF_ref, E_act );
    nlopt_destroy(opt);
    aa_mem_region_pop(reg,ptrtop);

    return result;

}


/*
 * Copyright 2020 Makani Technologies LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef SIM_MATH_ODE_SOLVER_GSL_H_
#define SIM_MATH_ODE_SOLVER_GSL_H_

#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <stdint.h>

#include <vector>

#include "common/macros.h"
#include "sim/math/ode_solver.h"
#include "sim/sim_types.h"

namespace sim {

// Wrapper for the GSL ODE library.
class GslOdeSolver : public OdeSolver {
 public:
  explicit GslOdeSolver(const OdeSystem &ode_system,
                        const SimOdeSolverParams &params);
  ~GslOdeSolver() {
    if (ode_driver_ != nullptr) gsl_odeiv2_driver_free(ode_driver_);
  }

  OdeSolverStatus Integrate(double t0, double tf, const std::vector<double> &x0,
                            double *t_int, std::vector<double> *x) override;

 private:
  // Static callback function for the GSL ODE solver.
  //
  // Args:
  //   t: Time at which derivative will be evaluated.
  //   x: Array of length num_states() containing the state at which
  //       the derivative will be evaluated.
  //   dx: Array of length num_states() into which the derivative is stored.
  //   context: Pointer to the OdeSolver class containing the OdeSystem.
  //
  // Returns:
  //   GSL_SUCCESS if the derivative was calculated successfully,
  //   GSL_FAILURE if the time step was too large, or
  //   GSL_EBADFUNC if the integration should be aborted immediately.
  static int32_t GslCallback(double t, const double x[], double dx[],
                             void *context);

  // Solver parameters.
  const SimOdeSolverParams &params_;

  // ODE to be solved.
  const OdeSystem &ode_system_;

  // Parameters for GSL.
  gsl_odeiv2_system sys_;
  gsl_odeiv2_driver *ode_driver_;

  DISALLOW_COPY_AND_ASSIGN(GslOdeSolver);
};

}  // namespace sim

#endif  // SIM_MATH_ODE_SOLVER_GSL_H_


/*
 * Copyright (c) 2011-2018 The University of Tennessee and The University
 *                         of Tennessee Research Foundation.  All rights
 *                         reserved.
 *
 * @precisions normal z -> s d c
 *
 */
#include "dplasma_cores.h"
#include "dplasma_zcores.h"

#if defined(PARSEC_HAVE_STRING_H)
#include <string.h>
#endif  /* defined(PARSEC_HAVE_STRING_H) */
#if defined(PARSEC_HAVE_STDARG_H)
#include <stdarg.h>
#endif  /* defined(PARSEC_HAVE_STDARG_H) */
#include <stdio.h>
#ifdef PARSEC_HAVE_LIMITS_H
#include <limits.h>
#endif
#include <stdlib.h>

#include <cblas.h>
#include <core_blas.h>

#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) < (b) ? (a) : (b))
#if defined(ADD_)
#define  DLARFG      zlarfg_
#else
#define  DLARFG      zlarfg
#endif
extern void DLARFG(int *N, parsec_complex64_t *ALPHA, parsec_complex64_t *X, int *INCX, parsec_complex64_t *TAU);

//static void band_to_trd_vmpi1(int N, int NB, parsec_complex64_t *A, int LDA);
//static void band_to_trd_vmpi2(int N, int NB, parsec_complex64_t *A, int LDA);
//static void band_to_trd_v8seq(int N, int NB, parsec_complex64_t *A, int LDA, int INgrsiz, int INthgrsiz);
//static int  TRD_seqgralgtype(int N, int NB, parsec_complex64_t *A, int LDA, parsec_complex64_t *C, parsec_complex64_t *S, int i, int j, int m, int grsiz, int BAND);
int  blgchase_ztrdv1(int NT, int N, int NB, parsec_complex64_t *A, parsec_complex64_t *V, parsec_complex64_t *TAU, int sweep, int id, int blktile);
int  blgchase_ztrdv2(int NT, int N, int NB, parsec_complex64_t *A1, parsec_complex64_t *A2, parsec_complex64_t *V1, parsec_complex64_t *TAU1, parsec_complex64_t *V2, parsec_complex64_t *TAU2, int sweep, int id, int blktile);

int CORE_zlarfx2(int side, int N,
                parsec_complex64_t V,
                parsec_complex64_t TAU,
                parsec_complex64_t *C1, int LDC1,
                parsec_complex64_t *C2, int LDC2);

int CORE_zlarfx2c(int uplo,
                parsec_complex64_t V,
                parsec_complex64_t TAU,
                parsec_complex64_t *C1,
                parsec_complex64_t *C2,
                parsec_complex64_t *C3);

static void CORE_zhbtelr(int N, int NB, parsec_complex64_t *A1, int LDA1, parsec_complex64_t *A2, int LDA2, parsec_complex64_t *V1, parsec_complex64_t *TAU1, int st, int ed);
static void CORE_zhbtrce(int N, int NB, parsec_complex64_t *A1, int LDA1, parsec_complex64_t *A2, int LDA2, parsec_complex64_t *V1, parsec_complex64_t *TAU1, parsec_complex64_t *V2, parsec_complex64_t *TAU2, int st, int ed, int edglob);
static void CORE_zhbtlrx(int N, int NB, parsec_complex64_t *A1, int LDA1, parsec_complex64_t *A2, int LDA2, parsec_complex64_t *V1, parsec_complex64_t *TAU1, int st, int ed);

static void DLARFX_C(char side, int N, parsec_complex64_t V, parsec_complex64_t TAU, parsec_complex64_t *C, int LDC);
static void TRD_type1bHL(int N, int NB, parsec_complex64_t *A, int LDA, parsec_complex64_t *V, parsec_complex64_t *TAU, int st, int ed);
static void TRD_type2bHL(int N, int NB, parsec_complex64_t *A, int LDA, parsec_complex64_t *V, parsec_complex64_t *TAU, int st, int ed);
static void TRD_type3bHL(int N, int NB, parsec_complex64_t *A, int LDA, parsec_complex64_t *V, parsec_complex64_t *TAU, int st, int ed);

int CORE_zlarfx2(int side, int N,
                parsec_complex64_t V,
                parsec_complex64_t TAU,
                parsec_complex64_t *C1, int LDC1,
                parsec_complex64_t *C2, int LDC2)
{
    static parsec_complex64_t zzero = 0.0;
    int    J;
    parsec_complex64_t V2, T2, SUM;

    /* Quick return */
    /*
    if (N == 0)
        return PLASMA_SUCCESS;
    */
    if (TAU == zzero)
        return 0;

    /*
     * Special code for 2 x 2 Householder where V1 = I
    */
    if(side==PlasmaLeft){
        V2 = conj(V);
        T2 = TAU*conj(V2);
        for (J = 0; J < N ; J++){
           SUM         = C1[J*LDC1]   + V2*C2[J*LDC2];
           C1[J*LDC1]  = C1[J*LDC1]   - SUM*TAU;
           C2[J*LDC2]  = C2[J*LDC2]   - SUM*T2;
        }
    }else if(side==PlasmaRight){
        V2 = V;
        T2 = TAU*conj(V2);
        for (J = 0; J < N ; J++){
           SUM     = C1[J]   + V2*C2[J];
           C1[J]   = C1[J]   - SUM*TAU;
           C2[J]   = C2[J]   - SUM*T2;
        }
    }

    return 0;
}

/***************************************************************************//**
 *
 **/
int CORE_zlarfx2c(int uplo,
                parsec_complex64_t V,
                parsec_complex64_t TAU,
                parsec_complex64_t *C1,
                parsec_complex64_t *C2,
                parsec_complex64_t *C3)
{
    static parsec_complex64_t zzero = 0.0;
    parsec_complex64_t T2, SUM, TEMP;

    /* Quick return */
    if (TAU == zzero)
        return 0;

   /*
    *        Special code for a diagonal block  C1
    *                                           C2  C3
    */
 if(uplo==PlasmaLower){   //do the corner Left then Right (used for the lower case tridiag)
      // L and R for the 2x2 corner
      //            C(N-1, N-1)  C(N-1,N)        C1  TEMP
      //            C(N  , N-1)  C(N  ,N)        C2  C3
      // For Left : use conj(TAU) and V.
      // For Right: nothing, keep TAU and V.
      // Left 1 ==> C1
      //            C2
      TEMP    = conj(C2[0]); // copy C2 here before modifying it.
      T2      = conj(TAU)*V;
      SUM     = C1[0]   + conj(V)*C2[0];
      C1[0]   = C1[0]   - SUM*conj(TAU);
      C2[0]   = C2[0]   - SUM*T2;
      // Left 2 ==> TEMP
      //            C3
      SUM     = TEMP    + conj(V)*C3[0];
      TEMP    = TEMP    - SUM*conj(TAU);
      C3[0]   = C3[0]   - SUM*T2;
      // Right 1 ==>  C1 TEMP.  NB: no need to compute corner (2,2)=TEMP
      T2      = TAU*conj(V);
      SUM     = C1[0]   + V*TEMP;
      C1[0]   = C1[0]   - SUM*TAU;
      // Right 2 ==> C2 C3
      SUM     = C2[0]   + V*C3[0];
      C2[0]   = C2[0]   - SUM*TAU;
      C3[0]   = C3[0]   - SUM*T2;
 }else if(uplo==PlasmaUpper){ // do the corner Right then Left (used for the upper case tridiag)
      //            C(N-1, N-1)  C(N-1,N)        C1    C2
      //            C(N  , N-1)  C(N  ,N)        TEMP  C3
      // For Left : use TAU       and conj(V).
      // For Right: use conj(TAU) and conj(V).
      // Right 1 ==> C1 C2
      V       = conj(V);
      TEMP    = conj(C2[0]); // copy C2 here before modifying it.
      T2      = conj(TAU)*conj(V);
      SUM     = C1[0]   + V*C2[0];
      C1[0]   = C1[0]   - SUM*conj(TAU);
      C2[0]   = C2[0]   - SUM*T2;
      // Right 2 ==> TEMP C3
      SUM     = TEMP    + V*C3[0];
      TEMP    = TEMP    - SUM*conj(TAU);
      C3[0]   = C3[0]   - SUM*T2;
      // Left 1 ==> C1
      //            TEMP. NB: no need to compute corner (2,1)=TEMP
      T2      = TAU*V;
      SUM     = C1[0]   + conj(V)*TEMP;
      C1[0]   = C1[0]   - SUM*TAU;
      // Left 2 ==> C2
      //            C3
      SUM     = C2[0]   + conj(V)*C3[0];
      C2[0]   = C2[0]   - SUM*TAU;
      C3[0]   = C3[0]   - SUM*T2;
 }

 return 0;
}

///////////////////////////////////////////////////////////
//                  DLARFX en C
///////////////////////////////////////////////////////////
static void DLARFX_C(char side, int N, parsec_complex64_t V, parsec_complex64_t TAU, parsec_complex64_t *C, int LDC)
{
 parsec_complex64_t T2, SUM, TEMP;
 int    J, pt;

/*
 *        Special code for 2 x 2 Householder
 */

 T2 = TAU*V;
 if(side=='L'){
     for (J = 0; J < N ; J++){
        pt      = LDC*J;
        SUM     = C[pt]   + V*C[pt+1];
        C[pt]   = C[pt]   - SUM*TAU;
        C[pt+1] = C[pt+1] - SUM*T2;
     }
 }else if(side=='R'){
     for (J = 0; J < N ; J++){
        pt      = J+LDC;
        SUM     = C[J]   + V*C[pt];
        C[J]    = C[J]   - SUM*TAU;
        C[pt]   = C[pt]  - SUM*T2;
     }
 }else if(side=='B'){
     TEMP    = C[ LDC * (N-2) +1];
     for (J = 0; J < N-1 ; J++){
        pt      = LDC*J;
        SUM     = C[pt]   + V*C[pt+1];
        C[pt]   = C[pt]   - SUM*TAU;
        C[pt+1] = C[pt+1] - SUM*T2;
     }
     // L and R for the 2x2 corner
     //            C(1, N-1)  C(1,N)        C(1, N-1)  TEMP
     //            C(2, N-1)  C(2,N)        C(2, N-1)  C(2,N)
     // Left 1 ==> C(1,N-1) C(2,N-1) deja fait dans la boucle
     //pt      = LDC * (N-2);
     //TEMP    = C[pt+1];
     //SUM     = C[pt]   + V*C[pt+1];
     //C[pt]   = C[pt]   - SUM*TAU;
     //C[pt+1] = C[pt+1] - SUM*T2;
     // Left 2 ==> TEMP C(2,N)
     pt      = LDC * (N-1) +1;
     SUM     = TEMP    + V*C[pt];
     TEMP    = TEMP    - SUM*TAU;
     C[pt]   = C[pt]   - SUM*T2;
     // Right 1 ==>  C(1,N-1) TEMP.  NB: no need to compute corner (2,2)
     J       = LDC * (N-2);
     SUM     = C[J]   + V*TEMP;
     C[J]    = C[J]   - SUM*TAU;
     // Right 2 ==> C(2,N-1) C(2,N)
     J      = LDC * (N-2) + 1;
     pt     = LDC * (N-1) + 1;
     SUM    = C[J]   + V*C[pt];
     C[J]   = C[J]   - SUM*TAU;
     C[pt]  = C[pt]  - SUM*T2;
 }
}
///////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////
#define A1(m,n)   &(A1[((m)-(n)) + LDA1*(n)])
#define A2(m,n)   &(A2[((m)-(n)) + LDA2*((n)-NB)])
#define V1(m)     &(V1[m-st])
#define TAU1(m)   &(TAU1[m-st])
#define V2(m)     &(V2[m-st])
#define TAU2(m)   &(TAU2[m-st])
///////////////////////////////////////////////////////////
//                  TYPE 1-BAND Householder
///////////////////////////////////////////////////////////
static void CORE_zhbtelr(int N, int NB, parsec_complex64_t *A1, int LDA1, parsec_complex64_t *A2, int LDA2, parsec_complex64_t *V1, parsec_complex64_t *TAU1, int st, int ed) {
  int    J1, J2, KDM1, LDX;
  int    len, len1, len2, t1ed, t2st;
  int    i, IONE, ITWO;
  IONE=1;
  ITWO=2;
  (void)N;

  KDM1 = NB-1;
  LDX = LDA2-1;
  /* **********************************************************************************************
   *   Annihiliate, then LEFT:
   * ***********************************************************************************************/
  for (i = ed; i >= st+1 ; i--){
     /* generate Householder to annihilate a(i+k-1,i) within the band */
     *V1(i)          = *A1(i, (st-1));
     *A1(i, (st-1))  = 0.0;
     DLARFG( &ITWO, A1((i-1),(st-1)), V1(i), &IONE, TAU1(i) );

     J1   = st;
     J2   = i-2;
     t1ed = min(J2,KDM1);
     t2st = max(J1, NB);
     len1 = t1ed - J1 +1;
     len2 = J2 - t2st +1;
     // printf("Type 1L st %d   ed %d    i %d    J1 %d   J2 %d  t1ed %d  t2st %d len1 %d len2 %d \n", st, ed, i, J1,J2,t1ed,t2st,len1,len2);
     /* apply reflector from the left (horizontal row) and from the right for only the diagonal 2x2.*/
     if(len2>=0){
        /* part of the left(if len2>0) and the corner are on tile T2 */
        if(len2>0) CORE_zlarfx2(PlasmaLeft, len2 , *V1(i), conj(*TAU1(i)), A2((i-1), t2st), LDX, A2(i, t2st), LDX);
        CORE_zlarfx2c(PlasmaLower, *V1(i), *TAU1(i), A2(i-1,i-1), A2(i,i-1), A2(i,i));
        if(len1>0) CORE_zlarfx2(PlasmaLeft, len1 , *V1(i), conj(*TAU1(i)), A1(i-1, J1), LDX, A1(i, J1), LDX);
     }else if(len2==-1){
        /* the left is on tile T1, and only A(i,i) of the corner is on tile T2 */
        CORE_zlarfx2c(PlasmaLower, *V1(i), *TAU1(i), A1(i-1,i-1), A1(i,i-1), A2(i,i));
        if(len1>0) CORE_zlarfx2(PlasmaLeft, len1 , *V1(i), conj(*TAU1(i)), A1(i-1, J1), LDX, A1(i, J1), LDX);
     }else{
        /* the left and the corner are on tile T1, nothing on tile T2 */
        CORE_zlarfx2c(PlasmaLower, *V1(i), *TAU1(i), A1(i-1,i-1), A1(i,i-1), A1(i,i)) ;
        if(len1>0) CORE_zlarfx2(PlasmaLeft, len1 , *V1(i), conj(*TAU1(i)), A1((i-1), J1), LDX, A1(i, J1), LDX);
     }
  }
  /* **********************************************************************************************
   *   APPLY RIGHT ON THE REMAINING ELEMENT OF KERNEL 1
   * ***********************************************************************************************/
  for (i = ed; i >= st+1 ; i--){
     J1    = i+1;
     J2    = ed;
     len   = J2-J1+1;
     if(len>0){
        if(i>NB)
           /* both column (i-1) and i are on tile T2 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A2(J1, i-1), LDX, A2(J1, i), LDX);
        else if(i==NB)
           /* column (i-1) is on tile T1 while column i is on tile T2 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A1(J1, i-1), LDX, A2(J1, i), LDX);
        else
           /* both column (i-1) and i are on tile T1 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A1(J1, i-1), LDX, A1(J1, i), LDX);
     }
  }
}
///////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////
//                  TYPE 2-BAND Householder
///////////////////////////////////////////////////////////
static void CORE_zhbtrce(int N, int NB, parsec_complex64_t *A1, int LDA1, parsec_complex64_t *A2, int LDA2, parsec_complex64_t *V1, parsec_complex64_t *TAU1, parsec_complex64_t *V2, parsec_complex64_t *TAU2, int st, int ed, int edglob) {
  int    J1, J2, J3, KDM1, LDX, pt;
  int    len, len1, len2, t1ed, t2st, iglob;
  int    i, IONE, ITWO;
  parsec_complex64_t V,T,SUM;
  IONE=1;
  ITWO=2;

  iglob = edglob+1;
  LDX   = LDA1-1;
  KDM1  = NB-1;
  /* **********************************************************************************************
   *   Right:
   * ***********************************************************************************************/
  for (i = ed; i >= st+1 ; i--){
     /* apply Householder from the right. and create newnnz outside the band if J3 < N */
     iglob = iglob -1;
     J1  = ed+1;
     if((iglob+NB)>= N){
        J2 = i +(N-iglob-1);
        J3 = J2;
     }else{
        J2 = i + KDM1;
        J3 = J2+1;
     }
     len   = J2-J1+1;
     /* printf("Type 2R st %d   ed %d    i %d   J1 %d   J2 %d  len %d iglob %d  \n",st,ed,i,J1,J2,len,iglob);*/

     if(len>0){
        if(i>NB){
           /* both column (i-1) and i are on tile T2 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A2(J1, i-1), LDX, A2(J1, i), LDX);
           /* if nonzero element need to be created outside the band (if index < N) then create and eliminate it. */
           if(J3>J2){
              /* new nnz at TEMP=V2(i)  */
              V          = *V1(i);
              T          = *TAU1(i) * conj(V);
              SUM        =  V * (*A2(J3, i));
              *V2(i)     = -SUM * (*TAU1(i));
              *A2(J3, i) = *A2(J3, i) - SUM * T;
              /* generate Householder to annihilate a(j+kd,j-1) within the band */
              DLARFG( &ITWO, A2(J2,i-1), V2(i), &IONE, TAU2(i) );
           }
        }else if(i==NB){
           /* column (i-1) is on tile T1 while column i is on tile T2 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A1(J1, i-1), LDX, A2(J1, i), LDX);
           /* if nonzero element need to be created outside the band (if index < N) then create and eliminate it. */
           if(J3>J2){
              /* new nnz at TEMP=V2(i)  */
              V          = *V1(i);
              T          = *TAU1(i) * conj(V);
              SUM        =  V * (*A2(J3, i));
              *V2(i)     = -SUM * (*TAU1(i));
              *A2(J3, i) = *A2(J3, i) - SUM * T;
              /* generate Householder to annihilate a(j+kd,j-1) within the band */
              DLARFG( &ITWO, A1(J2, i-1), V2(i), &IONE, TAU2(i) );
           }
        }else{
           /* both column (i-1) and i are on tile T1 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A1(J1, i-1), LDX, A1(J1, i), LDX);
           /* if nonzero element need to be created outside the band (if index < N) then create and eliminate it. */
           if(J3>J2){
              /* new nnz at TEMP=V2(i)  */
              V              = *V1(i);
              T              = *TAU1(i) * conj(V);
              SUM            =  V * (*A1(J3, i));
              *V2(i)         = -SUM * (*TAU1(i));
              *A1(J3, i)    = *A1(J3, i) - SUM * T;
              /* generate Householder to annihilate a(j+kd,j-1) within the band */
              DLARFG( &ITWO, A1(J2, i-1), V2(i), &IONE, TAU2(i) );
           }
        }
     }
  }
              // if(id==1) return;

  /* **********************************************************************************************
   *   APPLY LEFT ON THE REMAINING ELEMENT OF KERNEL 1
   * ***********************************************************************************************/
  iglob = edglob+1;
  for (i = ed; i >= st+1 ; i--){
     iglob = iglob -1;
     if((iglob+NB)< N){ /* mean that J3>J2 and so a nnz has been created and so a left is required. */
        J1   = i;
        J2   = ed;
        t1ed = min(J2,KDM1);
        t2st = max(J1, NB);
        len1 = t1ed - J1 +1;
        len2 = J2 - t2st +1;
        pt   = i + KDM1; /* pt correspond to the J2 position of the corresponding right done above */
        //printf("Type 2L st %d   ed %d    i %d    J1 %d   J2 %d  t1ed %d  t2st %d len1 %d len2 %d \n", st, ed, i, J1,J2,t1ed,t2st,len1,len2);

        /* apply reflector from the left (horizontal row) and from the right for only the diagonal 2x2.*/
        if(len2>0){
           /* part of the left(if len2>0) and the corner are on tile T2 */
           CORE_zlarfx2(PlasmaLeft, len2 , *V2(i), conj(*TAU2(i)), A2(pt, t2st), LDX, A2(pt+1, t2st), LDX);
           if(len1>0) CORE_zlarfx2(PlasmaLeft, len1 , *V2(i), conj(*TAU2(i)), A1(pt, J1), LDX, A1(pt+1, J1), LDX);
        }else if(len1>0){
           /* the left and the corner are on tile T1, nothing on tile T2 */
           CORE_zlarfx2(PlasmaLeft, len1 , *V2(i), conj(*TAU2(i)), A1(pt, J1), LDX, A1(pt+1, J1), LDX);
        }
     }
  }
}
///////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////
//                  TYPE 1-BAND Householder
///////////////////////////////////////////////////////////
static void CORE_zhbtlrx(int N, int NB, parsec_complex64_t *A1, int LDA1, parsec_complex64_t *A2, int LDA2, parsec_complex64_t *V1, parsec_complex64_t *TAU1, int st, int ed) {
  int    J1, J2, KDM1, LDX;
  int    len, len1, len2, t1ed, t2st;
  int    i;

  (void)N;
  KDM1 = NB-1;
  LDX = LDA2-1;
  /* **********************************************************************************************
   *   Annihiliate, then LEFT:
   * ***********************************************************************************************/
  for (i = ed; i >= st+1 ; i--){
     J1   = st;
     J2   = i-2;
     t1ed = min(J2,KDM1);
     t2st = max(J1, NB);
     len1 = t1ed - J1 +1;
     len2 = J2 - t2st +1;
     //printf("Type 3L st %d   ed %d    i %d    J1 %d   J2 %d  t1ed %d  t2st %d len1 %d len2 %d \n", st, ed, i, J1,J2,t1ed,t2st,len1,len2);
     /* apply reflector from the left (horizontal row) and from the right for only the diagonal 2x2.*/
     if(len2>=0){
        /* part of the left(if len2>0) and the corner are on tile T2 */
        if(len2>0) CORE_zlarfx2(PlasmaLeft, len2 , *V1(i), conj(*TAU1(i)), A2((i-1), t2st), LDX, A2(i, t2st), LDX);
        CORE_zlarfx2c(PlasmaLower, *V1(i), *TAU1(i), A2(i-1,i-1), A2(i,i-1), A2(i,i));
        if(len1>0) CORE_zlarfx2(PlasmaLeft, len1 , *V1(i), conj(*TAU1(i)), A1(i-1, J1), LDX, A1(i, J1), LDX);
     }else if(len2==-1){
        /* the left is on tile T1, and only A(i,i) of the corner is on tile T2 */
        CORE_zlarfx2c(PlasmaLower, *V1(i), *TAU1(i), A1(i-1,i-1), A1(i,i-1), A2(i,i));
        if(len1>0) CORE_zlarfx2(PlasmaLeft, len1 , *V1(i), conj(*TAU1(i)), A1(i-1, J1), LDX, A1(i, J1), LDX);
     }else{
        /* the left and the corner are on tile T1, nothing on tile T2 */
        CORE_zlarfx2c(PlasmaLower, *V1(i), *TAU1(i), A1(i-1,i-1), A1(i,i-1), A1(i,i)) ;
        if(len1>0) CORE_zlarfx2(PlasmaLeft, len1 , *V1(i), conj(*TAU1(i)), A1((i-1), J1), LDX, A1(i, J1), LDX);
     }
  }
  /* **********************************************************************************************
   *   APPLY RIGHT ON THE REMAINING ELEMENT OF KERNEL 1
   * ***********************************************************************************************/
  for (i = ed; i >= st+1 ; i--){
     J1    = i+1;
     J2    = ed;
     len   = J2-J1+1;
     if(len>0){
        if(i>NB)
           /* both column (i-1) and i are on tile T2 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A2(J1, i-1), LDX, A2(J1, i), LDX);
        else if(i==NB)
           /* column (i-1) is on tile T1 while column i is on tile T2 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A1(J1, i-1), LDX, A2(J1, i), LDX);
        else
           /* both column (i-1) and i are on tile T1 */
           CORE_zlarfx2(PlasmaRight, len, *V1(i), *TAU1(i), A1(J1, i-1), LDX, A1(J1, i), LDX);
     }
  }
}
///////////////////////////////////////////////////////////
#undef A1
#undef A2
#undef V1
#undef TAU1
#undef V2
#undef TAU2

///////////////////////////////////////////////////////////
//                  TYPE 1-BAND Householder
///////////////////////////////////////////////////////////
//// add -1 because of C
#define A(m,n)   &(A[((m)-(n)) + LDA*((n)-1)])
#define V(m)     &(V[m-1])
#define TAU(m)   &(TAU[m-1])
static void TRD_type1bHL(int N, int NB, parsec_complex64_t *A, int LDA, parsec_complex64_t *V, parsec_complex64_t *TAU, int st, int ed) {
  int    J1, J2, len, LDX;
  int    i, IONE, ITWO;
  IONE=1;
  ITWO=2;
  (void)NB;

  if(ed <= st){
    printf("TRD_type 1bH: ERROR st and ed %d  %d \n",st,ed);
    exit(-10);
  }

  LDX = LDA-1;
  for (i = ed; i >= st+1 ; i--){
     // generate Householder to annihilate a(i+k-1,i) within the band
     *V(i)          = *A(i, (st-1));
     *A(i, (st-1))  = 0.0;
     DLARFG( &ITWO, A((i-1),(st-1)), V(i), &IONE, TAU(i) );

     // apply reflector from the left (horizontal row) and from the right for only the diagonal 2x2.
     J1  = st;
     J2  = i;
     len = J2-J1+1;
     printf("voici J1 %d J2 %d len %d \n",J1,J2,len);
     DLARFX_C('B', len , *V(i), *TAU(i), A((i-1),J1   ), LDX);
  }

  for (i = ed; i >= st+1 ; i--){
     len = min(ed,N)-i;
     if(len>0)DLARFX_C('R', len, *V(i), *TAU(i), A((i+1),(i-1)), LDX);
  }

}
#undef A
#undef V
#undef TAU
///////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////
//                  TYPE 2-BAND Householder
///////////////////////////////////////////////////////////
//// add -1 because of C
#define A(m,n)   &(A[((m)-(n)) + LDA*((n)-1)])
#define V(m)     &(V[m-1])
#define TAU(m)   &(TAU[m-1])
static void TRD_type2bHL(int N, int NB, parsec_complex64_t *A, int LDA, parsec_complex64_t *V, parsec_complex64_t *TAU, int st, int ed) {
  int    J1, J2, J3, KDM2, len, LDX;
  int    i, IONE, ITWO;
  IONE=1;
  ITWO=2;

  if(ed <= st){
    printf("TRD_type 2H: ERROR st and ed %d  %d \n",st,ed);
    exit(-10);;
  }

  LDX  = LDA -1;
  KDM2 = NB-2;
  for (i = ed; i >= st+1 ; i--){
     // apply Householder from the right. and create newnnz outside the band if J3 < N
     J1  = ed+1;
     J2  = min((i+1+KDM2), N);
     J3  = min((J2+1), N);
     len = J2-J1+1;
     DLARFX_C('R', len, *V(i), *TAU(i), A(J1,(i-1)), LDX);

     // if nonzero element a(j+kd,j-1) has been created outside the band (if index < N) then eliminate it.
     len    = J3-J2; // soit 1 soit 0
     if(len>0){
         //  new nnz at TEMP=V(J3)
         *V(J3)     =            - *A(J3,(i)) * (*TAU(i)) * (*V(i));
         *A(J3,(i)) = *A(J3,(i)) + *V(J3)  * (*V(i)); //ATTENTION THIS replacement IS VALID IN FLOAT CASE NOT IN COMPLEX
         // generate Householder to annihilate a(j+kd,j-1) within the band
         DLARFG( &ITWO, A(J2,(i-1)), V(J3), &IONE, TAU(J3) );
     }
  }
               //if(id==1) return;

  for (i = ed; i >= st+1 ; i--){
     J2  = min((i+1+KDM2), N);
     J3  = min((J2+1), N);
     len    = J3-J2;
     if(len>0){
        len = min(ed,N)-i+1;
        DLARFX_C('L', len , *V(J3), *TAU(J3), A(J2, i), LDX);
     }
  }

}
#undef A
#undef V
#undef TAU
///////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////
//                  TYPE 3-BAND Householder
///////////////////////////////////////////////////////////
//// add -1 because of C
#define A(m,n)   &(A[((m)-(n)) + LDA*((n)-1)])
#define V(m)     &(V[m-1])
#define TAU(m)   &(TAU[m-1])
static void TRD_type3bHL(int N, int NB, parsec_complex64_t *A, int LDA, parsec_complex64_t *V, parsec_complex64_t *TAU, int st, int ed) {
  int    J1, J2, len, LDX;
  int    i;
  (void)NB;

  if(ed <= st){
    printf("TRD_type 3H: ERROR st and ed %d  %d \n",st,ed);
    exit(-10);
  }

  LDX = LDA-1;
  for (i = ed; i >= st+1 ; i--){
     // apply rotation from the left. faire le DROT horizontal
     J1  = st;
     J2  = i;
     len = J2-J1+1;
     //len = NB+1;
     DLARFX_C('B', len , *V(i), *TAU(i), A((i-1),J1   ), LDX);
  }

  for (i = ed; i >= st+1 ; i--){
     len = min(ed,N)-i;
     if(len>0)DLARFX_C('R', len, *V(i), *TAU(i), A((i+1),(i-1)), LDX);
  }
}
#undef A
#undef V
#undef TAU
///////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////
//                  grouping sched wrapper call
///////////////////////////////////////////////////////////
#if 0
int TRD_seqgralgtype(int N, int NB, parsec_complex64_t *A, int LDA, parsec_complex64_t *C, parsec_complex64_t *S, int i, int j, int m, int grsiz, int BAND) {
  int    k,shift=3;
  int    myid,colpt,stind,edind,blklastind,stepercol;
  (void) BAND;

  k   = shift/grsiz;
  stepercol =  k*grsiz == shift ? k:k+1;

   for (k = 1; k <=grsiz; k++){
      myid = (i-j)*(stepercol*grsiz) +(m-1)*grsiz + k;
      if(myid%2 ==0){
           colpt      = (myid/2)*NB+1+j-1;
           stind      = colpt-NB+1;
           edind      = min(colpt,N);
           blklastind = colpt;
           if(stind>=edind){
               printf("TRD_seqalg ERROR---------> st>=ed  %d  %d \n\n",stind, edind);
               return -10;
           }
      }else{
           colpt      = ((myid+1)/2)*NB + 1 +j -1 ;
           stind      = colpt-NB+1;
           edind      = min(colpt,N);
           if( (stind>=edind-1) && (edind==N) )
               blklastind=N;
           else
               blklastind=0;
           if(stind>=edind){
               printf("TRD_seqalg ERROR---------> st>=ed  %d  %d \n\n",stind, edind);
               return -10;

           }
      }

          if(myid == 1)
              TRD_type1bHL(N, NB, A, LDA, C, S, stind, edind);
          else if(myid%2 == 0)
              TRD_type2bHL(N, NB, A, LDA, C, S, stind, edind);
          else if(myid%2 == 1)
              TRD_type3bHL(N, NB, A, LDA, C, S, stind, edind);
          else{
              printf("COUCOU ERROR myid/2 %d\n",myid);
               return -10;
          }

      if(blklastind >= (N-1))  break;
      //if(myid==2)return;
  }   // END for k=1:grsiz
return 0;
}
///////////////////////////////////////////////////////////
#endif

///////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                  REDUCTION BAND TO TRIDIAG
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
#if 0
static void band_to_trd_v8seq(int N, int NB, parsec_complex64_t *A, int LDA, int INgrsiz, int INthgrsiz) {
  int myid, grsiz, shift, stt, st, ed, stind, edind, BAND;
  int blklastind, colpt;
  int stepercol,mylastid;
  parsec_complex64_t *C, *S;
  int i,j,m;
  int thgrsiz, thgrnb, thgrid, thed;
  int INFO;
  INFO=-1;
  BAND = 0;
  C   = malloc(N*sizeof(parsec_complex64_t));
  S   = malloc(N*sizeof(parsec_complex64_t));
  memset(C,0,N*sizeof(parsec_complex64_t));
  memset(S,0,N*sizeof(parsec_complex64_t));

  grsiz   = INgrsiz;
  thgrsiz = INthgrsiz;

  shift   = 3;
  if(grsiz==0)grsiz   = 6;
  if(thgrsiz==0)thgrsiz = N;

  if(LDA != (NB+1))
  {
      printf(" ERROR LDA not equal NB+1 and this code is special for LDA=NB+1. LDA=%d NB+1=%d \n",LDA,NB+1);
      return;
  }
  printf("  Version -8seq- grsiz %4d     thgrsiz %4d       N %5d      NB %5d    BAND %5d\n",grsiz,thgrsiz, N, NB, BAND);

  i = shift/grsiz;
  stepercol =  i*grsiz == shift ? i:i+1;

  i       = (N-2)/thgrsiz;
  thgrnb  = i*thgrsiz == (N-2) ? i:i+1;

  for (thgrid = 1; thgrid<=thgrnb; thgrid++){
     stt  = (thgrid-1)*thgrsiz+1;
     thed = min( (stt + thgrsiz -1), (N-2));
     for (i = stt; i <= N-2; i++){
        ed=min(i,thed);
        if(stt>ed)break;
        for (m = 1; m <=stepercol; m++){
            st=stt;
            for (j = st; j <=ed; j++){
                 myid     = (i-j)*(stepercol*grsiz) +(m-1)*grsiz + 1;
                 mylastid = myid+grsiz-1;
                 INFO = TRD_seqgralgtype(N, NB, A, LDA, C, S, i, j, m, grsiz, BAND);
                 if(INFO!=0){
                         printf("ERROR band_to_trd_v8seq INFO=%d\n",INFO);
                         return;
                 }
                 if(mylastid%2 ==0){
                     blklastind      = (mylastid/2)*NB+1+j-1;
                 }else{
                      colpt      = ((mylastid+1)/2)*NB + 1 +j -1 ;
                      stind      = colpt-NB+1;
                      edind      = min(colpt,N);
                      if( (stind>=edind-1) && (edind==N) )
                          blklastind=N;
                      else
                          blklastind=0;
                 }
                 if(blklastind >= (N-1))  stt=stt+1;
           } // END for j=st:ed
        } // END for m=1:stepercol
     } // END for i=1:N-2
  } // END for thgrid=1:thgrnb

} // END FUNCTION
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
#endif

///////////////////////////////////////////////////////////////////////////////////////////////////////////////
int blgchase_ztrdv1(int NT, int N, int NB, parsec_complex64_t *A, parsec_complex64_t *V, parsec_complex64_t *TAU, int sweep, int id, int blktile) {
  int    /*edloc,*/ stloc, st, ed, KDM1, LDA;

  (void)NT;
  KDM1   = NB-1;
  LDA    = NB+1;
  /* generate the indiceslocal and global*/
  stloc  = (sweep+1)%NB;
  if(stloc==0) stloc=NB;
  /*if(id==NT-1)
     edloc = NB-1;
  else
     edloc = stloc + KDM1;
  */

  st = min(id*NB+stloc, N-1);
  ed = min(st+KDM1, N-1);
  /*
   * i     = (N-1)%ed;
   * edloc = i%NB+NB;
   */

  /* quick return in case of last tile */
  if(st==ed)
     return 0;

  /* because the kernel have been writted for fortran, add 1 */
   st = st +1;
   ed = ed +1;
  /* code for all tiles */
  if(id==blktile){
     TRD_type1bHL(N, NB, A, LDA, V, TAU, st, ed);
     TRD_type2bHL(N, NB, A, LDA, V, TAU, st, ed);
  }else{
     TRD_type3bHL(N, NB, A, LDA, V, TAU, st, ed);
     //if(id==6) return;
     TRD_type2bHL(N, NB, A, LDA, V, TAU, st, ed);
  }
  return 0;
}

#if 0
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
static void band_to_trd_vmpi1(int N, int NB, parsec_complex64_t *A, int LDA) {
  int NT;
  parsec_complex64_t *V, *TAU;
  int blktile, S, id, sweep;
  V   = malloc(N*sizeof(parsec_complex64_t));
  TAU = malloc(N*sizeof(parsec_complex64_t));
  memset(V,0,N*sizeof(parsec_complex64_t));
  memset(TAU,0,N*sizeof(parsec_complex64_t));

  NT = N/NB;
  if(NT*NB != N){
      printf("ERROR NT*NB not equal N \n");
      return;
  }

  //printf("voici NT, N NB %d %d %d\n",NT,N,NB);
  for (blktile = 0; blktile<NT; blktile++){
     for (S = 0; S<NB; S++){
        sweep = blktile*NB + S ;
        for (id = blktile; id<NT; id++){
               blgchase_ztrdv1 (NT, N, NB, A, V, TAU, sweep , id, blktile);
        }
     }
  }

}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
#endif

///////////////////////////////////////////////////////////////////////////////////////////////////////////////
int blgchase_ztrdv2(int NT, int N, int NB, parsec_complex64_t *A1, parsec_complex64_t *A2, parsec_complex64_t *V1, parsec_complex64_t *TAU1, parsec_complex64_t *V2, parsec_complex64_t *TAU2, int sweep, int id, int blktile) {
  int    edloc, stloc, st, ed, KDM1, LDA;

  KDM1   = NB-1;
  LDA    = NB+1;
  /* generate the indiceslocal and global*/
  stloc  = (sweep+1)%NB;
  if(stloc==0) stloc=NB;
  if(id==NT-1)
     edloc = NB-1;
  else
     edloc = stloc + KDM1;

  st = min(id*NB+stloc, N-1);
  ed = min(st+KDM1, N-1);
  /*
   * i     = (N-1)%ed;
   * edloc = i%NB+NB;
   */

  /* quick return in case of last tile */
  if(st==ed)
     return 0;

  /* code for all tiles */
  if(id==blktile){
     CORE_zhbtelr(N, NB, A1, LDA, A2, LDA, V1, TAU1, stloc, edloc);
     CORE_zhbtrce(N, NB, A1, LDA, A2, LDA, V1, TAU1, V2, TAU2, stloc, edloc, ed);
  }else{
     CORE_zhbtlrx(N, NB, A1, LDA, A2, LDA, V1, TAU1, stloc, edloc);
     CORE_zhbtrce(N, NB, A1, LDA, A2, LDA, V1, TAU1, V2, TAU2, stloc, edloc, ed);
  }
  return 0;
}

#if 0
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
static void band_to_trd_vmpi2(int N, int NB, parsec_complex64_t *A, int LDA) {
    int NT;
    parsec_complex64_t *V, *TAU;
    int blktile, S, id, sweep;
    V   = malloc(N*sizeof(parsec_complex64_t));
    TAU = malloc(N*sizeof(parsec_complex64_t));
    memset(V,0,N*sizeof(parsec_complex64_t));
    memset(TAU,0,N*sizeof(parsec_complex64_t));

    NT = N/NB;
    if(NT*NB != N){
        printf("ERROR NT*NB not equal N \n");
        return;
    }

    //printf("voici NT, N NB %d %d %d\n",NT,N,NB);
    for (blktile = 0; blktile<NT; blktile++){
        for (S = 0; S<NB; S++){
            sweep = blktile*NB + S ;
            for (id = blktile; id<NT; id++){
                //printf("voici  blktile %d    S %d     id %d   sweep %d   \n",blktile, S, id, sweep);
                blgchase_ztrdv2 (NT, N, NB,
                                A+(id*NB*LDA), A+((id+1)*NB*LDA),
                                V+(id*NB), TAU+(id*NB),
                                V+((id+1)*NB), TAU+((id+1)*NB),
                                sweep, id, blktile);
            }
            //if(sweep==0) return;
        }
    }

}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////
#endif


/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2018-2019, Colorado School of Mines
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ndantam@miens.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef AMINO_MAT_H
#define AMINO_MAT_H

#include <cblas.h>

/**
 * @file mat.h
 *
 * Block matrix descriptors and linear algebra operations.
 *
 */

typedef size_t aa_la_size;

/**
 * Descriptor for a vector.
 */
struct aa_dvec {
    aa_la_size len;   ///< Number of elements in vector
    double *data; ///< Pointer to data
    aa_la_size inc;   ///< Increment between successive vector elements
};

/**
 * Descriptor for a block matrix.
 */
struct aa_dmat {
    aa_la_size rows;    ///< number of rows in matrix
    aa_la_size cols;    ///< number of columns
    double *data;   ///< Pointer to matrix data
    aa_la_size ld;      ///< Leading dimension of matrix
};

#define AA_DVEC_REF(v,i) ((v)->data[(i) * (v)->inc])

/**
 * Reference a matrix entry.
 *
 * @param M matrix
 * @param i row
 * @param j column
 */
#define AA_DMAT_REF(M,i,j) (((M)->data)[(M)->ld*(j) + (i)])

typedef void
(aa_la_err_fun)( const char *message );

AA_API void
aa_la_err( const char *message );

AA_API void
aa_la_set_err( aa_la_err_fun *fun );

/**
 * BLAS arguments for a vector
 */
#define AA_VEC_ARGS(X) (X->data), ((int)(X->inc))

/**
 * BLAS arguments for a matrix
 */
#define AA_MAT_ARGS(X) (X->data), ((int)(X->ld))

/* Construction */

/**
 * Fill in a vector descriptor.
 *
 * @param len  Number of elements in vector
 * @param data Pointer to vector data
 * @param inc  Increment between sucessive elements
 */
static inline struct aa_dvec
AA_DVEC_INIT( size_t len, double *data, size_t inc )
{
    struct aa_dvec vec;
    vec.len = len;
    vec.data = data;
    vec.inc = inc;
    return vec;
}

/**
 * Fill in a vector descriptor.
 *
 * @param vec  Pointer to descriptor
 * @param len  Number of elements in vector
 * @param data Pointer to vector data
 * @param inc  Increment between sucessive elements
 */
AA_API void
aa_dvec_view( struct aa_dvec *vec, size_t len, double *data, size_t inc );

/**
 * Fill in a matrix descriptor.
 *
 * @param mat   Pointer to descriptor
 * @param rows  Number of rows in matrix
 * @param cols  Number of colums in matrix
 * @param data  Pointer to vector data
 * @param ld    Leading dimension of matrix
 */
AA_API void
aa_dmat_view( struct aa_dmat *mat, size_t rows, size_t cols, double *data, size_t ld );

/**
 * View a block of a matrix.
 */
AA_API void
aa_dmat_view_block( struct aa_dmat *dst,
                    const struct aa_dmat *src,
                    size_t row_start, size_t col_start,
                    size_t rows, size_t cols );

/**
 * View a slice of a vector.
 */
AA_API void
aa_dvec_slice( const struct aa_dvec *src,
               size_t start,
               size_t stop,
               size_t step,
               struct aa_dvec *dst );

/**
 * View a block of a matrix.
 */
AA_API void
aa_dmat_block( const struct aa_dmat *src,
               size_t row_start, size_t col_start,
               size_t row_end, size_t col_end,
               struct aa_dmat *dst );

/**
 * View a row of a matrix as a vector.
 */
AA_API void
aa_dmat_row_vec( const struct aa_dmat *src, size_t row, struct aa_dvec *dst );

/**
 * View a column of a matrix as a vector.
 */
AA_API void
aa_dmat_col_vec( const struct aa_dmat *src, size_t col, struct aa_dvec *dst );

/**
 * View the diagonal of a matrix as a vector.
 */
AA_API void
aa_dmat_diag_vec( const struct aa_dmat *src, struct aa_dvec *dst );

/**
 * Fill in a matrix descriptor.
 *
 * @param rows  Number of rows in matrix
 * @param cols  Number of colums in matrix
 * @param data  Pointer to vector data
 * @param ld    Leading dimension of matrix
 */
static inline struct aa_dmat
AA_DMAT_INIT( size_t rows, size_t cols, double *data, size_t ld )
{
    struct aa_dmat mat;
    mat.rows = rows;
    mat.cols = cols;
    mat.data = data;
    mat.ld = ld;
    return mat;
}

#define AA_MAT_DIAG(VEC,MAT)                                    \
    aa_dvec_view((VEC), (MAT)->cols, (MAT)->data, 1+(MAT)->ld);

/**
 * Region-allocate a vector.
 *
 * When finished, pop the descriptor.
 */
AA_API struct aa_dvec *
aa_dvec_alloc( struct aa_mem_region *reg, size_t len );

/**
 * Duplicate vector out of region
 *
 * When finished, pop the descriptor.
 */
AA_API struct aa_dvec *
aa_dvec_dup( struct aa_mem_region *reg, const struct aa_dvec *src);

/**
 * Duplicate matrix out of region
 *
 * When finished, pop the descriptor.
 */
AA_API struct aa_dmat *
aa_dmat_dup( struct aa_mem_region *reg, const struct aa_dmat *src);

/**
 * Region-allocate a matrix.
 *
 * When finished, pop the descriptor.
 */
AA_API struct aa_dmat *
aa_dmat_alloc( struct aa_mem_region *reg, size_t rows, size_t cols );

/**
 * Heap-allocate a vector.
 *
 * The descriptor and data are contained in a single malloc()'ed block.
 * When finished, call free() on the descriptor.
 */
AA_API struct aa_dvec *
aa_dvec_malloc( size_t len );

/**
 * Heap-allocate a matrix.
 *
 * The descriptor and data are contained in a single malloc()'ed block.
 * When finished, call free() on the descriptor.
 */
AA_API struct aa_dmat *
aa_dmat_malloc( size_t rows, size_t cols );

/**
 * Zero a vector.
 */
AA_API void
aa_dvec_zero( struct aa_dvec *vec );

/**
 * Fill a vector.
 */
AA_API void
aa_dvec_set( struct aa_dvec *vec, double alpha );

/**
 * Fill a matrix diagonal and off-diagonal elements.
 *
 * @param[in] A        The matrix to fill
 * @param[in] alpha    off-diagonal entries of A
 * @param[in] beta     diagonal entries of A
 */
AA_API void
aa_dmat_set( struct aa_dmat *A, double alpha, double beta );

/**
 * Zero a matrix.
 */
AA_API void
aa_dmat_zero( struct aa_dmat *mat );

/* Level 1 BLAS */

/**
 * Swap x and y
 *
 * \f[ \mathbf{x} \leftrightarrow \mathbf{y} \f]
 */
AA_API void
aa_dvec_swap( struct aa_dvec *x, struct aa_dvec *y );

/**
 * Scale x by alpha.
 *
 * \f[ \mathbf{x} \leftarrow \alpha \mathbf{x} \f]
 */
AA_API void
aa_dvec_scal( double alpha, struct aa_dvec *x );

/**
 * Increment x by alpha.
 *
 * \f[ \mathbf{x} \leftarrow \alpha + \mathbf{x} \f]
 */
AA_API void
aa_dvec_inc( double alpha, struct aa_dvec *x );

/**
 * Copy x to y.
 *
 * \f[ \mathbf{y} \leftarrow \mathbf{x} \f]
 */
AA_API void
aa_dvec_copy( const struct aa_dvec *x, struct aa_dvec *y );

/**
 * Alpha x plus y.
 *
 * \f[ \mathbf{y} \leftarrow \alpha \mathbf{x} + \mathbf{y} \f]
 */
AA_API void
aa_dvec_axpy( double a, const struct aa_dvec *x, struct aa_dvec *y );

/**
 * Dot product
 *
 * \f[ \mathbf{x}^T \mathbf{y} \f]
 */
AA_API double
aa_dvec_dot( const struct aa_dvec *x, struct aa_dvec *y );

/**
 * Euclidean Norm
 *
 * \f[ \left\Vert \mathbf{x} \right\Vert_2  \f]
 */
AA_API double
aa_dvec_nrm2( const struct aa_dvec *x );

/* Level 2 BLAS */

/**
 * General Matrix-Vector multiply
 *
 * \f[ \mathbf{y} \leftarrow \alpha \mathbf{A}^{\rm op} \mathbf{x} + \beta \mathbf{y}  \f]
 */
AA_API void
aa_dmat_gemv( CBLAS_TRANSPOSE trans,
              double alpha, const struct aa_dmat *A,
              const struct aa_dvec *x,
              double beta, struct aa_dvec *y );

/* Level 3 BLAS */

/**
 * General Matrix-Matrix multiply
 *
 * \f[ \mathbf{y} \leftarrow \alpha \mathbf{A}^{\rm opA} \mathbf{B}^\rm{opB} + \beta \mathbf{C}  \f]
 */
AA_API void
aa_dmat_gemm( CBLAS_TRANSPOSE transA, CBLAS_TRANSPOSE transB,
              double alpha, const struct aa_dmat *A,
              const struct aa_dmat *B,
              double beta, struct aa_dmat *C );

/* LAPACK */

/**
 * Copies all or part of a two-dimensional matrix A to another
 * matrix B.
 *
 *  @param[in] UPLO
 *          Specifies the part of the matrix A to be copied to B.
 *          - = 'U':      Upper triangular part
 *          - = 'L':      Lower triangular part
 *          - Otherwise:  All of the matrix A
 *
 *  @param[in] A
 *          dimension (LDA,N)
 *          The m by n matrix A.  If UPLO = 'U', only the upper triangle
 *          or trapezoid is accessed; if UPLO = 'L', only the lower
 *          triangle or trapezoid is accessed.
 *
 *  @param[out] B
 *          dimension (LDB,N)
 *          On exit, B = A in the locations specified by UPLO.
 *
 */
AA_API void
aa_dmat_lacpy( const char uplo[1],
               const struct aa_dmat *A,
               struct aa_dmat *B );

/* Matrix/Vector Functions */

/**
 * sum-square-differences of two vectors
 */
AA_API double
aa_dvec_ssd( const struct aa_dvec *x, const struct aa_dvec *y);

/**
 * Y += alpha * X
 */
AA_API void
aa_dmat_axpy( double alpha, const struct aa_dmat *X, struct aa_dmat *Y);

/**
 * sum-square-differences of two matrices
 */
AA_API double
aa_dmat_ssd( const struct aa_dmat *x, const struct aa_dmat *y);

/**
 * Scale the matrix A by alpha
 */
AA_API void
aa_dmat_scal( struct aa_dmat *A, double alpha );

/**
 * Increment the matrix A by alpha
 */
AA_API void
aa_dmat_inc( struct aa_dmat *A, double alpha );

/**
 * Euclidean Norm
 */
AA_API double
aa_dmat_nrm2( const struct aa_dmat *x );

/**
 * Matrix transpose.
 */
AA_API void
aa_dmat_trans( const struct aa_dmat *A, struct aa_dmat *At);

/**
 * Matrix inverse, in-place.
 */
AA_API int
aa_dmat_inv( struct aa_dmat *A);

/**
 * Pseudo-inverse.
 *
 * Singular values less than tol are ignored.  If tol < 0, then a sane
 * default is used.
 */
AA_API int
aa_dmat_pinv( const struct aa_dmat *A, double tol, struct aa_dmat *As);

/**
 * Damped pseudo-inverse.
 */
AA_API int
aa_dmat_dpinv( const struct aa_dmat *A, double k, struct aa_dmat *As);

/**
 * Dead-zone damped pseudo-inverse.
 */
AA_API int
aa_dmat_dzdpinv(  const struct aa_dmat *A, double s_min, struct aa_dmat *As);

/**
 * Copy a matrix
 */
AA_API void
aa_dmat_copy(  const struct aa_dmat *A, struct aa_dmat *B);

#endif /* AMINO_MAT_H */