/**
 *
 * @file qwrapper_sgemm.c
 *
 *  PLASMA core_blas quark wrapper
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Jakub Kurzak
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:44:56 2014
 *
 **/
#include <cblas.h>
#include "common.h"

/***************************************************************************//**
 *
 **/
void QUARK_CORE_sgemm(Quark *quark, Quark_Task_Flags *task_flags,
                      PLASMA_enum transA, int transB,
                      int m, int n, int k, int nb,
                      float alpha, const float *A, int lda,
                                                const float *B, int ldb,
                      float beta, float *C, int ldc)
{
    DAG_CORE_GEMM;
    QUARK_Insert_Task(quark, CORE_sgemm_quark, task_flags,
        sizeof(PLASMA_enum),                &transA,    VALUE,
        sizeof(PLASMA_enum),                &transB,    VALUE,
        sizeof(int),                        &m,         VALUE,
        sizeof(int),                        &n,         VALUE,
        sizeof(int),                        &k,         VALUE,
        sizeof(float),         &alpha,     VALUE,
        sizeof(float)*nb*nb,    A,                 INPUT,
        sizeof(int),                        &lda,       VALUE,
        sizeof(float)*nb*nb,    B,                 INPUT,
        sizeof(int),                        &ldb,       VALUE,
        sizeof(float),         &beta,      VALUE,
        sizeof(float)*nb*nb,    C,                 INOUT,
        sizeof(int),                        &ldc,       VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_sgemm2( Quark *quark, Quark_Task_Flags *task_flags,
                        PLASMA_enum transA, int transB,
                        int m, int n, int k, int nb,
                        float alpha, const float *A, int lda,
                        const float *B, int ldb,
                        float beta, float *C, int ldc)
{
    DAG_CORE_GEMM;
    QUARK_Insert_Task(quark, CORE_sgemm_quark, task_flags,
        sizeof(PLASMA_enum),                &transA,    VALUE,
        sizeof(PLASMA_enum),                &transB,    VALUE,
        sizeof(int),                        &m,         VALUE,
        sizeof(int),                        &n,         VALUE,
        sizeof(int),                        &k,         VALUE,
        sizeof(float),         &alpha,     VALUE,
        sizeof(float)*nb*nb,    A,                 INPUT,
        sizeof(int),                        &lda,       VALUE,
        sizeof(float)*nb*nb,    B,                 INPUT,
        sizeof(int),                        &ldb,       VALUE,
        sizeof(float),         &beta,      VALUE,
        sizeof(float)*nb*nb,    C,                 INOUT | LOCALITY | GATHERV,
        sizeof(int),                        &ldc,       VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgemm_quark = PCORE_sgemm_quark
#define CORE_sgemm_quark PCORE_sgemm_quark
#endif
void CORE_sgemm_quark(Quark *quark)
{
    PLASMA_enum transA;
    PLASMA_enum transB;
    int m;
    int n;
    int k;
    float alpha;
    float *A;
    int lda;
    float *B;
    int ldb;
    float beta;
    float *C;
    int ldc;

    quark_unpack_args_13(quark, transA, transB, m, n, k, alpha, A, lda, B, ldb, beta, C, ldc);
    cblas_sgemm(
        CblasColMajor,
        (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB,
        m, n, k,
        (alpha), A, lda,
        B, ldb,
        (beta), C, ldc);
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_sgemm_f2(Quark *quark, Quark_Task_Flags *task_flags,
                         PLASMA_enum transA, int transB,
                         int m, int n, int k, int nb,
                         float alpha, const float *A, int lda,
                                                   const float *B, int ldb,
                         float beta, float *C, int ldc,
                         float *fake1, int szefake1, int flag1,
                         float *fake2, int szefake2, int flag2)
{
    DAG_CORE_GEMM;
    QUARK_Insert_Task(quark, CORE_sgemm_f2_quark, task_flags,
        sizeof(PLASMA_enum),                &transA,    VALUE,
        sizeof(PLASMA_enum),                &transB,    VALUE,
        sizeof(int),                        &m,         VALUE,
        sizeof(int),                        &n,         VALUE,
        sizeof(int),                        &k,         VALUE,
        sizeof(float),         &alpha,     VALUE,
        sizeof(float)*nb*nb,    A,                 INPUT,
        sizeof(int),                        &lda,       VALUE,
        sizeof(float)*nb*nb,    B,                 INPUT,
        sizeof(int),                        &ldb,       VALUE,
        sizeof(float),         &beta,      VALUE,
        sizeof(float)*nb*nb,    C,                 INOUT | LOCALITY,
        sizeof(int),                        &ldc,       VALUE,
        sizeof(float)*szefake1, fake1,             flag1,
        sizeof(float)*szefake2, fake2,             flag2,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgemm_f2_quark = PCORE_sgemm_f2_quark
#define CORE_sgemm_f2_quark PCORE_sgemm_f2_quark
#endif
void CORE_sgemm_f2_quark(Quark* quark)
{
    PLASMA_enum transA;
    PLASMA_enum transB;
    int M;
    int N;
    int K;
    float alpha;
    float *A;
    int LDA;
    float *B;
    int LDB;
    float beta;
    float *C;
    int LDC;
    void *fake1, *fake2;

    quark_unpack_args_15(quark, transA, transB, M, N, K, alpha,
                         A, LDA, B, LDB, beta, C, LDC, fake1, fake2);
    cblas_sgemm(
        CblasColMajor,
        (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB,
        M, N, K,
        (alpha), A, LDA,
        B, LDB,
        (beta), C, LDC);
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_sgemm_p2(Quark *quark, Quark_Task_Flags *task_flags,
                         PLASMA_enum transA, int transB,
                         int m, int n, int k, int nb,
                         float alpha, const float *A, int lda,
                         const float **B, int ldb,
                         float beta, float *C, int ldc)
{
    DAG_CORE_GEMM;
    QUARK_Insert_Task(quark, CORE_sgemm_p2_quark, task_flags,
        sizeof(PLASMA_enum),                &transA,    VALUE,
        sizeof(PLASMA_enum),                &transB,    VALUE,
        sizeof(int),                        &m,         VALUE,
        sizeof(int),                        &n,         VALUE,
        sizeof(int),                        &k,         VALUE,
        sizeof(float),         &alpha,     VALUE,
        sizeof(float)*lda*nb,   A,                 INPUT,
        sizeof(int),                        &lda,       VALUE,
        sizeof(float*),         B,                 INPUT,
        sizeof(int),                        &ldb,       VALUE,
        sizeof(float),         &beta,      VALUE,
        sizeof(float)*ldc*nb,    C,                 INOUT | LOCALITY,
        sizeof(int),                        &ldc,       VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgemm_p2_quark = PCORE_sgemm_p2_quark
#define CORE_sgemm_p2_quark PCORE_sgemm_p2_quark
#endif
void CORE_sgemm_p2_quark(Quark* quark)
{
    PLASMA_enum transA;
    PLASMA_enum transB;
    int M;
    int N;
    int K;
    float alpha;
    float *A;
    int LDA;
    float **B;
    int LDB;
    float beta;
    float *C;
    int LDC;

    quark_unpack_args_13(quark, transA, transB, M, N, K, alpha,
                         A, LDA, B, LDB, beta, C, LDC);
    cblas_sgemm(
        CblasColMajor,
        (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB,
        M, N, K,
        (alpha), A, LDA,
        *B, LDB,
        (beta), C, LDC);
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_sgemm_p3(Quark *quark, Quark_Task_Flags *task_flags,
                           PLASMA_enum transA, int transB,
                           int m, int n, int k, int nb,
                           float alpha, const float *A, int lda,
                           const float *B, int ldb,
                           float beta, float **C, int ldc)
{
    DAG_CORE_GEMM;
    QUARK_Insert_Task(quark, CORE_sgemm_p3_quark, task_flags,
        sizeof(PLASMA_enum),                &transA,    VALUE,
        sizeof(PLASMA_enum),                &transB,    VALUE,
        sizeof(int),                        &m,         VALUE,
        sizeof(int),                        &n,         VALUE,
        sizeof(int),                        &k,         VALUE,
        sizeof(float),         &alpha,     VALUE,
        sizeof(float)*lda*nb,   A,                 INPUT,
        sizeof(int),                        &lda,       VALUE,
        sizeof(float)*ldb*nb,   B,                 INPUT,
        sizeof(int),                        &ldb,       VALUE,
        sizeof(float),         &beta,      VALUE,
        sizeof(float*),         C,                 INOUT | LOCALITY,
        sizeof(int),                        &ldc,       VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgemm_p3_quark = PCORE_sgemm_p3_quark
#define CORE_sgemm_p3_quark PCORE_sgemm_p3_quark
#endif
void CORE_sgemm_p3_quark(Quark* quark)
{
    PLASMA_enum transA;
    PLASMA_enum transB;
    int M;
    int N;
    int K;
    float alpha;
    float *A;
    int LDA;
    float *B;
    int LDB;
    float beta;
    float **C;
    int LDC;

    quark_unpack_args_13(quark, transA, transB, M, N, K, alpha,
                         A, LDA, B, LDB, beta, C, LDC);
    cblas_sgemm(
        CblasColMajor,
        (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB,
        M, N, K,
        (alpha), A, LDA,
        B, LDB,
        (beta), *C, LDC);
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_sgemm_p2f1(Quark *quark, Quark_Task_Flags *task_flags,
                           PLASMA_enum transA, int transB,
                           int m, int n, int k, int nb,
                           float alpha, const float *A, int lda,
                           const float **B, int ldb,
                           float beta, float *C, int ldc,
                           float *fake1, int szefake1, int flag1)
{
    DAG_CORE_GEMM;
    QUARK_Insert_Task(quark, CORE_sgemm_p2f1_quark, task_flags,
        sizeof(PLASMA_enum),                &transA,    VALUE,
        sizeof(PLASMA_enum),                &transB,    VALUE,
        sizeof(int),                        &m,         VALUE,
        sizeof(int),                        &n,         VALUE,
        sizeof(int),                        &k,         VALUE,
        sizeof(float),         &alpha,     VALUE,
        sizeof(float)*lda*nb,   A,                 INPUT,
        sizeof(int),                        &lda,       VALUE,
        sizeof(float*),         B,                 INPUT,
        sizeof(int),                        &ldb,       VALUE,
        sizeof(float),         &beta,      VALUE,
        sizeof(float)*ldc*nb,    C,                 INOUT | LOCALITY,
        sizeof(int),                        &ldc,       VALUE,
        sizeof(float)*szefake1, fake1,             flag1,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_sgemm_p2f1_quark = PCORE_sgemm_p2f1_quark
#define CORE_sgemm_p2f1_quark PCORE_sgemm_p2f1_quark
#endif
void CORE_sgemm_p2f1_quark(Quark* quark)
{
    PLASMA_enum transA;
    PLASMA_enum transB;
    int M;
    int N;
    int K;
    float alpha;
    float *A;
    int LDA;
    float **B;
    int LDB;
    float beta;
    float *C;
    int LDC;
    void *fake1;

    quark_unpack_args_14(quark, transA, transB, M, N, K, alpha,
                         A, LDA, B, LDB, beta, C, LDC, fake1);
    cblas_sgemm(
        CblasColMajor,
        (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB,
        M, N, K,
        (alpha), A, LDA,
        *B, LDB,
        (beta), C, LDC);
}


/*
 * SpanDSP - a series of DSP components for telephony
 *
 * makecss.c - Create the composite source signal (CSS) for G.168 testing.
 *
 * Written by Steve Underwood <steveu@coppice.org>
 *
 * Copyright (C) 2003 Steve Underwood
 *
 * All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2, as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/*! \page makecss_page CSS construction for G.168 testing
\section makecss_page_sec_1 What does it do?
???.

\section makecss_page_sec_2 How does it work?
???.
*/

#if defined(HAVE_CONFIG_H)
#include "config.h"
#endif

#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <time.h>
#include <stdio.h>
#include <fcntl.h>
#include <sndfile.h>
#if defined(HAVE_FFTW3_H)
#include <fftw3.h>
#else
#include <fftw.h>
#endif

#include "spandsp.h"
#include "spandsp/g168models.h"

#if !defined(NULL)
#define NULL (void *) 0
#endif

#define FAST_SAMPLE_RATE    44100.0

#define C1_VOICED_SAMPLES   2144    /* 48.62ms at 44100 samples/second => 2144.142 */
#define C1_NOISE_SAMPLES    8820    /* 200ms at 44100 samples/second => 8820.0 */
#define C1_SILENCE_SAMPLES  4471    /* 101.38ms at 44100 samples/second => 4470.858 */

#define C3_VOICED_SAMPLES   3206    /* 72.69ms at 44100 samples/second => 3205.629 */
#define C3_NOISE_SAMPLES    8820    /* 200ms at 44100 samples/second => 8820.0 */
#define C3_SILENCE_SAMPLES  5614    /* 127.31ms at 44100 samples/second => 5614.371 */

static double scaling(double f, double start, double end, double start_gain, double end_gain)
{
    double scale;

    scale = start_gain + (f - start)*(end_gain - start_gain)/(end - start);
    return scale;
}
/*- End of function --------------------------------------------------------*/

static double peak(const int16_t amp[], int len)
{
    int16_t peak;
    int i;

    peak = 0;
    for (i = 0;  i < len;  i++)
    {
        if (abs(amp[i]) > peak)
            peak = abs(amp[i]);
    }
    return peak/32767.0;
}
/*- End of function --------------------------------------------------------*/

static double rms(const int16_t amp[], int len)
{
    double ms;
    int i;

    ms = 0.0;
    for (i = 0;  i < len;  i++)
        ms += amp[i]*amp[i];
    return sqrt(ms/len)/32767.0;
}
/*- End of function --------------------------------------------------------*/

static double rms_to_dbm0(double rms)
{
    return 20.0*log10(rms) + DBM0_MAX_POWER;
}
/*- End of function --------------------------------------------------------*/

static double rms_to_db(double rms)
{
    return 20.0*log10(rms);
}
/*- End of function --------------------------------------------------------*/

int main(int argc, char *argv[])
{
#if defined(HAVE_FFTW3_H)
    double in[8192][2];
    double out[8192][2];
#else
    fftw_complex in[8192];
    fftw_complex out[8192];
#endif
    fftw_plan p;
    int16_t voiced_sound[8192];
    int16_t noise_sound[8830];
    int16_t silence_sound[8192];
    int i;
    int voiced_length;
    int randy;
    double f;
    double pk;
    double ms;
    double scale;
    SNDFILE *filehandle;
    SF_INFO info;
    awgn_state_t *noise_source;

    memset(&info, 0, sizeof(info));
    info.frames = 0;
    info.samplerate = FAST_SAMPLE_RATE;
    info.channels = 1;
    info.format = SF_FORMAT_WAV | SF_FORMAT_PCM_16;
    info.sections = 1;
    info.seekable = 1;
    if ((filehandle = sf_open("sound_c1.wav", SFM_WRITE, &info)) == NULL)
    {
        fprintf(stderr, "    Failed to open result file\n");
        exit(2);
    }

    printf("Generate C1\n");
    /* The sequence is
            48.62ms  of voiced sound from table C.1 of G.168
            200.0ms  of pseudo-noise
            101.38ms of silence
       The above is then repeated phase inverted.

       The voice comes straight from C.1, repeated enough times to
       fill out the 48.62ms period - i.e. 16 copies of the sequence.

       The pseudo noise section is random numbers filtered by the spectral
       pattern in Figure C.2 */

    /* The set of C1 voice samples is ready for use in the output file. */
    voiced_length = sizeof(css_c1)/sizeof(css_c1[0]);
    for (i = 0;  i < voiced_length;  i++)
        voiced_sound[i] = css_c1[i];
    pk = peak(voiced_sound, voiced_length);
    ms = rms(voiced_sound, voiced_length);
    printf("Voiced level = %.2fdB, crest factor = %.2fdB\n", rms_to_dbm0(ms), rms_to_db(pk/ms));

#if defined(HAVE_FFTW3_H)
    p = fftw_plan_dft_1d(8192, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
#else
    p = fftw_create_plan(8192, FFTW_BACKWARD, FFTW_ESTIMATE);
#endif
    for (i = 0;  i < 8192;  i++)
    {
#if defined(HAVE_FFTW3_H)
        in[i][0] = 0.0;
        in[i][1] = 0.0;
#else
        in[i].re = 0.0;
        in[i].im = 0.0;
#endif
    }
    for (i = 1;  i <= 3715;  i++)
    {
        f = FAST_SAMPLE_RATE*i/8192.0;

#if 1
        if (f < 50.0)
            scale = -60.0;
        else if (f < 100.0)
            scale = scaling(f, 50.0, 100.0, -25.8, -12.8);
        else if (f < 200.0)
            scale = scaling(f, 100.0, 200.0, -12.8, 17.4);
        else if (f < 215.0)
            scale = scaling(f, 200.0, 215.0, 17.4, 17.8);
        else if (f < 500.0)
            scale = scaling(f, 215.0, 500.0, 17.8, 12.2);
        else if (f < 1000.0)
            scale = scaling(f, 500.0, 1000.0, 12.2, 7.2);
        else if (f < 2850.0)
            scale = scaling(f, 1000.0, 2850.0, 7.2, 0.0);
        else if (f < 3600.0)
            scale = scaling(f, 2850.0, 3600.0, 0.0, -2.0);
        else if (f < 3660.0)
            scale = scaling(f, 3600.0, 3660.0, -2.0, -20.0);
        else if (f < 3680.0)
            scale = scaling(f, 3600.0, 3680.0, -20.0, -30.0);
        else
            scale = -60.0;
#else
        scale = 0.0;
#endif
        randy = ((rand() >> 10) & 0x1)  ?  1.0  :  -1.0;
#if defined(HAVE_FFTW3_H)
        in[i][0] = randy*pow(10.0, scale/20.0)*35.0;
        in[8192 - i][0] = -in[i][0];
#else
        in[i].re = randy*pow(10.0, scale/20.0)*35.0;
        in[8192 - i].re = -in[i].re;
#endif
    }
#if defined(HAVE_FFTW3_H)
    fftw_execute(p);
#else
    fftw_one(p, in, out);
#endif
    for (i = 0;  i < 8192;  i++)
    {
#if defined(HAVE_FFTW3_H)
        noise_sound[i] = out[i][1];
#else
        noise_sound[i] = out[i].im;
#endif
    }
    pk = peak(noise_sound, 8192);
    ms = rms(noise_sound, 8192);
    printf("Filtered noise level = %.2fdB, crest factor = %.2fdB\n", rms_to_dbm0(ms), rms_to_db(pk/ms));

    for (i = 0;  i < 8192;  i++)
        silence_sound[i] = 0.0;

    for (i = 0;  i < 16;  i++)
        sf_writef_short(filehandle, voiced_sound, voiced_length);
    printf("%d samples of voice\n", 16*voiced_length);
    sf_writef_short(filehandle, noise_sound, 8192);
    sf_writef_short(filehandle, noise_sound, C1_NOISE_SAMPLES - 8192);
    printf("%d samples of noise\n", C1_NOISE_SAMPLES);
    sf_writef_short(filehandle, silence_sound, C1_SILENCE_SAMPLES);
    printf("%d samples of silence\n", C1_SILENCE_SAMPLES);

    /* Now phase invert the C1 set of samples. */
    voiced_length = sizeof(css_c1)/sizeof(css_c1[0]);
    for (i = 0;  i < voiced_length;  i++)
        voiced_sound[i] = -css_c1[i];
    for (i = 0;  i < 8192;  i++)
        noise_sound[i] = -noise_sound[i];

    for (i = 0;  i < 16;  i++)
        sf_writef_short(filehandle, voiced_sound, voiced_length);
    printf("%d samples of voice\n", 16*voiced_length);
    sf_writef_short(filehandle, noise_sound, 8192);
    sf_writef_short(filehandle, noise_sound, C1_NOISE_SAMPLES - 8192);
    printf("%d samples of noise\n", C1_NOISE_SAMPLES);
    sf_writef_short(filehandle, silence_sound, C1_SILENCE_SAMPLES);
    printf("%d samples of silence\n", C1_SILENCE_SAMPLES);

    if (sf_close(filehandle))
    {
        fprintf(stderr, "    Cannot close speech file '%s'\n", "sound_c1.wav");
        exit(2);
    }

    memset(&info, 0, sizeof(info));
    info.frames = 0;
    info.samplerate = FAST_SAMPLE_RATE;
    info.channels = 1;
    info.format = SF_FORMAT_WAV | SF_FORMAT_PCM_16;
    info.sections = 1;
    info.seekable = 1;
    if ((filehandle = sf_open("sound_c3.wav", SFM_WRITE, &info)) == NULL)
    {
        fprintf(stderr, "    Failed to open result file\n");
        exit(2);
    }

    printf("Generate C3\n");
    /* The sequence is
            72.69ms  of voiced sound from table C.3 of G.168
            200.0ms  of pseudo-noise
            127.31ms of silence
       The above is then repeated phase inverted.

       The voice comes straight from C.3, repeated enough times to
       fill out the 72.69ms period - i.e. 14 copies of the sequence.

       The pseudo noise section is AWGN filtered by the spectral
       pattern in Figure C.2. Since AWGN has the quality of being its
       own Fourier transform, we can use an approach like the one above
       for the C1 signal, using AWGN samples instead of randomly alternating
       ones and zeros. */

    /* Take the supplied set of C3 voice samples. */
    voiced_length = (sizeof(css_c3)/sizeof(css_c3[0]));
    for (i = 0;  i < voiced_length;  i++)
        voiced_sound[i] = css_c3[i];
    pk = peak(voiced_sound, voiced_length);
    ms = rms(voiced_sound, voiced_length);
    printf("Voiced level = %.2fdB, crest factor = %.2fdB\n", rms_to_dbm0(ms), rms_to_db(pk/ms));

    noise_source = awgn_init_dbm0(NULL, 7162534, rms_to_dbm0(ms));
    for (i = 0;  i < 8192;  i++)
        noise_sound[i] = awgn(noise_source);
    pk = peak(noise_sound, 8192);
    ms = rms(noise_sound, 8192);
    printf("Unfiltered noise level = %.2fdB, crest factor = %.2fdB\n", rms_to_dbm0(ms), rms_to_db(pk/ms));

    /* Now filter them */
#if defined(HAVE_FFTW3_H)
    p = fftw_plan_dft_1d(8192, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
#else
    p = fftw_create_plan(8192, FFTW_BACKWARD, FFTW_ESTIMATE);
#endif
    for (i = 0;  i < 8192;  i++)
    {
#if defined(HAVE_FFTW3_H)
        in[i][0] = 0.0;
        in[i][1] = 0.0;
#else
        in[i].re = 0.0;
        in[i].im = 0.0;
#endif
    }
    for (i = 1;  i <= 3715;  i++)
    {
        f = FAST_SAMPLE_RATE*i/8192.0;

#if 1
        if (f < 50.0)
            scale = -60.0;
        else if (f < 100.0)
            scale = scaling(f, 50.0, 100.0, -25.8, -12.8);
        else if (f < 200.0)
            scale = scaling(f, 100.0, 200.0, -12.8, 17.4);
        else if (f < 215.0)
            scale = scaling(f, 200.0, 215.0, 17.4, 17.8);
        else if (f < 500.0)
            scale = scaling(f, 215.0, 500.0, 17.8, 12.2);
        else if (f < 1000.0)
            scale = scaling(f, 500.0, 1000.0, 12.2, 7.2);
        else if (f < 2850.0)
            scale = scaling(f, 1000.0, 2850.0, 7.2, 0.0);
        else if (f < 3600.0)
            scale = scaling(f, 2850.0, 3600.0, 0.0, -2.0);
        else if (f < 3660.0)
            scale = scaling(f, 3600.0, 3660.0, -2.0, -20.0);
        else if (f < 3680.0)
            scale = scaling(f, 3600.0, 3680.0, -20.0, -30.0);
        else
            scale = -60.0;
#else
        scale = 0.0;
#endif
#if defined(HAVE_FFTW3_H)
        in[i][0] = noise_sound[i]*pow(10.0, scale/20.0)*0.0106;
        in[8192 - i][0] = -in[i][0];
#else
        in[i].re = noise_sound[i]*pow(10.0, scale/20.0)*0.0106;
        in[8192 - i].re = -in[i].re;
#endif
    }
#if defined(HAVE_FFTW3_H)
    fftw_execute(p);
#else
    fftw_one(p, in, out);
#endif
    for (i = 0;  i < 8192;  i++)
    {
#if defined(HAVE_FFTW3_H)
        noise_sound[i] = out[i][1];
#else
        noise_sound[i] = out[i].im;
#endif
    }
    pk = peak(noise_sound, 8192);
    ms = rms(noise_sound, 8192);
    printf("Filtered noise level = %.2fdB, crest factor = %.2fdB\n", rms_to_dbm0(ms), rms_to_db(pk/ms));

    for (i = 0;  i < 14;  i++)
        sf_writef_short(filehandle, voiced_sound, voiced_length);
    printf("%d samples of voice\n", 14*voiced_length);
    sf_writef_short(filehandle, noise_sound, 8192);
    sf_writef_short(filehandle, noise_sound, C3_NOISE_SAMPLES - 8192);
    printf("%d samples of noise\n", C3_NOISE_SAMPLES);
    sf_writef_short(filehandle, silence_sound, C3_SILENCE_SAMPLES);
    printf("%d samples of silence\n", C3_SILENCE_SAMPLES);

    /* Now phase invert the C3 set of samples. */
    voiced_length = (sizeof(css_c3)/sizeof(css_c3[0]));
    for (i = 0;  i < voiced_length;  i++)
        voiced_sound[i] = -css_c3[i];
    for (i = 0;  i < 8192;  i++)
        noise_sound[i] = -noise_sound[i];

    for (i = 0;  i < 14;  i++)
        sf_writef_short(filehandle, voiced_sound, voiced_length);
    printf("%d samples of voice\n", 14*voiced_length);
    sf_writef_short(filehandle, noise_sound, 8192);
    sf_writef_short(filehandle, noise_sound, C3_NOISE_SAMPLES - 8192);
    printf("%d samples of noise\n", C3_NOISE_SAMPLES);
    sf_writef_short(filehandle, silence_sound, C3_SILENCE_SAMPLES);
    printf("%d samples of silence\n", C3_SILENCE_SAMPLES);

    if (sf_close(filehandle))
    {
        fprintf(stderr, "    Cannot close speech file '%s'\n", "sound_c3.wav");
        exit(2);
    }

    fftw_destroy_plan(p);
    return 0;
}
/*- End of function --------------------------------------------------------*/
/*- End of file ------------------------------------------------------------*/


/* this manages and calls function to execute function
* from beginning to end
*/
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_statistics.h>

#include "fileio.h"
#include "data_process.h"
#include "sensor_history.h"
#include "sensor_validation.h"

#include "externs.h"

void handle_files(char *, double*, double*);
void sensor_validation(double*);
double data_processing(double*);

int sensor_number;
int temperature_min = 30;
int temperature_max = 60;
int data_number;
int group_number;
float q_percent;

/* path and names of the files used in the program
* which include input file, output file and sensor history file */
char input_file_name[255] = "../data/input_data/sample_input.csv";
char history_file_name[255] = "../data/sensor_history/sensor_history.csv";
char output_file_name[255] = "../data/output_data/output.csv";

int main() {

    printf("\nApplication is running....\n");

    /* user input for the number of sensors to process */
    printf("Insert number of sensors:");
    scanf("%d", &sensor_number);

    /* determining the time intervals for the sensors */
    int time_interval;

    printf("Insert collected time intervals:");
    scanf("%d", &time_interval);

    printf("Insert q percent(Should be 0~1):");
    scanf("%f", &q_percent);

    if ((q_percent<0) || (q_percent>1)) {
        printf("Invalid q_percent! It will be regarded as 0.7 of default value.\n");
        q_percent = 0.7;
    }

    double values[256];
    double time_value[256];
    double group_values[sensor_number];

    /* function performs reading of sensor data
    * @input : file name
    * @output : sensor time and associated values
    */
    handle_files(input_file_name, &time_value[0], &values[0]);

    /* number of calculations to be performed based on the user input */
    group_number = data_number / sensor_number;

    for (int i = 0; i < group_number; i++) {
        time_value[i] = time_value[i * sensor_number];
    }

    /* check whether the user input and actual file data matches */
    if (group_number != time_interval) {
        printf("ERROR: Time interval numbers, sensor numbers do not match with your input file!\n");
        exit(0);
    }

    sensor_validation(&values[0]);

    /* seperating multiple data computations in the output file*/
    FILE *fpout = fopen(output_file_name, "a");
    fprintf(fpout, "--------------\n");
    fclose(fpout);

    FILE *fphis = fopen(history_file_name, "a");
    fprintf(fphis, "--------------\n");
    fclose(fphis);

    for (int i = 0 ; i < group_number; i++) {
        for (int j = 0; j < sensor_number; j++) {
            group_values[j]  = values[i*sensor_number + j];
        }
        printf("\n---------------------Time interval %d---------------------\n\n", i);

        double fused = data_processing(&group_values[0]);
        float time_val_file = (float)time_value[i];

        write_data(time_val_file, fused, output_file_name);
    }

    return 1;
}

/* function checks whether an attempt made to open a file is succuessful or not */
void handle_files(char* input_file_name, double* time_value, double* values) {
    printf("\n");
    if (input_file_name != NULL) {
        data_number = read_data(&time_value[0], &values[0], input_file_name);

        if ( data_number == -1 ) {
            printf("ERROR: File open failed!\n");
            exit(0);
        } else if ( data_number == 0 ) {
            printf("ERROR: Sensor numbers inserted and file are different!\n");
            exit(0);
        } else if ( data_number > 0) {
            printf("Read data successfully!\n");
        }
    } else {
        printf("ERROR: Input file does not exist!\n");
        exit(0);
    }
}

/* function performs sensor validation for group values
* @input : group Values
*/
void sensor_validation(double* group_values) {

    int res = reading_validation(&group_values[0]);
    if (res == 1) {
        frozen_value_check(&group_values[0]);

    }

    if (res == 0) {
        printf("ERROR: Temperature values are out of range! Check history file.\n");
        frozen_value_check(&group_values[0]);
    }

}

/* function performs sensor fusion algo using group Values
* @input : group Values
* @output : fused result
*/
double data_processing(double* group_values) {

    printf("Sensor Values:\n");

    for (int i = 0; i < sensor_number; i++) {
        printf("x%d=%f, ", i, group_values[i]);
    }

    /* Step 1: Calc the Support Degree Matrix */
    gsl_matrix* D = gsl_matrix_alloc(sensor_number,sensor_number);
    support_degree_generator(D, &group_values[0]);    //function in data_process.c to get D - Support Degree Matrix

    /* Step 2: Calc eigenval & eigenvec */
    gsl_matrix* T = gsl_matrix_alloc(sensor_number,sensor_number);
    gsl_vector* evec = gsl_vector_alloc(sensor_number);
    gsl_matrix* Temp = gsl_matrix_alloc(sensor_number,sensor_number);

    gsl_matrix_memcpy(Temp, D);
    eigenvec_calc(evec, T, Temp);    //function in data_process.c to get evec - eigen group_values & T - vectors

    /* Step 3: Principal Comp Calc */
    gsl_matrix* y = gsl_matrix_alloc(sensor_number,sensor_number);
    principal_comp_calc(T, D, y);    //function in data_process.c to get T - Principal Components

    /* Step 4: Calc the contri rate of the kth principal comp */
    double alpha[sensor_number];
    contri_rate_calc_kth(evec, &alpha[0]);    //function in data_process.c to get alpha

    /* Step 5: Calc the contri rate of the m principal comp */
    double phi[sensor_number];
    major_contri_calc(&alpha[0], &phi[0]);    //function in data_process.c to get phi

    /* Step 6: Compute the integrated support degree score */
    gsl_vector* Z = gsl_vector_alloc(sensor_number);
    integ_supp_score_calc(&alpha[0], y, Z);    //function in data_process.c to get z_i

    /* Step 7-1: Eliminate incorrect data */
    int sensor_correction[sensor_number];
    elliminate_incorrect_data(Z, &sensor_correction[0]);    //function in data_process.c to elliminate incorrect datas

    /* Step 7-2: Compute the weight coefficient for each sensor */
    double omega[sensor_number];
    weight_coeff_calc(Z, &sensor_correction[0], &omega[0]);     //function in data_process.c to get omega

    /* Step 7-3: Compute the fused output */
    double fused;
    fused = fused_output(&omega[0], &group_values[0]);    //function in data_process.c to get fused output
    printf("FINAL STEP: \nThe fused output is %f\n", fused);

    /* Free memory */
    gsl_matrix_free(D);
    gsl_matrix_free(Temp);
    gsl_matrix_free(T);
    gsl_vector_free(evec);
    gsl_matrix_free(y);
    gsl_vector_free(Z);

    return fused;

}


#include "jobqueue.h"
#include "ance_degnome.h"
#include "fitfunc.h"
#include "flagparse.c"
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <time.h>
#include <pthread.h>
#include <unistd.h>
#include <limits.h>

typedef struct JobData JobData;
struct JobData {
	Degnome* child;
	Degnome* p1;
	Degnome* p2;
};

void usage(void);
void help_menu(void);
int jobfunc(void* p, void* tdat);
void calculate_diversity(Degnome* generation, double** percent_decent, double* diversity);

const char* usageMsg =
	"Usage: devosim [-bhrv] [-s | -u] [-c chromosome_length]\n"
	"\t\t  [-e mutation_effect] [-g num_generations]\n"
	"\t\t  [-m mutation_rate] [-o crossover_rate]\n"
	"\t\t  [-p population_size] [-t num_threads]\n"
	"\t\t  [--seed rngseed] [--target hat_height target]\n"
	"\t\t  [--sqrt | --linear | --close | --ceiling | --log]\n";

const char* helpMsg =
	"OPTIONS\n"
	"\t -b\t Simulation will stop when all degnomes are identical.\n\n"
	"\t -c chromosome_length\n"
	"\t\t Set chromosome length for the current simulation.\n"
	"\t\t Default chromosome length is 10.\n\n"
	"\t -e mutation_effect\n"
	"\t\t Set how much a mutation will effect a gene on average.\n"
	"\t\t Default mutation effect is 2.\n\n"
	"\t -g num_generations\n"
	"\t\t Set how many generations this simulation will run for.\n"
	"\t\t Default number of generations is 1000.\n\n"
	"\t -h\t Display this help menu.\n\n"
	"\t -m mutation_rate\n"
	"\t\t Set the mutation rate for the current simulation.\n"
	"\t\t Default mutation rate is 1.\n\n"
	"\t -o crossover_rate\n"
	"\t\t Set the crossover rate for the current simulation.\n"
	"\t\t Default crossover rate is 2.\n\n"
	"\t -p population_size\n"
	"\t\t Set the population size for the current simulation.\n"
	"\t\t Default population size is 10.\n\n"
	"\t -r\t Only show percentages of descent from the original genomes.\n\n"
	"\t -s\t Degnome selection will occur.\n\n"
	"\t -u\t All degnomes contribute to two offspring.\n\n"
	"\t -v\t Output will be given for every generation.\n\n"
	"\t -t num_threads\n"
	"\t\t Select the number of threads to be used in the current run.\n"
	"\t\t Default is 0 (which will result in 3/4 of cores being used).\n"
	"\t\t Must be 1 if a seed is used in order to prevent race conditions.\n\n"
	"\t --seed rngseed\n"
	"\t\t Select the seed used by the RNG in the current run.\n"
	"\t\t Default seed is 0 (which will result in a random seed).\n\n"
	"\t --target hat_height target\n"
	"\t\t Sets the ideal hat height for the current simulation\n"
	"\t\t Used for fitness functions that have an \"ideal\" value.\n\n"
	"\t --sqrt\t\t fitness will be sqrt(hat_height)\n\n"
	"\t --linear\t fitness will be hat_height\n\n"
	"\t --close\t fitness will be (target - abs(target - hat_height))\n\n"
	"\t --ceiling\t fitness will quickly level off after passing target\n\n";

pthread_mutex_t seedLock = PTHREAD_MUTEX_INITIALIZER;
unsigned long rngseed=0;

//	there is no need for the line 'int chrom_size' as it is declared as a global variable in degnome.h
int pop_size;
int num_gens;
int mutation_rate;
int mutation_effect;
int crossover_rate;
int selective;
int uniform;
int verbose;
int reduced;
int break_at_zero_diversity;

void usage(void) {
	fputs(usageMsg, stderr);
	exit(EXIT_FAILURE);
}

void help_menu(void) {
	fputs(helpMsg, stderr);
	exit(EXIT_FAILURE);
}

int num_threads = 0;
JobQueue* jq;

void *ThreadState_new(void *notused);
void ThreadState_free(void *rng);

void *ThreadState_new(void *notused) {
	// Lock seed, initialize random number generator, increment seed,
	// and unlock.
	gsl_rng *rng = gsl_rng_alloc(gsl_rng_taus);

	pthread_mutex_lock(&seedLock);
	gsl_rng_set(rng, rngseed);
	rngseed = (rngseed == ULONG_MAX ? 0 : rngseed + 1);
	pthread_mutex_unlock(&seedLock);

	return rng;
}

void ThreadState_free(void *rng) {
	gsl_rng_free((gsl_rng *) rng);
}

int jobfunc(void* p, void* tdat) {
	gsl_rng* rng = (gsl_rng*) tdat;
	JobData* data = (JobData*) p;																				//get data out
	Degnome_mate(data->child, data->p1, data->p2, rng, mutation_rate, mutation_effect, crossover_rate);			//mate

	return 0;		//exited without error
}

void calculate_diversity(Degnome* generation, double** percent_decent, double* diversity) {
	*diversity = 0;
	for (int i = 0; i < pop_size; i++) {			//calculate percent decent for each degnome
		for (int j = 0; j < pop_size; j++) {
			percent_decent[i][j] = 0;
			for (int k = 0; k < chrom_size; k++) {
				if (generation[i].GOI_array[k] == j) {
					percent_decent[i][j]++;
				}
			}
			percent_decent[i][j] /= chrom_size;
		}
	}
	for (int j = 0; j < pop_size; j++) {			//sum and average
		percent_decent[pop_size][j] = 0;
		for (int k = 0; k < pop_size; k++) {
			percent_decent[pop_size][j] += percent_decent[k][j];
		}
		percent_decent[pop_size][j] /= pop_size;
	}

	for (int i = 0; i < pop_size; i++) {			//calculate percent diversity for the entire generation
		for (int j = 0; j < pop_size; j++) {
			if (i == j) {
				continue;
			}
			for (int k = 0; k < chrom_size; k++) {
				if (generation[i].GOI_array[k] != generation[j].GOI_array[k]) {
					(*diversity)++;
				}
			}
		}
	}
	*diversity /= ((pop_size-1) * pop_size * chrom_size);
}

int main(int argc, char **argv) {

	int * flags = NULL;

	if (parse_flags(argc, argv, 3, &flags) == -1) {
		free(flags);
		usage();
	}

	if (flags[2] == 1) {
		free(flags);
		help_menu();
	}

	break_at_zero_diversity = flags[1];
	reduced = flags[3];
	verbose = flags[4];
	if (flags[5] == 1) {
		selective = 1;
	}
	else if (flags[5] == 2) {
		uniform = 1;
	}
	chrom_size = flags[6];
	mutation_effect = flags[7];
	num_gens = flags[8];
	mutation_rate = flags[9];
	crossover_rate = flags[10];
	pop_size = flags[11];

	num_threads = flags[12];

	if(flags[13] == 0){
		set_function("linear");
	}
	else if(flags[13] == 1){
		set_function("sqrt");
	}
	else if(flags[13] == 2){
		set_function("close");
	}
	else if(flags[13] == 3){
		set_function("ceiling");
	}
	else if(flags[13] == 4){
		set_function("log");
	}

	target_num = flags[14];

	if(flags[15] <= 0) {
		time_t currtime = time(NULL);                  // time
		unsigned long pid = (unsigned long) getpid();  // process id
		rngseed = currtime ^ pid;                      // random seed
	}
	else{
		rngseed = flags[15];
	}
	gsl_rng* rng = gsl_rng_alloc(gsl_rng_taus);    // rand generator
	gsl_rng_set(rng, rngseed);

	free(flags);

	if (num_threads <= 0) {
		if (num_threads < 0) {
			#ifdef DEBUG_MODE
				fprintf(stderr, "Error invalid number of threads: %u\n", num_threads);
			#endif
		}
		num_threads = (3*getNumCores()/4);
	}
	#ifdef DEBUG_MODE
		fprintf(stderr, "Final number of threads: %u\n", num_threads);
	#endif

	Degnome* parents;
	Degnome* children;
	Degnome* temp;

	printf("%u, %u, %u\n", chrom_size, pop_size, num_gens);

	parents = malloc(pop_size*sizeof(Degnome));
	children = malloc(pop_size*sizeof(Degnome));

	for (int i = 0; i < pop_size; i++) {
		parents[i].dna_array = malloc(chrom_size*sizeof(double));
		parents[i].GOI_array = malloc(chrom_size*sizeof(int));

		children[i].dna_array = malloc(chrom_size*sizeof(double));
		children[i].GOI_array = malloc(chrom_size*sizeof(int));

		parents[i].hat_size = 0;

		for (int j = 0; j < chrom_size; j++) {
			parents[i].dna_array[j] = 10;	//children aren't initialized
			parents[i].hat_size += 10;
			parents[i].GOI_array[j] = (i);	//track ancestries
		}
	}

	double* diversity;
	double** percent_decent;

	diversity = malloc(sizeof(double));
	*diversity = 1;
	percent_decent = malloc((pop_size+1)*sizeof(double*));
	for (int i = 0; i < pop_size+1; i++) {
		percent_decent[i] = malloc(pop_size*sizeof(double));
		if (i == pop_size) {
			continue;
		}
		for (int j = 0; j < pop_size; j++) {
			if (i == j) {
				percent_decent[i][j] = 1;
			}
			else {
				percent_decent[i][j] = 0;
			}
		}
	}

	if (!reduced && !verbose) {
		printf("\nGeneration 0:\n\n");
		for (int i = 0; i < pop_size; i++) {
			printf("Degnome %u allele values:\n", i);
			if (!reduced) {
				for (int j = 0; j < chrom_size; j++) {
					printf("%lf\t", parents[i].dna_array[j]);
				}
				printf("\n");
			}
			else {
				printf("%lf\n", parents[i].dna_array[0]);
			}

			printf("Degnome %u ancestries:\n", i);
			if (!reduced) {
				for (int j = 0; j < chrom_size; j++) {
					printf("%u\t", parents[i].GOI_array[j]);
				}
				printf("\n");
			}
			else {
				printf("%u\n", parents[i].GOI_array[0]);
			}
		}
	}
	printf("\n\n");

	int final_gen;
	int broke_early = 0;

	jq = JobQueue_new(num_threads, NULL, ThreadState_new, ThreadState_free);

	JobData* dat = malloc(pop_size*sizeof(JobData));

	for (int i = 0; i < num_gens; i++) {
		if (break_at_zero_diversity) {
			calculate_diversity(parents, percent_decent, diversity);
			if ((*diversity) <= 0) {
				final_gen = i;
				broke_early = 1;
				break;
			}
		}
		if (!uniform) {
			double fit;
			if (selective) {
				fit = get_fitness(parents[0].hat_size);
			}
			else {
				fit = 100;			//in runs withoutslection, everybody is equally fit
			}

			double total_hat_size = fit;
			double cum_hat_size[pop_size];
			cum_hat_size[0] = fit;

			for (int j = 1; j < pop_size; j++) {
				if (selective) {
					fit = get_fitness(parents[j].hat_size);
				}
				else {
					fit = 100;
				}

				total_hat_size += fit;
				cum_hat_size[j] = (cum_hat_size[j-1] + fit);
			}

			for (int j = 0; j < pop_size; j++) {

				pthread_mutex_lock(&seedLock);
				gsl_rng_set(rng, rngseed);
				rngseed = (rngseed == ULONG_MAX ? 0 : rngseed + 1);
				pthread_mutex_unlock(&seedLock);

				int m, d;

				double win_m = gsl_rng_uniform(rng);
				win_m *= total_hat_size;
				double win_d = gsl_rng_uniform(rng);
				win_d *= total_hat_size;

				// printf("win_m:%lf, wind:%lf, max: %lf\n", win_m,win_d,total_hat_size);

				for (m = 0; cum_hat_size[m] < win_m; m++) {
					continue;
				}

				for (d = 0; cum_hat_size[d] < win_d; d++) {
					continue;
				}

				// printf("m:%u, d:%u\n", m,d);
				dat[j].child = (children + j);
				dat[j].p1 = (parents + m);
				dat[j].p2 = (parents + d);

				JobQueue_addJob(jq, jobfunc, dat + j);			}
		}
		else {
			// printf("uniform!!!\n");

			int moms[pop_size];
			int dads[pop_size];
			int mom_max = pop_size;
			int dad_max = pop_size;

			int m, d;

			for (int j = 0; j < pop_size; j++) {
				moms[j] = j;
				dads[j] = j;
			}

			for (int j = 0; j < pop_size; j++) {
				pthread_mutex_lock(&seedLock);
				gsl_rng_set(rng, rngseed);
				rngseed = (rngseed == ULONG_MAX ? 0 : rngseed + 1);

				pthread_mutex_unlock(&seedLock);

				int index_m = (int) gsl_rng_uniform_int (rng, mom_max);
				int index_d = (int) gsl_rng_uniform_int (rng, dad_max);

				m = moms[index_m];
				d = dads[index_d];

				// printf("m:\t%u\nd:\t%u\n", m, d);

				//reduce the pool of available degnomes
				//in order to make sure everybody get's two chances to mate
				//one as a dad and one as a mom

				int temp_m = moms[index_m];
				int temp_d = dads[index_d];
				moms[index_m] = moms[mom_max-1];
				dads[index_d] = dads[dad_max-1];
				moms[mom_max-1] = temp_m;
				dads[dad_max-1] = temp_d;

				mom_max--;
				dad_max--;

				dat[j].child = (children + j);
				dat[j].p1 = (parents + m);
				dat[j].p2 = (parents + d);

				JobQueue_addJob(jq, jobfunc, dat + j);
			}
		}

		JobQueue_waitOnJobs(jq);

		temp = children;
		children = parents;
		parents = temp;
		if (verbose) {
			calculate_diversity(parents, percent_decent, diversity);
			printf("\nGeneration %u:\n", i);
			if (!reduced) {
				for (int k = 0; k < pop_size; k++) {
					printf("\n\nDegnome %u allele values:\n", k);

					for (int j = 0; j < chrom_size; j++) {
						printf("%lf\t", parents[k].dna_array[j]);
					}
					if (selective) {
						printf("\nTOTAL HAT SIZE: %lg\n\n", parents[k].hat_size);
					}
					else {
						printf("\n");
					}

					printf("\n\nDegnome %u ancestries:\n", k);
					for (int j = 0; j < chrom_size; j++) {
						printf("%u\t", parents[k].GOI_array[j]);
					}
					printf("\n");

					for (int j = 0; j < pop_size; j++) {
						if (percent_decent[k][j] > 0) {
							printf("%lf%% Degnome %u\t", (100*percent_decent[k][j]), j);
						}
					}
				}
			}
			printf("\nAverage population descent percentages:\n");
			for (int j = 0; j < pop_size; j++) {
				if (percent_decent[pop_size][j] > 0) {
					printf("%lf%% Degnome %u\t", (100*percent_decent[pop_size][j]), j);
				}
			}
			printf("\nPercent diversity: %lf\n", (100* (*diversity)));
		printf("\n\n");
		}
	}

	JobQueue_noMoreJobs(jq);

	if (verbose) {
		printf("\n");
	}

	calculate_diversity(parents, percent_decent, diversity);
	// printf("\n\n DIVERSITY%lf\n\n\n", *diversity);
	if (broke_early) {
		printf("Generation %u:\n", final_gen);
	}
	else {
		printf("Generation %u:\n", num_gens);
	}
	if (!reduced) {
		for (int i = 0; i < pop_size; i++) {
			printf("\n\nDegnome %u allele values:\n", i);
			if (!reduced) {
				for (int j = 0; j < chrom_size; j++) {
					printf("%lf\t", parents[i].dna_array[j]);
				}
			}

			printf("\n\nDegnome %u ancestries:\n", i);
			if (!reduced) {
				for (int j = 0; j < chrom_size; j++) {
					printf("%u\t", parents[i].GOI_array[j]);
				}
				printf("\n");
			}

			for (int j = 0; j < pop_size; j++) {
				if (percent_decent[i][j] > 0) {
					printf("%lf%% Degnome %u\t", (100*percent_decent[i][j]), j);
				}
			}
			printf("\n");

			if (selective) {
				printf("\nTOTAL HAT SIZE: %lg\n\n", parents[i].hat_size);
			}
			else {
				printf("\n\n");
			}
		}
	}
	printf("Average population decent percentages:\n");
	for (int j = 0; j < pop_size; j++) {
		if (percent_decent[pop_size][j] > 0) {
			printf("%lf%% Degnome %u\t", (100*percent_decent[pop_size][j]), j);
		}
	}
	printf("\nPercent diversity: %lf\n", (100* (*diversity)));
	printf("\n\n\n");

	//free everything
	JobQueue_free(jq);
	free(dat);

	for (int i = 0; i < pop_size; i++) {
		free(parents[i].dna_array);
		free(children[i].dna_array);
		free(parents[i].GOI_array);
		free(children[i].GOI_array);
		parents[i].hat_size = 0;

		free(percent_decent[i]);
	}
	free(percent_decent[pop_size]);

	free(parents);
	free(children);

	free(percent_decent);
	free(diversity);

	gsl_rng_free (rng);
}


/***************************************************************************/
/*                                                                         */
/* matrix.c - Matrix class for mruby                                       */
/* Copyright (C) 2015 Paolo Bosetti                                        */
/* paolo[dot]bosetti[at]unitn.it                                           */
/* Department of Industrial Engineering, University of Trento              */
/*                                                                         */
/* This library is free software.  You can redistribute it and/or          */
/* modify it under the terms of the GNU GENERAL PUBLIC LICENSE 2.0.        */
/*                                                                         */
/* This library is distributed in the hope that it will be useful,         */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of          */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           */
/* Artistic License 2.0 for more details.                                  */
/*                                                                         */
/* See the file LICENSE                                                    */
/*                                                                         */
/***************************************************************************/

#include <gsl/gsl_blas.h>
#include <gsl/gsl_rng.h>
#include "matrix.h"
#include "vector.h"

#pragma mark -
#pragma mark • Utilities

// Garbage collector handler, for play_data struct
// if play_data contains other dynamic data, free it too!
// Check it with GC.start
void matrix_destructor(mrb_state *mrb, void *p_) {
  gsl_matrix *v = (gsl_matrix *)p_;
  gsl_matrix_free(v);
};

// Creating data type and reference for GC, in a const struct
const struct mrb_data_type matrix_data_type = {"matrix_data",
                                               matrix_destructor};

// Utility function for getting the struct out of the wrapping IV @data
void mrb_matrix_get_data(mrb_state *mrb, mrb_value self, gsl_matrix **data) {
  mrb_value data_value;
  data_value = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@data"));

  // Loading data from data_value into p_data:
  Data_Get_Struct(mrb, data_value, &matrix_data_type, *data);
  if (!*data)
    mrb_raise(mrb, E_RUNTIME_ERROR, "Could not access @data");
}

#pragma mark -
#pragma mark • Initializations and setup

// Data Initializer C function (not exposed!)
static void mrb_matrix_init(mrb_state *mrb, mrb_value self, mrb_int n,
                            mrb_int m) {
  mrb_value data_value; // this IV holds the data
  gsl_matrix *p_data;   // pointer to the C struct

  data_value = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@data"));

  // if @data already exists, free its content:
  if (!mrb_nil_p(data_value)) {
    Data_Get_Struct(mrb, data_value, &matrix_data_type, p_data);
    free(p_data);
  }
  // Allocate and zero-out the data struct:
  p_data = gsl_matrix_calloc(n, m);
  if (!p_data)
    mrb_raise(mrb, E_RUNTIME_ERROR, "Could not allocate @data");

  // Wrap struct into @data:
  mrb_iv_set(
      mrb, self, mrb_intern_lit(mrb, "@data"), // set @data
      mrb_obj_value(                           // with value hold in struct
          Data_Wrap_Struct(mrb, mrb->object_class, &matrix_data_type, p_data)));
}

static mrb_value mrb_matrix_initialize(mrb_state *mrb, mrb_value self) {
  mrb_int n, m;
  mrb_get_args(mrb, "ii", &n, &m);

  // Call strcut initializer:
  mrb_matrix_init(mrb, self, n, m);
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@nrows"), mrb_fixnum_value(n));
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@ncols"), mrb_fixnum_value(m));
  mrb_iv_set(mrb, self, mrb_intern_lit(mrb, "@format"),
             mrb_str_new_cstr(mrb, "%10.3f"));
  return mrb_nil_value();
}

static mrb_value mrb_matrix_dup(mrb_state *mrb, mrb_value self) {
  mrb_value other;
  gsl_matrix *p_mat = NULL, *p_mat_other = NULL;
  mrb_value args[2];

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  args[0] = mrb_fixnum_value(p_mat->size1);
  args[1] = mrb_fixnum_value(p_mat->size2);
  other = mrb_obj_new(mrb, mrb_class_get(mrb, "Matrix"), 2, args);
  mrb_matrix_get_data(mrb, other, &p_mat_other);
  gsl_matrix_memcpy(p_mat_other, p_mat);
  return other;
}

static mrb_value mrb_matrix_rnd_fill(mrb_state *mrb, mrb_value self) {
  gsl_matrix *p_mat = NULL;
  const gsl_rng_type *T;
  gsl_rng *r;
  mrb_int h, k;

  mrb_matrix_get_data(mrb, self, &p_mat);

  gsl_rng_env_setup();
  T = gsl_rng_default;
  r = gsl_rng_alloc(T);

  for (k = 0; k < p_mat->size2; k++) {
    for (h = 0; h < p_mat->size1; h++) {
      gsl_matrix_set(p_mat, h, k, gsl_rng_uniform(r));
    }
  }
  return self;
}

static mrb_value mrb_matrix_all(mrb_state *mrb, mrb_value self) {
  mrb_float v;
  gsl_matrix *p_mat = NULL;

  mrb_get_args(mrb, "f", &v);
  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  gsl_matrix_set_all(p_mat, v);
  return self;
}

static mrb_value mrb_matrix_zero(mrb_state *mrb, mrb_value self) {
  gsl_matrix *p_mat = NULL;

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  gsl_matrix_set_zero(p_mat);
  return self;
}

static mrb_value mrb_matrix_identity(mrb_state *mrb, mrb_value self) {
  gsl_matrix *p_mat = NULL;

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  gsl_matrix_set_identity(p_mat);
  return self;
}

#pragma mark -
#pragma mark • Tests

static mrb_value mrb_matrix_equal(mrb_state *mrb, mrb_value self) {
  mrb_value other;
  gsl_matrix *p_mat, *p_mat_other;
  mrb_get_args(mrb, "o", &other);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  mrb_matrix_get_data(mrb, other, &p_mat_other);
  if (1 == gsl_matrix_equal(p_mat, p_mat_other))
    return mrb_true_value();
  else
    return mrb_false_value();
}

#pragma mark -
#pragma mark • Accessors

static mrb_value mrb_matrix_get_row(mrb_state *mrb, mrb_value self) {
  mrb_int i, n;
  mrb_value result, args[1];
  gsl_matrix *p_mat = NULL;
  gsl_vector *p_vec = NULL;

  n = mrb_get_args(mrb, "|i", &i);
  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);

  if (n == 1) {
    if (i >= p_mat->size1) {
      mrb_raise(mrb, E_MATRIX_ERROR, "matrix index out of range!");
    }
    args[0] = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@ncols"));
    result = mrb_obj_new(mrb, mrb_class_get(mrb, "Vector"), 1, args);
    mrb_vector_get_data(mrb, result, &p_vec);
    gsl_matrix_get_row(p_vec, p_mat, i);
  }
  else {
    result = mrb_ary_new_capa(mrb, p_mat->size1);
    for (i = 0; i < p_mat->size1; i++) {
      mrb_ary_push(mrb, result, mrb_funcall(mrb, self, "get_row", 1, mrb_fixnum_value(i)));
    }
  }
  return result;
}

static mrb_value mrb_matrix_get_col(mrb_state *mrb, mrb_value self) {
  mrb_int i;
  mrb_value res, args[1];
  gsl_matrix *p_mat = NULL;
  gsl_vector *p_vec = NULL;

  mrb_get_args(mrb, "i", &i);
  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (i >= p_mat->size2) {
    mrb_raise(mrb, E_MATRIX_ERROR, "matrix index out of range!");
  }
  args[0] = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@nrows"));
  res = mrb_obj_new(mrb, mrb_class_get(mrb, "Vector"), 1, args);
  mrb_vector_get_data(mrb, res, &p_vec);
  gsl_matrix_get_col(p_vec, p_mat, i);
  return res;
}

static mrb_value mrb_matrix_get_ij(mrb_state *mrb, mrb_value self) {
  mrb_value result;
  mrb_int i, j;
  gsl_matrix *p_mat = NULL;
  mrb_int n;

  n = mrb_get_args(mrb, "|ii", &i, &j);
  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (n == 2) {
    if (i >= p_mat->size1 || j >= p_mat->size2) {
      mrb_raise(mrb, E_MATRIX_ERROR, "matrix index out of range!");
    }
    result = mrb_float_value(mrb, gsl_matrix_get(p_mat, (size_t)i, (size_t)j));
  } else {
    result = mrb_matrix_get_row(mrb, self);
  }
  return result;
}

static mrb_value mrb_matrix_set_ij(mrb_state *mrb, mrb_value self) {
  mrb_int i, j;
  mrb_float f;
  gsl_matrix *p_mat = NULL;

  mrb_get_args(mrb, "iif", &i, &j, &f);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (i >= p_mat->size1 || j >= p_mat->size2) {
    mrb_raise(mrb, E_MATRIX_ERROR, "matrix index out of range!");
  }
  gsl_matrix_set(p_mat, (size_t)i, (size_t)j, (double)f);
  return mrb_float_value(mrb, f);
}

static mrb_value mrb_matrix_set_row(mrb_state *mrb, mrb_value self) {
  mrb_int i;
  mrb_value other;
  gsl_matrix *p_mat = NULL;
  gsl_vector *p_vec = NULL;

  mrb_get_args(mrb, "io", &i, &other);
  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (i >= p_mat->size1) {
    mrb_raise(mrb, E_MATRIX_ERROR, "matrix row index out of range!");
  }
  mrb_vector_get_data(mrb, other, &p_vec);
  if (p_mat->size2 != p_vec->size) {
    mrb_raise(mrb, E_MATRIX_ERROR, "Size mismatch!");
  }
  gsl_matrix_set_row(p_mat, i, p_vec);
  return self;
}

static mrb_value mrb_matrix_set_col(mrb_state *mrb, mrb_value self) {
  mrb_int i;
  mrb_value other;
  gsl_matrix *p_mat = NULL;
  gsl_vector *p_vec = NULL;

  mrb_get_args(mrb, "io", &i, &other);
  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (i >= p_mat->size2) {
    mrb_raise(mrb, E_MATRIX_ERROR, "matrix col index out of range!");
  }
  mrb_vector_get_data(mrb, other, &p_vec);
  if (p_mat->size1 != p_vec->size) {
    mrb_raise(mrb, E_MATRIX_ERROR, "Size mismatch!");
  }
  gsl_matrix_set_col(p_mat, i, p_vec);
  return self;
}

#pragma mark -
#pragma mark • Properties

static mrb_value mrb_matrix_max(mrb_state *mrb, mrb_value self) {
  gsl_matrix *p_mat = NULL;
  mrb_matrix_get_data(mrb, self, &p_mat);
  return mrb_float_value(mrb, gsl_matrix_max(p_mat));
}

static mrb_value mrb_matrix_min(mrb_state *mrb, mrb_value self) {
  gsl_matrix *p_mat = NULL;
  mrb_matrix_get_data(mrb, self, &p_mat);
  return mrb_float_value(mrb, gsl_matrix_min(p_mat));
}

static mrb_value mrb_matrix_max_index(mrb_state *mrb, mrb_value self) {
  size_t i, j;
  mrb_value res = mrb_ary_new_capa(mrb, 2);
  gsl_matrix *p_mat = NULL;
  mrb_matrix_get_data(mrb, self, &p_mat);
  gsl_matrix_max_index(p_mat, &i, &j);
  mrb_ary_push(mrb, res, mrb_fixnum_value(i));
  mrb_ary_push(mrb, res, mrb_fixnum_value(j));
  return res;
}

static mrb_value mrb_matrix_min_index(mrb_state *mrb, mrb_value self) {
  size_t i, j;
  mrb_value res = mrb_ary_new_capa(mrb, 2);
  gsl_matrix *p_mat = NULL;
  mrb_matrix_get_data(mrb, self, &p_mat);
  gsl_matrix_min_index(p_mat, &i, &j);
  mrb_ary_push(mrb, res, mrb_fixnum_value(i));
  mrb_ary_push(mrb, res, mrb_fixnum_value(j));
  return res;
}

#pragma mark -
#pragma mark • Operations

static mrb_value mrb_matrix_add(mrb_state *mrb, mrb_value self) {
  mrb_value other;
  gsl_matrix *p_mat, *p_mat_other;
  mrb_get_args(mrb, "o", &other);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (mrb_obj_is_kind_of(mrb, other, mrb_class_get(mrb, "Matrix"))) {
    mrb_matrix_get_data(mrb, other, &p_mat_other);
    if (p_mat->size1 != p_mat_other->size1 ||
        p_mat->size2 != p_mat_other->size2) {
      mrb_raise(mrb, E_MATRIX_ERROR, "matrix dimensions don't match!");
    }
    gsl_matrix_add(p_mat, p_mat_other);
  } else if (mrb_obj_is_kind_of(mrb, other, mrb_class_get(mrb, "Numeric"))) {
    gsl_matrix_add_constant(p_mat, mrb_to_flo(mrb, other));
  }
  return self;
}

static mrb_value mrb_matrix_sub(mrb_state *mrb, mrb_value self) {
  mrb_value other;
  gsl_matrix *p_mat, *p_mat_other;
  mrb_get_args(mrb, "o", &other);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  mrb_matrix_get_data(mrb, other, &p_mat_other);
  if (p_mat->size1 != p_mat_other->size1 ||
      p_mat->size2 != p_mat_other->size2) {
    mrb_raise(mrb, E_MATRIX_ERROR, "matrix dimensions don't match!");
  }
  gsl_matrix_sub(p_mat, p_mat_other);
  return self;
}

static mrb_value mrb_matrix_mul(mrb_state *mrb, mrb_value self) {
  mrb_value other;
  gsl_matrix *p_mat, *p_mat_other;
  mrb_get_args(mrb, "o", &other);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (mrb_obj_is_kind_of(mrb, other, mrb_class_get(mrb, "Matrix"))) {
    mrb_matrix_get_data(mrb, other, &p_mat_other);
    if (p_mat->size1 != p_mat_other->size1 ||
        p_mat->size2 != p_mat_other->size2) {
      mrb_raise(mrb, E_MATRIX_ERROR, "matrix dimensions don't match!");
    }
    gsl_matrix_mul_elements(p_mat, p_mat_other);
  } else if (mrb_obj_is_kind_of(mrb, other, mrb_class_get(mrb, "Numeric"))) {
    gsl_matrix_scale(p_mat, mrb_to_flo(mrb, other));
  }
  return self;
}

static mrb_value mrb_matrix_prod(mrb_state *mrb, mrb_value self) {
  mrb_value other, res;
  gsl_matrix *p_mat, *p_mat_other, *p_mat_res;
  gsl_vector *p_vec_other, *p_vec_res;
  mrb_value args[2];
  mrb_get_args(mrb, "o", &other);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);

  if (mrb_obj_is_kind_of(mrb, other, mrb_class_get(mrb, "Matrix"))) {
    args[1] = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@nrows"));
    args[0] = mrb_iv_get(mrb, other, mrb_intern_lit(mrb, "@ncols"));
    res = mrb_obj_new(mrb, mrb_class_get(mrb, "Matrix"), 2, args);
    mrb_matrix_get_data(mrb, other, &p_mat_other);
    mrb_matrix_get_data(mrb, res, &p_mat_res);

    if (p_mat->size2 != p_mat_other->size1) {
      mrb_raise(mrb, E_MATRIX_ERROR, "matrix dimensions don't match!");
    }
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, p_mat, p_mat_other, 0.0,
                   p_mat_res);
  } else if (mrb_obj_is_kind_of(mrb, other, mrb_class_get(mrb, "Vector"))) {
    args[0] = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@nrows"));
    res = mrb_obj_new(mrb, mrb_class_get(mrb, "Vector"), 1, args);
    mrb_vector_get_data(mrb, other, &p_vec_other);
    mrb_vector_get_data(mrb, res, &p_vec_res);

    if (p_mat->size2 != p_vec_other->size) {
      mrb_raise(mrb, E_MATRIX_ERROR, "matrix dimensions don't match!");
    }
    gsl_blas_dgemv(CblasNoTrans, 1.0, p_mat, p_vec_other, 0.0, p_vec_res);
  }
  return res;
}

static mrb_value mrb_matrix_div(mrb_state *mrb, mrb_value self) {
  mrb_value other;
  gsl_matrix *p_mat, *p_mat_other;
  mrb_get_args(mrb, "o", &other);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  mrb_matrix_get_data(mrb, other, &p_mat_other);
  if (p_mat->size1 != p_mat_other->size1 ||
      p_mat->size2 != p_mat_other->size2) {
    mrb_raise(mrb, E_MATRIX_ERROR, "matrix dimensions don't match!");
  }
  gsl_matrix_div_elements(p_mat, p_mat_other);
  return self;
}

static mrb_value mrb_matrix_transpose_self(mrb_state *mrb, mrb_value self) {
  gsl_matrix *p_mat;
  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (p_mat->size1 != p_mat->size2) {
    mrb_raise(mrb, E_MATRIX_ERROR, "matrix must be square!");
  }
  if (gsl_matrix_transpose(p_mat)) {
    mrb_raise(mrb, E_MATRIX_ERROR, "Cannot calculate transposed matrix");
  }
  return self;
}

static mrb_value mrb_matrix_transpose(mrb_state *mrb, mrb_value self) {
  mrb_value other;
  gsl_matrix *p_mat, *p_mat_other;
  mrb_value args[2];
  // swap dimensions!
  args[1] = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@nrows"));
  args[0] = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@ncols"));
  other = mrb_obj_new(mrb, mrb_class_get(mrb, "Matrix"), 2, args);
  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  mrb_matrix_get_data(mrb, other, &p_mat_other);
  if (gsl_matrix_transpose_memcpy(p_mat_other, p_mat)) {
    mrb_raise(mrb, E_MATRIX_ERROR, "Cannot calculate transposed matrix");
  }
  return other;
}

static mrb_value mrb_matrix_swap_rows(mrb_state *mrb, mrb_value self) {
  gsl_matrix *p_mat;
  mrb_int i, j;
  mrb_get_args(mrb, "ii", &i, &j);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (gsl_matrix_swap_rows(p_mat, i, j)) {
    mrb_raise(mrb, E_MATRIX_ERROR, "Cannot swap rows");
  }
  return self;
}

static mrb_value mrb_matrix_swap_cols(mrb_state *mrb, mrb_value self) {
  gsl_matrix *p_mat;
  mrb_int i, j;
  mrb_get_args(mrb, "ii", &i, &j);

  // call utility for unwrapping @data into p_data:
  mrb_matrix_get_data(mrb, self, &p_mat);
  if (gsl_matrix_swap_columns(p_mat, i, j)) {
    mrb_raise(mrb, E_MATRIX_ERROR, "Cannot swap cols");
  }
  return self;
}

#pragma mark -
#pragma mark • Gem setup

void mrb_gsl_matrix_init(mrb_state *mrb) {
  struct RClass *gsl;

  mrb_load_string(mrb, "class MatrixError < Exception; end");

  gsl = mrb_define_class(mrb, "Matrix", mrb->object_class);
  mrb_define_method(mrb, gsl, "initialize", mrb_matrix_initialize,
                    MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "dup", mrb_matrix_dup, MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "all", mrb_matrix_all, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "zero", mrb_matrix_zero, MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "identity", mrb_matrix_identity, MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "rnd_fill", mrb_matrix_rnd_fill, MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "===", mrb_matrix_equal, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "[]", mrb_matrix_get_ij, MRB_ARGS_OPT(2));
  mrb_define_method(mrb, gsl, "row", mrb_matrix_get_row, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "col", mrb_matrix_get_col, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "[]=", mrb_matrix_set_ij, MRB_ARGS_REQ(3));
  mrb_define_method(mrb, gsl, "get_row", mrb_matrix_get_row, MRB_ARGS_OPT(1));
  mrb_define_method(mrb, gsl, "get_col", mrb_matrix_get_col, MRB_ARGS_OPT(1));
  mrb_define_method(mrb, gsl, "set_row", mrb_matrix_set_row, MRB_ARGS_REQ(2));
  mrb_define_method(mrb, gsl, "set_col", mrb_matrix_set_col, MRB_ARGS_REQ(2));

  mrb_define_method(mrb, gsl, "max", mrb_matrix_max, MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "max_index", mrb_matrix_max_index,
                    MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "min", mrb_matrix_min, MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "min_index", mrb_matrix_min_index,
                    MRB_ARGS_NONE());

  mrb_define_method(mrb, gsl, "add!", mrb_matrix_add, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "sub!", mrb_matrix_sub, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "mul!", mrb_matrix_mul, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "div!", mrb_matrix_div, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "^", mrb_matrix_prod, MRB_ARGS_REQ(1));
  mrb_define_method(mrb, gsl, "t!", mrb_matrix_transpose_self, MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "t", mrb_matrix_transpose, MRB_ARGS_NONE());
  mrb_define_method(mrb, gsl, "swap_rows", mrb_matrix_swap_rows,
                    MRB_ARGS_REQ(2));
  mrb_define_method(mrb, gsl, "swap_cols", mrb_matrix_swap_cols,
                    MRB_ARGS_REQ(2));
}


/* eigen/schur.h
 *
 * Copyright (C) 2006 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_SCHUR_H__
#define __GSL_SCHUR_H__

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>

/*
 * Prototypes
 */

void gsl_schur_standardize(gsl_matrix *T, size_t row, gsl_complex *eval1,
                           gsl_complex *eval2, int update_t, gsl_matrix *Z);

#endif /* __GSL_SCHUR_H__ */


#pragma once
#include <gsl/gsl_rng.h>
#include <utility>

// we are using GSL random number generation because i don't trust
// random number generation to be consistent across various C++ stdlib
// implementations, and for the kind of MC simulator we are writing here,
// we want to be able to run it deterministically for testing purposes.

class Sampler {
private:
    gsl_rng *internal_rng_state;
public:
    unsigned long int seed;
    double generate() {
        return gsl_rng_uniform_pos(internal_rng_state);
    };

    Sampler(unsigned long int n) :
        seed (n) {
        internal_rng_state = gsl_rng_alloc(gsl_rng_default);
        gsl_rng_set(internal_rng_state, seed);
        };

    ~Sampler() {
        gsl_rng_free(internal_rng_state);
    };

    // since we don't have access to gsl internal state, can't write
    // copy constructor
    Sampler(Sampler &other) = delete;

    // move constructor
    Sampler(Sampler &&other) :
        internal_rng_state (std::exchange(other.internal_rng_state, nullptr)),
        seed (other.seed)
        {};

    // since we don't have access to gsl internal state, can't write
    // copy assignment operator
    Sampler &operator=(Sampler &other) = delete;

    // move assignment operator
    Sampler &operator=(Sampler &&other) {
        seed = other.seed;

        // we move the existing internal_rng_state into other so
        // it gets freed when other is dropped.
        std::swap(internal_rng_state, other.internal_rng_state);
        return *this;
    };

};


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#pragma once

#include <algorithm>
#include <array>
#include <vector>
#include <gsl/gsl>
#include <assert.h>
#include <boost/optional.hpp>
#include "Data/Intrinsics.h"
#include "TimeUnits.h"
#include "MageSettings.h"

namespace mage
{
    enum class TrackingState
    {
        INITIALIZING = 0,
        TRACKING = 1,
        RELOCALIZING = 2,
        SKIPPED = 3
    };

    enum class FuserMode
    {
        Invalid = 0 ,
        WaitForMageInit = 1,
        WaitForGravityConverge = 2,
        VisualTrackingLost = 3,
        VisualTrackingReacquired = 4,
        ScaleInit = 5,
        Tracking = 6
    };

    enum class PixelFormat
    {
        GRAYSCALE8,
        NV12
    };

    struct FrameId
    {
        std::uint64_t CorrelationId;    //this is the number used to match frames either from each stereo cameras, or from low-res to high-res in the mono case. Should be unique (and increasing) per camera.
        CameraIdentity Camera;

        FrameId()
            : CorrelationId{ 0 }, Camera{ mage::CameraIdentity::MONO }
        {}

        FrameId(const std::uint64_t& id, CameraIdentity cam)
            : CorrelationId{ id }, Camera{ cam }
        {}

        bool operator ==(const FrameId& other) const
        {
            return CorrelationId == other.CorrelationId && Camera == other.Camera;
        }

        bool operator !=(const FrameId& other) const
        {
            return !(*this == other);
        }

        bool operator <(const FrameId& other) const
        {
            if (CorrelationId < other.CorrelationId)
                return true;

            if (other.CorrelationId < CorrelationId)
                return false;

            return Camera < other.Camera;
        }
    };

    struct Size
    {
        size_t Width;
        size_t Height;
    };

    struct Rect
    {
        int X;
        int Y;
        size_t Width;
        size_t Height;
    };

    struct Matrix
    {
        float M11, M12, M13, M14;
        float M21, M22, M23, M24;
        float M31, M32, M33, M34;
        float M41, M42, M43, M44;
    };

    struct Position
    {
        float X;
        float Y;
        float Z;

        std::array<float, 3> AsArray() const
        {
            return{ X, Y, Z };
        }
    };

    struct ProjectedPoint
    {
        float X;
        float Y;
        float Depth;
        int32_t Id;

        ProjectedPoint(float x, float y, float depth, int32_t id)
            : X{ x }, Y{ y }, Depth{ depth }, Id{ id }
        {}

        ProjectedPoint() = default;
    };

    struct AxisAlignedVolume
    {
        Position Min;
        Position Max;
    };

    struct Direction
    {
        float X;
        float Y;
        float Z;
    };

    enum class DataFormat
    {
        BINARY,
        PNG,
        LIVE,
        BOB,
        MIDDLEBURY,
    };

    namespace calibration
    {
        enum class DistortionType
        {
            None,
            Poly3k,
            Rational6k
        };

        class CameraModel
        {
        public:

            CameraModel() = default;
            CameraModel(const Intrinsics& intrinsics, DistortionType distType) : m_intrinsics(intrinsics), m_distortionType(distType) {}
            const Intrinsics& GetIntrinsics() const { return m_intrinsics; }

            //not in opencv order
            virtual gsl::span<const float> GetDistortionCoefficients() const = 0;
            virtual DistortionType GetDistortionType() const { return m_distortionType; }

        private:
            Intrinsics m_intrinsics;
            DistortionType m_distortionType;
        };

        class PinholeCameraModel : public CameraModel
        {
        public:
            PinholeCameraModel() = delete;
            PinholeCameraModel(const Intrinsics& intrinsics) : CameraModel(intrinsics, DistortionType::None) {}

            gsl::span<const float> GetDistortionCoefficients() const { return {}; }
        };

        class Poly3KCameraModel : public CameraModel
        {
        public:
            Poly3KCameraModel() = delete;
            Poly3KCameraModel(const Intrinsics& intrinsics, std::vector<float> distortionCoefficients) :
                CameraModel(intrinsics, DistortionType::Poly3k),
                m_distortionCoefficients{ distortionCoefficients }
            {
                assert(m_distortionCoefficients.size()== 5 && "incorrect number of distortion coefficients. want k1, k2, k3, p1, p2");
            }

            //not in opencv order
            gsl::span<const float> GetDistortionCoefficients() const { return m_distortionCoefficients; }

            float GetK1() const { return m_distortionCoefficients[0];}
            float GetK2() const { return m_distortionCoefficients[1];}
            float GetK3() const { return m_distortionCoefficients[2];}
            float GetP1() const { return m_distortionCoefficients[3];}
            float GetP2() const { return m_distortionCoefficients[4];}

            void SetK1(float k1) { m_distortionCoefficients[0] = k1; }
            void SetK2(float k2) { m_distortionCoefficients[1] = k2; }
            void SetK3(float k3) { m_distortionCoefficients[2] = k3; }
            void SetP1(float p1) { m_distortionCoefficients[3] = p1; }
            void SetP2(float p2) { m_distortionCoefficients[4] = p2; }

        private:
            // k1, k2, k3, p1, p2
            std::vector<float> m_distortionCoefficients;
        };

        class Rational6KCameraModel : public CameraModel
        {
        public:
            Rational6KCameraModel() = delete;
            Rational6KCameraModel(const Intrinsics& intrinsics, std::vector<float> distortionCoefficients)
                : CameraModel(intrinsics, DistortionType::Rational6k),
                m_distortionCoefficients{ distortionCoefficients }
            {
                assert(m_distortionCoefficients.size() == 8 && "incorrect number of distortion coefficients. want k1, k2, k3, k4, k5, k6, p1, p2");
            }

            //not in opencv order
            gsl::span<const float> GetDistortionCoefficients() const { return m_distortionCoefficients; }

            float GetK1() const { return m_distortionCoefficients[0]; }
            float GetK2() const { return m_distortionCoefficients[1]; }
            float GetK3() const { return m_distortionCoefficients[2]; }
            float GetK4() const { return m_distortionCoefficients[3]; }
            float GetK5() const { return m_distortionCoefficients[4]; }
            float GetK6() const { return m_distortionCoefficients[5]; }
            float GetP1() const { return m_distortionCoefficients[6]; }
            float GetP2() const { return m_distortionCoefficients[7]; }

            void SetK1(float k1) { m_distortionCoefficients[0] = k1; }
            void SetK2(float k2) { m_distortionCoefficients[1] = k2; }
            void SetK3(float k3) { m_distortionCoefficients[2] = k3; }
            void SetK4(float k4) { m_distortionCoefficients[3] = k4; }
            void SetK5(float k5) { m_distortionCoefficients[4] = k5; }
            void SetK6(float k6) { m_distortionCoefficients[5] = k6; }
            void SetP1(float p1) { m_distortionCoefficients[6] = p1; }
            void SetP2(float p2) { m_distortionCoefficients[7] = p2; }

        private:
            // k1, k2, k3, k4, k5, k6, p1, p2
            std::vector<float> m_distortionCoefficients;
        };

        struct Line
        {
            float M;
            float B;
        };

        struct Bounds
        {
            float Lower;
            float Upper;
        };

        class LinearFocalLengthModel
        {
        public:

            LinearFocalLengthModel(Line fx, Line fy, float cx, float cy, Bounds focalbounds, Size calibrationSize,
                const std::vector<float>& distortionPoly3k = {},
                const std::vector<float>& distortionRational6k = {}) :
                m_fx(fx), m_fy(fy), m_cx(cx), m_cy(cy), m_focalBounds(focalbounds), m_calibrationSize(calibrationSize)
            {
                assert(distortionPoly3k.empty() || distortionRational6k.empty() && "can't pass two types currently. calibrations have different intrinsics depending on model");
                assert(cx >= 0 && cx <= 1 && "cx and cy need to be a ratio");
                assert(cy >= 0 && cy <= 1 && "cx and cy need to be a ratio");

                if (!distortionPoly3k.empty())
                {
                    assert(distortionPoly3k.size() == 5 && "expecting k0, k1, k2, p0, p1, for poly3k distortion");
                    std::copy(distortionPoly3k.begin(), distortionPoly3k.end(), std::back_inserter(m_distortionPoly3k));
                }

                if (!distortionRational6k.empty())
                {
                    assert(distortionRational6k.size() == 8 && "expecting k0, k1, k2, k3, k4, k5, p0, p1, for rational6k distortion");
                    std::copy(distortionRational6k.begin(), distortionRational6k.end(), std::back_inserter(m_distortionRational6k));
                }
            }

            LinearFocalLengthModel()
            {
                m_fx = { 0,0 };
                m_fy = { 0,0 };
                m_cx = 0;
                m_cy = 0;
                m_focalBounds = { 0,0 };
                m_calibrationSize = { 0, 0 };
            }

            Intrinsics CreateIntrinsics(boost::optional<uint32_t> lensPosition, size_t width, size_t height) const
            {
                assert(width / (float)height == m_calibrationSize.Width / (float)m_calibrationSize.Height && "aspect not equal need to crop to modify resolution");
                assert(lensPosition || (m_fx.M == 0 && m_fy.M == 0));

                float fx = m_fx.B;
                float fy = m_fy.B;

                if (lensPosition)
                {
                    fx += m_fx.M * lensPosition.value();
                    fy += m_fy.M * lensPosition.value();
                }

                float FxInPixelCoordinates = fx * width;
                float FyInPixelCoordinates = fy * height;
                float CxInPixelCoordinates = m_cx * width;
                float CyInPixelCoordinates = m_cy * height;

                return Intrinsics(CxInPixelCoordinates, CyInPixelCoordinates,
                    FxInPixelCoordinates, FyInPixelCoordinates,
                    gsl::narrow<uint32_t>(width), gsl::narrow<uint32_t>(height));
            }

            std::shared_ptr<const PinholeCameraModel> CreatePinholeCameraModel(const boost::optional<uint32_t>& lensPosition, size_t width, size_t height) const
            {
                return std::make_shared<const PinholeCameraModel>(CreateIntrinsics(lensPosition, width, height));
            }

            std::shared_ptr<const Poly3KCameraModel> CreatePoly3kCameraModel(const boost::optional<uint32_t>& lensPosition, size_t width, size_t height) const
            {
                return std::make_shared<const Poly3KCameraModel>(CreateIntrinsics(lensPosition, width, height), m_distortionPoly3k);
            }

            std::shared_ptr<const Rational6KCameraModel> CreateRational6kCameraModel(const boost::optional<uint32_t>& lensPosition, size_t width, size_t height) const
            {
                return std::make_shared<const Rational6KCameraModel>(CreateIntrinsics(lensPosition, width, height), m_distortionRational6k);
            }

            std::shared_ptr<const CameraModel> GetCameraModel(const boost::optional<uint32_t>& lensPosition, size_t width, size_t height)
            {
                if (HasPoly3kModel())
                {
                    return CreatePoly3kCameraModel(lensPosition, width, height);
                }
                else if (HasRational6kModel())
                {
                    return CreateRational6kCameraModel(lensPosition, width, height);
                }
                else
                {
                    return CreatePinholeCameraModel(lensPosition, width, height);
                }
            }

            Line GetFx() const { return m_fx; }
            Line GetFy() const { return m_fy; }
            float GetCx() const { return m_cx; }
            float GetCy() const { return m_cy; }
            Bounds GetFocalBounds() const { return m_focalBounds; }
            Size GetCalibrationSize() const { return m_calibrationSize; }

            bool HasPoly3kModel()
            {
                return m_distortionPoly3k.size() > 0;
            }

            bool HasRational6kModel()
            {
                return m_distortionRational6k.size() > 0;
            }

            std::vector<float> GetDistortionPoly3k() const { return m_distortionPoly3k; }
            std::vector<float> GetDistortionRational6k() const { return m_distortionRational6k; }

        private:
            //F = m*focusValue + b
            Line m_fx, m_fy;
            float m_cx, m_cy;
            Bounds m_focalBounds;
            Size m_calibrationSize;

            //distortion
            std::vector<float> m_distortionPoly3k;
            std::vector<float> m_distortionRational6k;
        };
    }

    struct Depth
    {
        static constexpr float INVALID_DEPTH = -1.0f;

        float NearPlaneDepth;
        float FarPlaneDepth;
        gsl::span<const ProjectedPoint> SparseDepth;

        Depth(float nearDepth, float farDepth, gsl::span<const ProjectedPoint> sparse)
            : NearPlaneDepth{ nearDepth }, FarPlaneDepth{ farDepth }, SparseDepth{ sparse }
        {}

        Depth()
            : NearPlaneDepth{ INVALID_DEPTH }, FarPlaneDepth{ INVALID_DEPTH }
        {}
    };
}