/*
    Licensed to the OpenAirInterface (OAI) Software Alliance under one or more
    contributor license agreements.  See the NOTICE file distributed with
    this work for additional information regarding copyright ownership.
    The OpenAirInterface Software Alliance licenses this file to You under
    the OAI Public License, Version 1.1  (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.openairinterface.org/?page_id=698

    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.
    -------------------------------------------------------------------------------
    For more information about the OpenAirInterface (OAI) Software Alliance:
        contact@openairinterface.org
*/

#include <math.h>
#include <cblas.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "PHY/TOOLS/tools_defs.h"
#include "sim.h"
#include "scm_corrmat.h"
#include "common/utils/LOG/log.h"
//#define DEBUG_CH

#include "assertions.h"

extern void print_shorts(char *s, __m128i *x);

void fill_channel_desc(channel_desc_t *chan_desc,
                       uint8_t nb_tx,
                       uint8_t nb_rx,
                       uint8_t nb_taps,
                       uint8_t channel_length,
                       double *amps,
                       double *delays,
                       struct complex **R_sqrt,
                       double Td,
                       double sampling_rate,
                       double channel_bandwidth,
                       double ricean_factor,
                       double aoa,
                       double forgetting_factor,
                       double max_Doppler,
                       int32_t channel_offset,
                       double path_loss_dB,
                       uint8_t random_aoa)
{

    uint16_t i, j;
    double delta_tau;

    LOG_I(OCM, "[CHANNEL] Getting new channel descriptor, nb_tx %d, nb_rx %d, nb_taps %d, channel_length %d\n",
          nb_tx, nb_rx, nb_taps, channel_length);

    chan_desc->nb_tx          = nb_tx;
    chan_desc->nb_rx          = nb_rx;
    chan_desc->nb_taps        = nb_taps;
    chan_desc->channel_length = channel_length;
    chan_desc->amps           = amps;
    LOG_D(OCM, "[CHANNEL] Doing delays ...\n");
    if(delays == NULL)
    {
        chan_desc->delays = (double *) malloc(nb_taps * sizeof(double));
        delta_tau = Td / nb_taps;
        for(i = 0; i < nb_taps; i++)
        {
            chan_desc->delays[i] = ((double)i) * delta_tau;
        }
    }
    else
    {
        chan_desc->delays              = delays;
    }
    chan_desc->Td                         = Td;
    chan_desc->sampling_rate              = sampling_rate;
    chan_desc->channel_bandwidth          = channel_bandwidth;
    chan_desc->ricean_factor              = ricean_factor;
    chan_desc->aoa                        = aoa;
    chan_desc->random_aoa                 = random_aoa;
    chan_desc->forgetting_factor          = forgetting_factor;
    chan_desc->channel_offset             = channel_offset;
    chan_desc->path_loss_dB               = path_loss_dB;
    chan_desc->first_run                  = 1;
    chan_desc->ip                         = 0.0;
    chan_desc->max_Doppler                = max_Doppler;
    chan_desc->ch                         = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
    chan_desc->chF                        = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
    chan_desc->a                          = (struct complex **) malloc(nb_taps * sizeof(struct complex *));

    LOG_D(OCM, "[CHANNEL] Filling ch \n");

    for(i = 0; i < nb_tx * nb_rx; i++)
    {
        chan_desc->ch[i] = (struct complex *) malloc(channel_length * sizeof(struct complex));
    }

    for(i = 0; i < nb_tx * nb_rx; i++)
    {
        chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));    // allocate for up to 100 RBs, 12 samples per RB
    }

    LOG_D(OCM, "[CHANNEL] Filling a (nb_taps %d)\n", nb_taps);
    for(i = 0; i < nb_taps; i++)
    {
        LOG_D(OCM, "tap %d (%p,%zu)\n", i, &chan_desc->a[i], nb_tx * nb_rx * sizeof(struct complex));
        chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
    }

    LOG_D(OCM, "[CHANNEL] Doing R_sqrt ...\n");
    if(R_sqrt == NULL)
    {
        chan_desc->R_sqrt         = (struct complex **) calloc(nb_taps, sizeof(struct complex *));
        for(i = 0; i < nb_taps; i++)
        {
            chan_desc->R_sqrt[i]    = (struct complex *) calloc(nb_tx * nb_rx * nb_tx * nb_rx, sizeof(struct complex));
            for(j = 0; j < nb_tx * nb_rx * nb_tx * nb_rx; j += (nb_tx * nb_rx + 1))
            {
                chan_desc->R_sqrt[i][j].x = 1.0;
                chan_desc->R_sqrt[i][j].y = 0.0;
            }
        }
    }
    else
    {
        chan_desc->R_sqrt = (struct complex **) calloc(nb_taps, sizeof(struct complex *));
        for(i = 0; i < nb_taps; i++)
        {
            //chan_desc->R_sqrt[i]    = (struct complex*) calloc(nb_tx*nb_rx*nb_tx*nb_rx,sizeof(struct complex));
            //chan_desc->R_sqrt = (struct complex*)&R_sqrt[i][0];
            /* all chan_desc share the same R_sqrt, coming from caller */
            chan_desc->R_sqrt[i] = R_sqrt[0];
        }
    }

    for(i = 0; i < nb_taps; i++)
    {
        for(j = 0; j < nb_tx * nb_rx * nb_tx * nb_rx; j += (nb_tx * nb_rx + 1))
        {
            LOG_D(OCM, "Rsqrt[%d][%d] %f %f\n", i, j, chan_desc->R_sqrt[i][j].x, chan_desc->R_sqrt[i][j].y);
        }
    }

    LOG_D(OCM, "[CHANNEL] RF %f\n", chan_desc->ricean_factor);
    for(i = 0; i < chan_desc->nb_taps; i++)
    {
        LOG_D(OCM, "[CHANNEL] tap %d: amp %f, delay %f\n", i, chan_desc->amps[i], chan_desc->delays[i]);
    }

    chan_desc->nb_paths = 10;

    reset_meas(&chan_desc->random_channel);
    reset_meas(&chan_desc->interp_time);
    reset_meas(&chan_desc->interp_freq);
    reset_meas(&chan_desc->convolution);

}

double mbsfn_delays[] = {0, .03, .15, .31, .37, 1.09, 12.490, 12.52, 12.64, 12.80, 12.86, 13.58, 27.49, 27.52, 27.64, 27.80, 27.86, 28.58};
double mbsfn_amps_dB[] = {0, -1.5, -1.4, -3.6, -0.6, -7.0, -10, -11.5, -11.4, -13.6, -10.6, -17.0, -20, -21.5, -21.4, -23.6, -20.6, -27};

double scm_c_delays[] = {0, 0.0125, 0.0250, 0.3625, 0.3750, 0.3875, 0.2500, 0.2625, 0.2750, 1.0375, 1.0500, 1.0625, 2.7250, 2.7375, 2.7500, 4.6000, 4.6125, 4.6250};
double scm_c_amps_dB[] = {0.00, -2.22, -3.98, -1.86, -4.08, -5.84, -1.08, -3.30, -5.06, -9.08, -11.30, -13.06, -15.14, -17.36, -19.12, -20.64, -22.85, -24.62};

double epa_delays[] = { 0, .03, .07, .09, .11, .19, .41};
double epa_amps_dB[] = {0.0, -1.0, -2.0, -3.0, -8.0, -17.2, -20.8};

double eva_delays[] = { 0, .03, .15, .31, .37, .71, 1.09, 1.73, 2.51};
double eva_amps_dB[] = {0.0, -1.5, -1.4, -3.6, -0.6, -9.1, -7.0, -12.0, -16.9};

double etu_delays[] = { 0, .05, .12, .2, .23, .5, 1.6, 2.3, 5.0};
double etu_amps_dB[] = {-1.0, -1.0, -1.0, 0.0, 0.0, 0.0, -3.0, -5.0, -7.0};

double default_amps_lin[] = {0.3868472, 0.3094778, 0.1547389, 0.0773694, 0.0386847, 0.0193424, 0.0096712, 0.0038685};
double default_amp_lin[] = {1};

double ts_shift_delays[] = {0, 1 / 7.68};
double ts_shift_amps[] = {0, 1};

//correlation matrix for a 2x2 channel with full Tx correlation
struct complex R_sqrt_22_corr_tap[16] = {{0.70711, 0}, {0.0, 0.0}, {0.70711, 0}, {0.0, 0.0},
    {0.0, 0.0}, {0.70711, 0}, {0.0, 0.0}, {0.70711, 0},
    {0.70711, 0}, {0.0, 0.0}, {0.70711, 0}, {0.0, 0.0},
    {0.0, 0.0}, {0.70711, 0}, {0.0, 0.0}, {0.70711, 0}
};
struct complex *R_sqrt_22_corr[1]     = {R_sqrt_22_corr_tap};

//correlation matrix for a fully correlated 2x1 channel (h1==h2)
struct complex R_sqrt_21_corr_tap[4]  = {{0.70711, 0}, {0.70711, 0}, {0.70711, 0}, {0.70711, 0}};
struct complex *R_sqrt_21_corr[1]      = {R_sqrt_21_corr_tap};

//correlation matrix for a 2x2 channel with full Tx anti-correlation
struct complex R_sqrt_22_anticorr_tap[16] = {{0.70711, 0}, {0.0, 0.0}, {-0.70711, 0}, {0.0, 0.0},
    {0.0, 0.0}, {0.70711, 0}, {0.0, 0.0}, {-0.70711, 0},
    {-0.70711, 0}, {0.0, 0.0}, {0.70711, 0}, {0.0, 0.0},
    {0.0, 0.0}, {-0.70711, 0}, {0.0, 0.0}, {0.70711, 0}
};
struct complex *R_sqrt_22_anticorr[1]     = {R_sqrt_22_anticorr_tap};

//correlation matrix for a fully anti-correlated 2x1 channel (h1==-h2)
struct complex R_sqrt_21_anticorr_tap[4]  = {{0.70711, 0}, {-0.70711, 0}, {-0.70711, 0}, {0.70711, 0}};
struct complex *R_sqrt_21_anticorr[1]     = {R_sqrt_21_anticorr_tap};

struct complex **R_sqrt_ptr2;

// full correlation matrix in vectorized form for 2x2 channel, where h1 is  perfectly orthogonal to h2

struct complex R_sqrt_22_orthogonal_tap[16] = {{0.70711, 0.0}, {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0}, {0.70711, 0.0}
};
struct complex *R_sqrt_22_orthogonal[1]     = {R_sqrt_22_orthogonal_tap};

// full correlation matrix for TM4 to make orthogonal effective channel

struct complex R_sqrt_22_orth_eff_ch_TM4_prec_real_tap[16] = {{0.70711, 0.0}, {0.0, 0.0}, {0.70711, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {0.70711, 0.0}, {0.0, 0.0}, {-0.70711, 0.0},
    {0.70711, 0.0}, {0.0, 0.0}, {0.70711, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {-0.70711, 0.0}, {0.0, 0.0}, {0.70711, 0.0}
};
struct complex *R_sqrt_22_orth_eff_ch_TM4_prec_real[1]     = {R_sqrt_22_orth_eff_ch_TM4_prec_real_tap};

struct complex R_sqrt_22_orth_eff_ch_TM4_prec_imag_tap[16] = {{0.70711, 0.0}, {0.0, 0.0}, {0.0, -0.70711}, {0.0, 0.0},
    {0.0, 0.0}, {0.70711, 0.0}, {0.0, 0.0}, {0.0, 0.70711},
    {0.0, -0.70711}, {0.0, 0.0}, {-0.70711, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {0.0, 0.70711}, {0.0, 0.0}, {-0.70711, 0.0}
};
struct complex *R_sqrt_22_orth_eff_ch_TM4_prec_imag[1]     = {R_sqrt_22_orth_eff_ch_TM4_prec_imag_tap};

//Correlation matrix for EPA channel
struct complex R_sqrt_22_EPA_low_tap[16] = {{1.0, 0.0}, {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {1.0, 0.0}, {0.0, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {0.0, 0.0}, {1.0, 0.0}, {0.0, 0.0},
    {0.0, 0.0}, {0.0, 0.0}, {0.0, 0.0}, {1.0, 0.0}
};
struct complex *R_sqrt_22_EPA_low[1]     = {R_sqrt_22_EPA_low_tap};

struct complex R_sqrt_22_EPA_high_tap[16] = {{0.7179, 0.0}, {0.4500, 0.0}, {0.4500, 0.0}, {0.2821, 0.0},
    {0.4500, 0.0}, {0.7179, 0.0}, {0.2821, 0.0}, {0.4500, 0.0},
    {0.4500, 0.0}, {0.2821, 0.0}, {0.7179, 0.0}, {0.4500, 0.0},
    {0.2821, 0.0}, {0.4500, 0.0}, {0.4500, 0.0}, {0.7179, 0.0}
};
struct complex *R_sqrt_22_EPA_high[1]     = {R_sqrt_22_EPA_high_tap};

struct complex R_sqrt_22_EPA_medium_tap[16] = {{0.8375, 0.0}, {0.5249, 0.0}, {0.1286, 0.0}, {0.0806, 0.0},
    {0.5249, 0.0}, {0.8375, 0.0}, {0.0806, 0.0}, {0.1286, 0.0},
    {0.1286, 0.0}, {0.0806, 0.0}, {0.8375, 0.0}, {0.5249, 0.0},
    {0.0806, 0.0}, {0.1286, 0.0}, {0.5249, 0.0}, {0.8375, 0.0}
};
struct complex *R_sqrt_22_EPA_medium[1]     = {R_sqrt_22_EPA_medium_tap};

//Rayleigh1_orth_eff_ch_TM4

channel_desc_t *new_channel_desc_scm(uint8_t nb_tx,
                                     uint8_t nb_rx,
                                     SCM_t channel_model,
                                     double sampling_rate,
                                     double channel_bandwidth,
                                     double forgetting_factor,
                                     int32_t channel_offset,
                                     double path_loss_dB)
{

    channel_desc_t *chan_desc = (channel_desc_t *)malloc(sizeof(channel_desc_t));
    uint16_t i, j;
    double sum_amps;
    double aoa, ricean_factor, Td, maxDoppler;
    int channel_length, nb_taps;

    chan_desc->nb_tx                      = nb_tx;
    chan_desc->nb_rx                      = nb_rx;
    chan_desc->sampling_rate              = sampling_rate;
    chan_desc->channel_bandwidth          = channel_bandwidth;
    chan_desc->forgetting_factor          = forgetting_factor;
    chan_desc->channel_offset             = channel_offset;
    chan_desc->path_loss_dB               = path_loss_dB;
    chan_desc->first_run                  = 1;
    chan_desc->ip                                 = 0.0;

    LOG_I(OCM, "Channel Model (inside of new_channel_desc_scm)=%d\n\n", channel_model);

    switch(channel_model)
    {
        case SCM_A:
            LOG_W(OCM, "channel model not yet supported\n");
            free(chan_desc);
            return(NULL);
        case SCM_B:
            LOG_W(OCM, "channel model not yet supported\n");
            free(chan_desc);
            return(NULL);
        case SCM_C:
            chan_desc->nb_taps        = 18;
            chan_desc->Td             = 4.625;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * scm_c_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = scm_c_delays;
            chan_desc->ricean_factor  = 1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }

            chan_desc->R_sqrt  = (struct complex **) malloc(6 * sizeof(struct complex **));
            if(nb_tx == 2 && nb_rx == 2)
            {
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R22_sqrt[i][0];
                }
            }
            else if(nb_tx == 2 && nb_rx == 1)
            {
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R21_sqrt[i][0];
                }
            }
            else if(nb_tx == 1 && nb_rx == 2)
            {
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R12_sqrt[i][0];
                }
            }
            else
            {
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i]    = (struct complex *) malloc(nb_tx * nb_rx * nb_tx * nb_rx * sizeof(struct complex));
                    for(j = 0; j < nb_tx * nb_rx * nb_tx * nb_rx; j += (nb_tx * nb_rx + 1))
                    {
                        chan_desc->R_sqrt[i][j].x = 1.0;
                        chan_desc->R_sqrt[i][j].y = 0.0;
                    }
                    LOG_W(OCM, "correlation matrix not implemented for nb_tx==%d and nb_rx==%d, using identity\n", nb_tx, nb_rx);
                }
            }
            break;
        case SCM_D:
            LOG_W(OCM, "This is not the real SCM-D model! It is just SCM-C with an additional Rice factor!\n");
            chan_desc->nb_taps        = 18;
            chan_desc->Td             = 4.625;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * scm_c_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = scm_c_delays;
            chan_desc->ricean_factor  = 0.1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }

            chan_desc->R_sqrt  = (struct complex **) malloc(6 * sizeof(struct complex **));
            if(nb_tx == 2 && nb_rx == 2)
            {
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R22_sqrt[i][0];
                }
            }
            else if(nb_tx == 2 && nb_rx == 1)
            {
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R21_sqrt[i][0];
                }
            }
            else if(nb_tx == 1 && nb_rx == 2)
            {
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R12_sqrt[i][0];
                }
            }
            else
            {
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i]    = (struct complex *) malloc(nb_tx * nb_rx * nb_tx * nb_rx * sizeof(struct complex));
                    for(j = 0; j < nb_tx * nb_rx * nb_tx * nb_rx; j += (nb_tx * nb_rx + 1))
                    {
                        chan_desc->R_sqrt[i][j].x = 1.0;
                        chan_desc->R_sqrt[i][j].y = 0.0;
                    }
                    LOG_W(OCM, "correlation matrix not implemented for nb_tx==%d and nb_rx==%d, using identity\n", nb_tx, nb_rx);
                }
            }
            break;
        case EPA:
            chan_desc->nb_taps        = 7;
            chan_desc->Td             = .410;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * epa_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = epa_delays;
            chan_desc->ricean_factor  = 1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }
            if(nb_tx == 2 && nb_rx == 2)
            {
                chan_desc->R_sqrt  = (struct complex **) malloc(6 * sizeof(struct complex **));
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R22_sqrt[i][0];
                }
            }
            else
            {
                chan_desc->R_sqrt         = (struct complex **) malloc(6 * sizeof(struct complex **));
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i]    = (struct complex *) malloc(nb_tx * nb_rx * nb_tx * nb_rx * sizeof(struct complex));
                    for(j = 0; j < nb_tx * nb_rx * nb_tx * nb_rx; j += (nb_tx * nb_rx + 1))
                    {
                        chan_desc->R_sqrt[i][j].x = 1.0;
                        chan_desc->R_sqrt[i][j].y = 0.0;
                    }
                    LOG_W(OCM, "correlation matrix only implemented for nb_tx==2 and nb_rx==2, using identity\n");
                }
            }
            break;
        case EPA_low:
            chan_desc->nb_taps        = 7;
            chan_desc->Td             = .410;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * epa_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = epa_delays;
            chan_desc->ricean_factor  = 1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }
            if(nb_tx == 2 && nb_rx == 2)
            {
                chan_desc->R_sqrt  = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex **));
                for(i = 0; i < chan_desc->nb_taps; i++)
                {
                    chan_desc->R_sqrt[i] = R_sqrt_22_EPA_low[0];
                }
            }
            else
            {
                printf("Correlation matrices are implemented for 2 x 2 only");
            }
            /*  else {
                chan_desc->R_sqrt         = (struct complex**) malloc(6*sizeof(struct complex**));
                for (i = 0; i<6; i++) {
                chan_desc->R_sqrt[i]    = (struct complex*) malloc(nb_tx*nb_rx*nb_tx*nb_rx * sizeof(struct complex));
                for (j = 0; j<nb_tx*nb_rx*nb_tx*nb_rx; j+=(nb_tx*nb_rx+1)) {
                  chan_desc->R_sqrt[i][j].x = 1.0;
                  chan_desc->R_sqrt[i][j].y = 0.0;
                }
                LOG_W(OCM,"correlation matrix only implemented for nb_tx==2 and nb_rx==2, using identity\n");
                }
                }*/
            break;
        case EPA_high:
            chan_desc->nb_taps        = 7;
            chan_desc->Td             = .410;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * epa_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = epa_delays;
            chan_desc->ricean_factor  = 1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }
            if(nb_tx == 2 && nb_rx == 2)
            {
                chan_desc->R_sqrt  = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex **));
                for(i = 0; i < chan_desc->nb_taps; i++)
                {
                    chan_desc->R_sqrt[i] = R_sqrt_22_EPA_high[0];
                }
            }
            else
            {
                printf("Correlation matrices are implemented for 2 x 2 only");
            }
            /*  else {
                chan_desc->R_sqrt         = (struct complex**) malloc(6*sizeof(struct complex**));
                for (i = 0; i<6; i++) {
                chan_desc->R_sqrt[i]    = (struct complex*) malloc(nb_tx*nb_rx*nb_tx*nb_rx * sizeof(struct complex));
                for (j = 0; j<nb_tx*nb_rx*nb_tx*nb_rx; j+=(nb_tx*nb_rx+1)) {
                  chan_desc->R_sqrt[i][j].x = 1.0;
                  chan_desc->R_sqrt[i][j].y = 0.0;
                }
                LOG_W(OCM,"correlation matrix only implemented for nb_tx==2 and nb_rx==2, using identity\n");
                }
                }*/
            break;
        case EPA_medium:
            chan_desc->nb_taps        = 7;
            chan_desc->Td             = .410;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * epa_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = epa_delays;
            chan_desc->ricean_factor  = 1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }
            if(nb_tx == 2 && nb_rx == 2)
            {
                chan_desc->R_sqrt  = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex **));
                for(i = 0; i < chan_desc->nb_taps; i++)
                {
                    chan_desc->R_sqrt[i] = R_sqrt_22_EPA_medium[0];
                }
            }
            else
            {
                printf("Correlation matrices are implemented for 2 x 2 only");
            }
            /*  else {
                chan_desc->R_sqrt         = (struct complex**) malloc(6*sizeof(struct complex**));
                for (i = 0; i<6; i++) {
                chan_desc->R_sqrt[i]    = (struct complex*) malloc(nb_tx*nb_rx*nb_tx*nb_rx * sizeof(struct complex));
                for (j = 0; j<nb_tx*nb_rx*nb_tx*nb_rx; j+=(nb_tx*nb_rx+1)) {
                  chan_desc->R_sqrt[i][j].x = 1.0;
                  chan_desc->R_sqrt[i][j].y = 0.0;
                }
                LOG_W(OCM,"correlation matrix only implemented for nb_tx==2 and nb_rx==2, using identity\n");
                }
                }*/
            break;
        case EVA:
            chan_desc->nb_taps        = 9;
            chan_desc->Td             = 2.51;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * eva_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = eva_delays;
            chan_desc->ricean_factor  = 1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }
            if(nb_tx == 2 && nb_rx == 2)
            {
                chan_desc->R_sqrt  = (struct complex **) malloc(6 * sizeof(struct complex **));
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R22_sqrt[i][0];
                }
            }
            else
            {
                chan_desc->R_sqrt         = (struct complex **) malloc(6 * sizeof(struct complex **));
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i]    = (struct complex *) malloc(nb_tx * nb_rx * nb_tx * nb_rx * sizeof(struct complex));
                    for(j = 0; j < nb_tx * nb_rx * nb_tx * nb_rx; j += (nb_tx * nb_rx + 1))
                    {
                        chan_desc->R_sqrt[i][j].x = 1.0;
                        chan_desc->R_sqrt[i][j].y = 0.0;
                    }
                    LOG_W(OCM, "correlation matrix only implemented for nb_tx==2 and nb_rx==2, using identity\n");
                }
            }
            break;
        case ETU:
            chan_desc->nb_taps        = 9;
            chan_desc->Td             = 5.0;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * etu_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = etu_delays;
            chan_desc->ricean_factor  = 1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }
            if(nb_tx == 2 && nb_rx == 2)
            {
                chan_desc->R_sqrt  = (struct complex **) malloc(6 * sizeof(struct complex **));
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i] = (struct complex *) &R22_sqrt[i][0];
                }
            }
            else
            {
                chan_desc->R_sqrt         = (struct complex **) malloc(6 * sizeof(struct complex **));
                for(i = 0; i < 6; i++)
                {
                    chan_desc->R_sqrt[i]    = (struct complex *) malloc(nb_tx * nb_rx * nb_tx * nb_rx * sizeof(struct complex));
                    for(j = 0; j < nb_tx * nb_rx * nb_tx * nb_rx; j += (nb_tx * nb_rx + 1))
                    {
                        chan_desc->R_sqrt[i][j].x = 1.0;
                        chan_desc->R_sqrt[i][j].y = 0.0;
                    }
                    LOG_W(OCM, "correlation matrix only implemented for nb_tx==2 and nb_rx==2, using identity\n");
                }
            }
            break;
        case MBSFN:
            chan_desc->nb_taps        = 18;
            chan_desc->Td             = 28.58;
            chan_desc->channel_length = (int)(2 * chan_desc->sampling_rate * chan_desc->Td + 1 + 2 / (M_PI * M_PI) * log(4 * M_PI * chan_desc->sampling_rate * chan_desc->Td));
            sum_amps = 0;
            chan_desc->amps           = (double *) malloc(chan_desc->nb_taps * sizeof(double));
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i]      = pow(10, .1 * mbsfn_amps_dB[i]);
                sum_amps += chan_desc->amps[i];
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->amps[i] /= sum_amps;
            }
            chan_desc->delays         = mbsfn_delays;
            chan_desc->ricean_factor  = 1;
            chan_desc->aoa            = 0;
            chan_desc->random_aoa     = 0;
            chan_desc->ch             = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->chF            = (struct complex **) malloc(nb_tx * nb_rx * sizeof(struct complex *));
            chan_desc->a              = (struct complex **) malloc(chan_desc->nb_taps * sizeof(struct complex *));
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->ch[i] = (struct complex *) malloc(chan_desc->channel_length * sizeof(struct complex));
            }
            for(i = 0; i < nb_tx * nb_rx; i++)
            {
                chan_desc->chF[i] = (struct complex *) malloc(1200 * sizeof(struct complex));
            }
            for(i = 0; i < chan_desc->nb_taps; i++)
            {
                chan_desc->a[i]         = (struct complex *) malloc(nb_tx * nb_rx * sizeof(struct complex));
            }

            chan_desc->R_sqrt  = (struct complex **) malloc(6 * sizeof(struct complex *));
            for(i = 0; i < 6; i++)
            {
                chan_desc->R_sqrt[i]    = (struct complex *) malloc(nb_tx * nb_rx * nb_tx * nb_rx * sizeof(struct complex));
                for(j = 0; j < nb_tx * nb_rx * nb_tx * nb_rx; j += (nb_tx * nb_rx + 1))
                {
                    chan_desc->R_sqrt[i][j].x = 1.0;
                    chan_desc->R_sqrt[i][j].y = 0.0;
                }
                LOG_W(OCM, "correlation matrix only implemented for nb_tx==2 and nb_rx==2, using identity\n");
            }
            break;
        case Rayleigh8:

            nb_taps = 8;
            Td = 0.8;
            channel_length = (int)11 + 2 * sampling_rate * Td;
            ricean_factor = 1;
            aoa = .03;
            maxDoppler = 0;

            fill_channel_desc(chan_desc,
                              nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amps_lin,
                              NULL,
                              NULL,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        case Rice8:
            nb_taps = 8;
            Td = 0.8;
            channel_length = (int)11 + 2 * sampling_rate * Td;
            ricean_factor = 0.1;
            aoa = 0.7854;
            maxDoppler = 0;

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amps_lin,
                              NULL,
                              NULL,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              1);
            break;

        case Rayleigh1:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 1;
            aoa = .03;
            maxDoppler = 0;

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              NULL,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        case Rayleigh1_800:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 1;
            aoa = .03;
            maxDoppler = 800;

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              NULL,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        case Rayleigh1_corr:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 1;
            aoa = .03;
            maxDoppler = 0;

            if((nb_tx == 2) && (nb_rx == 1))
            {
                R_sqrt_ptr2 = R_sqrt_21_corr;
            }
            else if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_corr;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        case Rayleigh1_anticorr:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 1;
            aoa = .03;
            maxDoppler = 0;

            if((nb_tx == 2) && (nb_rx == 1)) //check this
            {
                R_sqrt_ptr2 = R_sqrt_21_anticorr;
            }
            else if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_anticorr;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        case Rice1:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 0.1;
            aoa = 0.7854;
            maxDoppler = 0;

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              NULL,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        case AWGN:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 0.0;
            aoa = 0.0;
            maxDoppler = 0;

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              NULL,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            printf("AWGN: ricean_factor %f\n", chan_desc->ricean_factor);

            break;

        case TS_SHIFT:
            nb_taps = 2;
            Td = ts_shift_delays[1];
            channel_length = 10;
            ricean_factor = 0.0;
            aoa = 0.0;
            maxDoppler = 0;

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              ts_shift_amps,
                              ts_shift_delays,
                              NULL,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            printf("TS_SHIFT: ricean_factor %f\n", chan_desc->ricean_factor);

            break;

        case Rice1_corr:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 0.1;
            aoa = .03;
            maxDoppler = 0;

            if((nb_tx == 2) && (nb_rx == 1))
            {
                R_sqrt_ptr2 = R_sqrt_21_corr;
            }
            else if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_corr;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              1);
            break;

        case Rice1_anticorr:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 0.1;
            aoa = .03;
            maxDoppler = 0;

            if((nb_tx == 2) && (nb_rx == 1))
            {
                R_sqrt_ptr2 = R_sqrt_21_anticorr;
            }
            else if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_anticorr;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              1);
            break;

        case Rayleigh1_orthogonal:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 1;
            aoa = 0.03;
            maxDoppler = 0;

            if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_orthogonal;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        case Rayleigh1_orth_eff_ch_TM4_prec_real:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 1;
            aoa = 0.03;
            maxDoppler = 0;

            if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_orth_eff_ch_TM4_prec_real;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              1);
            break;

        case Rayleigh1_orth_eff_ch_TM4_prec_imag:
            nb_taps = 1;
            Td = 0;
            channel_length = 1;
            ricean_factor = 1;
            aoa = 0.03;
            maxDoppler = 0;

            if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_orth_eff_ch_TM4_prec_imag;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            fill_channel_desc(chan_desc, nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amp_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        case Rayleigh8_orth_eff_ch_TM4_prec_real:

            if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_orth_eff_ch_TM4_prec_real;
                //R_sqrt_ptr2 = NULL;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            nb_taps = 8;
            Td = 0.8;
            channel_length = (int)11 + 2 * sampling_rate * Td;
            ricean_factor = 1;
            aoa = .03;
            maxDoppler = 0;

            fill_channel_desc(chan_desc,
                              nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amps_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);

            break;

        case Rayleigh8_orth_eff_ch_TM4_prec_imag:
            nb_taps = 8;
            Td = 0.8;
            channel_length = (int)11 + 2 * sampling_rate * Td;
            ricean_factor = 1;
            aoa = .03;
            maxDoppler = 0;

            if((nb_tx == 2) && (nb_rx == 2))
            {
                R_sqrt_ptr2 = R_sqrt_22_orth_eff_ch_TM4_prec_imag;
            }
            else
            {
                R_sqrt_ptr2 = NULL;
            }

            fill_channel_desc(chan_desc,
                              nb_tx,
                              nb_rx,
                              nb_taps,
                              channel_length,
                              default_amps_lin,
                              NULL,
                              R_sqrt_ptr2,
                              Td,
                              sampling_rate,
                              channel_bandwidth,
                              ricean_factor,
                              aoa,
                              forgetting_factor,
                              maxDoppler,
                              channel_offset,
                              path_loss_dB,
                              0);
            break;

        default:
            LOG_W(OCM, "channel model not yet supported\n");
            free(chan_desc);
            return(NULL);
    }
    LOG_D(OCM, "[CHANNEL] RF %f\n", chan_desc->ricean_factor);
    for(i = 0; i < chan_desc->nb_taps; i++)
    {
        LOG_D(OCM, "[CHANNEL] tap %d: amp %f, delay %f\n", i, chan_desc->amps[i], chan_desc->delays[i]);
    }

    chan_desc->nb_paths = 10;

    return(chan_desc);
}

void free_channel_desc_scm(channel_desc_t *ch)
{
    // Must be made cleanly, a lot of leaks...
    free(ch);
}

int random_channel(channel_desc_t *desc, uint8_t abstraction_flag)
{

    double s;
    int i, k, l, aarx, aatx;
    struct complex anew[NB_ANTENNAS_TX * NB_ANTENNAS_RX], acorr[NB_ANTENNAS_TX * NB_ANTENNAS_RX];
    struct complex phase, alpha, beta;

    AssertFatal(desc->nb_tx <= NB_ANTENNAS_TX && desc->nb_rx <= NB_ANTENNAS_RX,
                "random_channel.c: Error: temporary buffer for channel not big enough (%d,%d)\n", desc->nb_tx, desc->nb_rx);

    start_meas(&desc->random_channel);
    for(i = 0; i < (int)desc->nb_taps; i++)
    {
        for(aarx = 0; aarx < desc->nb_rx; aarx++)
        {
            for(aatx = 0; aatx < desc->nb_tx; aatx++)
            {

                anew[aarx + (aatx * desc->nb_rx)].x = sqrt(desc->ricean_factor * desc->amps[i] / 2) * gaussdouble(0.0, 1.0);
                anew[aarx + (aatx * desc->nb_rx)].y = sqrt(desc->ricean_factor * desc->amps[i] / 2) * gaussdouble(0.0, 1.0);

                if((i == 0) && (desc->ricean_factor != 1.0))
                {
                    if(desc->random_aoa == 1)
                    {
                        desc->aoa = uniformrandom() * 2 * M_PI;
                    }

                    // this assumes that both RX and TX have linear antenna arrays with lambda/2 antenna spacing.
                    // Furhter it is assumed that the arrays are parallel to each other and that they are far enough apart so
                    // that we can safely assume plane wave propagation.
                    phase.x = cos(M_PI * ((aarx - aatx) * sin(desc->aoa)));
                    phase.y = sin(M_PI * ((aarx - aatx) * sin(desc->aoa)));

                    anew[aarx + (aatx * desc->nb_rx)].x += phase.x * sqrt(1.0 - desc->ricean_factor);
                    anew[aarx + (aatx * desc->nb_rx)].y += phase.y * sqrt(1.0 - desc->ricean_factor);
                }
#ifdef DEBUG_CH
                printf("(%d,%d,%d) %f->(%f,%f) (%f,%f) phase (%f,%f)\n", aarx, aatx, i, desc->amps[i], anew[aarx + (aatx * desc->nb_rx)].x, anew[aarx + (aatx * desc->nb_rx)].y, desc->aoa, desc->ricean_factor, phase.x, phase.y);
#endif
            } //aatx
        } //aarx

        /*
            // for debugging set a=anew;
            for (aarx=0;aarx<desc->nb_rx;aarx++) {
            for (aatx=0;aatx<desc->nb_tx;aatx++) {
            desc->a[i][aarx+(aatx*desc->nb_rx)].x = anew[aarx+(aatx*desc->nb_rx)].x;
            desc->a[i][aarx+(aatx*desc->nb_rx)].y = anew[aarx+(aatx*desc->nb_rx)].y;
            printf("anew(%d,%d) = %f+1j*%f\n",aatx,aarx,anew[aarx+(aatx*desc->nb_rx)].x, anew[aarx+(aatx*desc->nb_rx)].y);
            }
            }
        */
        //apply correlation matrix
        //compute acorr = R_sqrt[i] * anew
        alpha.x = 1.0;
        alpha.y = 0.0;
        beta.x = 0.0;
        beta.y = 0.0;

        cblas_zgemv(CblasRowMajor, CblasNoTrans, desc->nb_tx * desc->nb_rx, desc->nb_tx * desc->nb_rx,
                    (void *) &alpha, (void *) desc->R_sqrt[i / 3], desc->nb_rx * desc->nb_tx,
                    (void *) anew, 1, (void *) &beta, (void *) acorr, 1);

        /*
            for (aarx=0;aarx<desc->nb_rx;aarx++) {
            for (aatx=0;aatx<desc->nb_tx;aatx++) {
            desc->a[i][aarx+(aatx*desc->nb_rx)].x = acorr[aarx+(aatx*desc->nb_rx)].x;
            desc->a[i][aarx+(aatx*desc->nb_rx)].y = acorr[aarx+(aatx*desc->nb_rx)].y;
            printf("tap %d, acorr1(%d,%d) = %f+1j*%f\n",i,aatx,aarx,acorr[aarx+(aatx*desc->nb_rx)].x, acorr[aarx+(aatx*desc->nb_rx)].y);
            }
            }
        */

        if(desc->first_run == 1)
        {
            cblas_zcopy(desc->nb_tx * desc->nb_rx, (void *) acorr, 1, (void *) desc->a[i], 1);
        }
        else
        {
            // a = alpha*acorr+beta*a
            // a = beta*a
            // a = a+alpha*acorr
            alpha.x = sqrt(1 - desc->forgetting_factor);
            alpha.y = 0;
            beta.x = sqrt(desc->forgetting_factor);
            beta.y = 0;
            cblas_zscal(desc->nb_tx * desc->nb_rx, (void *) &beta, (void *) desc->a[i], 1);
            cblas_zaxpy(desc->nb_tx * desc->nb_rx, (void *) &alpha, (void *) acorr, 1, (void *) desc->a[i], 1);

            //  desc->a[i][aarx+(aatx*desc->nb_rx)].x = (sqrt(desc->forgetting_factor)*desc->a[i][aarx+(aatx*desc->nb_rx)].x) + sqrt(1-desc->forgetting_factor)*anew.x;
            //  desc->a[i][aarx+(aatx*desc->nb_rx)].y = (sqrt(desc->forgetting_factor)*desc->a[i][aarx+(aatx*desc->nb_rx)].y) + sqrt(1-desc->forgetting_factor)*anew.y;
        }

        /*
            for (aarx=0;aarx<desc->nb_rx;aarx++) {
            for (aatx=0;aatx<desc->nb_tx;aatx++) {
            //desc->a[i][aarx+(aatx*desc->nb_rx)].x = acorr[aarx+(aatx*desc->nb_rx)].x;
            //desc->a[i][aarx+(aatx*desc->nb_rx)].y = acorr[aarx+(aatx*desc->nb_rx)].y;
            printf("tap %d, a(%d,%d) = %f+1j*%f\n",i,aatx,aarx,desc->a[i][aarx+(aatx*desc->nb_rx)].x, desc->a[i][aarx+(aatx*desc->nb_rx)].y);
            }
            }
        */

    } //nb_taps
    stop_meas(&desc->random_channel);

    //memset((void *)desc->ch[aarx+(aatx*desc->nb_rx)],0,(int)(desc->channel_length)*sizeof(struct complex));

    if(abstraction_flag == 0)
    {
        start_meas(&desc->interp_time);

        for(aarx = 0; aarx < desc->nb_rx; aarx++)
        {
            for(aatx = 0; aatx < desc->nb_tx; aatx++)
            {
                if(desc->channel_length == 1)
                {
                    desc->ch[aarx + (aatx * desc->nb_rx)][0].x = desc->a[0][aarx + (aatx * desc->nb_rx)].x;
                    desc->ch[aarx + (aatx * desc->nb_rx)][0].y = desc->a[0][aarx + (aatx * desc->nb_rx)].y;
                }
                else
                {

                    for(k = 0; k < (int)desc->channel_length; k++)
                    {
                        desc->ch[aarx + (aatx * desc->nb_rx)][k].x = 0.0;
                        desc->ch[aarx + (aatx * desc->nb_rx)][k].y = 0.0;

                        for(l = 0; l < desc->nb_taps; l++)
                        {
                            if((k - (desc->delays[l]*desc->sampling_rate) - NB_SAMPLES_CHANNEL_OFFSET) == 0)
                            {
                                s = 1.0;
                            }
                            else
                                s = sin(M_PI * (k - (desc->delays[l] * desc->sampling_rate) - NB_SAMPLES_CHANNEL_OFFSET)) /
                                    (M_PI * (k - (desc->delays[l] * desc->sampling_rate) - NB_SAMPLES_CHANNEL_OFFSET));

                            desc->ch[aarx + (aatx * desc->nb_rx)][k].x += s * desc->a[l][aarx + (aatx * desc->nb_rx)].x;
                            desc->ch[aarx + (aatx * desc->nb_rx)][k].y += s * desc->a[l][aarx + (aatx * desc->nb_rx)].y;
                            //    printf("l %d : desc->ch.x %f\n",l,desc->a[l][aarx+(aatx*desc->nb_rx)].x);

                        } //nb_taps

#ifdef DEBUG_CH
                        k = 0;
                        printf("(%d,%d,%d)->(%f,%f)\n", k, aarx, aatx, desc->ch[aarx + (aatx * desc->nb_rx)][k].x, desc->ch[aarx + (aatx * desc->nb_rx)][k].y);
#endif
                    }
                } //channel_length
            } //aatx
        } //aarx
        stop_meas(&desc->interp_time);
    }

    if(desc->first_run == 1)
    {
        desc->first_run = 0;
    }

    return (0);
}

double N_RB2sampling_rate(uint16_t N_RB)
{
    double sampling_rate;
    switch(N_RB)
    {
        case 6:
            sampling_rate = 1.92;
            break;

        case 25:
            sampling_rate = 7.68;
            break;

        case 50:
            sampling_rate = 15.36;
            break;

        case 100:
            sampling_rate = 30.72;
            break;

        default:
            AssertFatal(1 == 0, "Unknown N_PRB %d", N_RB);
    }

    return(sampling_rate);
}

double N_RB2channel_bandwidth(uint16_t N_RB)
{
    double channel_bandwidth;
    switch(N_RB)
    {
        case 6:
            channel_bandwidth = 1.25;
            break;

        case 25:
            channel_bandwidth = 5.00;
            break;

        case 50:
            channel_bandwidth = 10.00;
            break;

        case 100:
            channel_bandwidth = 20.00;
            break;

        default:
            LOG_E(PHY, "Unknown N_PRB\n");
            return(-1);
    }
    return(channel_bandwidth);
}

#ifdef RANDOM_CHANNEL_MAIN
#define sampling_rate 5.0
#define Td 2.0
main(int argc, char **argv)
{

    double amps[8] = {.8, .2, .1, .04, .02, .01, .005};
    struct complex ch[(int)(1 + 2 * sampling_rate * Td)], phase;
    int i;

    randominit();
    phase.x = 1.0;
    phase.y = 0;
    random_channel(amps, Td, 8, sampling_rate, ch, (double)1.0, &phase);
    /*
        for (i=0;i<(11+2*sampling_rate*Td);i++){
        printf("%f + sqrt(-1)*%f\n",ch[i].x,ch[i].y);
        }
    */
}

#endif


/* rng/rand.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>

/* This is the old BSD rand() generator. The sequence is

   x_{n+1} = (a x_n + c) mod m

   with a = 1103515245, c = 12345 and m = 2^31 = 2147483648. The seed
   specifies the initial value, x_1.

   The theoretical value of x_{10001} is 1910041713.

   The period of this generator is 2^31.

   The rand() generator is not very good -- the low bits of successive
   numbers are correlated. */

static inline unsigned long int rand_get (void *vstate);
static double rand_get_double (void *vstate);
static void rand_set (void *state, unsigned long int s);

typedef struct
  {
    unsigned long int x;
  }
rand_state_t;

static inline unsigned long int
rand_get (void *vstate)
{
  rand_state_t *state = (rand_state_t *) vstate;

  /* The following line relies on unsigned 32-bit arithmetic */

  state->x = (1103515245 * state->x + 12345) & 0x7fffffffUL;

  return state->x;
}

static double
rand_get_double (void *vstate)
{
  return rand_get (vstate) / 2147483648.0 ;
}

static void
rand_set (void *vstate, unsigned long int s)
{
  rand_state_t *state = (rand_state_t *) vstate;

  state->x = s;

  return;
}

static const gsl_rng_type rand_type =
{"rand",			/* name */
 0x7fffffffUL,			/* RAND_MAX */
 0,				/* RAND_MIN */
 sizeof (rand_state_t),
 &rand_set,
 &rand_get,
 &rand_get_double};

const gsl_rng_type *gsl_rng_rand = &rand_type;


#include <stdio.h>
#include <stdlib.h>
//#include <gsl_matrix.h>

#define IKFAST_HAS_LIBRARY // Build IKFast with API functions
#define IKFAST_NO_MAIN // Don't include main() from IKFast

#include "ikfast.cpp"
#include "kin.h"

int KinCalcFowardKinematics(jointAngles_t* angles, eeCoord_t* ee) {
	int i;
	double eetrans[3];
	double eerot[9];

	for(i=0;i<9;i++){
		eerot[i] = -48;
	}

	ComputeFk((double*)angles,eetrans,eerot);

	printf("Found fk solution for end frame: \n\n");

	printf("  Translation:  x: %f  y: %f  z: %f  \n", eetrans[0], eetrans[1], eetrans[2] );
	printf("\n");
	printf("     Rotation     %f   %f   %f  \n", eerot[0], eerot[1], eerot[2] );
	printf("       Matrix:    %f   %f   %f  \n", eerot[3], eerot[4], eerot[5] );
	printf("                  %f   %f   %f  \n", eerot[6], eerot[7], eerot[8] );
	printf("\n");

	return 0;
}

#ifdef STANDALONE

int main(int argc, char **argv){
	int i;
	if(argc != 9) {
		printf("usage: %s x1 y1 z1 w1 x2 y2 z2 w2\n", argv[0]);
		return 1;
	}

	const int points = 100;

	waypoint_t waypoint[points];

	waypoint[0].x = atof(argv[1]);
	waypoint[0].y = atof(argv[2]);
	waypoint[0].z = atof(argv[3]);
	waypoint[0].w = atof(argv[4]);
	waypoint[points-1].x = atof(argv[5]);
	waypoint[points-1].y = atof(argv[6]);
	waypoint[points-1].z = atof(argv[7]);
	waypoint[points-1].w = atof(argv[8]);

	KinCalcFowardKinematics(&angles, &ee);

}

#endif


/* specfunc/ellint.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author: G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_precision.h>
#include "gsl_sf_ellint.h"

#include "error.h"

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

inline
static double locMAX3(double x, double y, double z)
{
  double xy = GSL_MAX(x, y);
  return GSL_MAX(xy, z);
}

inline
static double locMAX4(double x, double y, double z, double w)
{
  double xy  = GSL_MAX(x,  y);
  double xyz = GSL_MAX(xy, z);
  return GSL_MAX(xyz, w);
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

/* based on Carlson's algorithms:
   [B. C. Carlson Numer. Math. 33, 1 (1979)]

   see also:
   [B.C. Carlson, Special Functions of Applied Mathematics (1977)]
 */

/* According to Carlson's algorithm, the errtol parameter
   typically effects the relative error in the following way:

   relative error < 16 errtol^6 / (1 - 2 errtol)

     errtol     precision
     ------     ----------
     0.001       1.0e-17
     0.003       2.0e-14
     0.01        2.0e-11
     0.03        2.0e-8
     0.1         2.0e-5
*/

int
gsl_sf_ellint_RC_e(double x, double y, gsl_mode_t mode, gsl_sf_result * result)
{
  const double lolim = 5.0 * GSL_DBL_MIN;
  const double uplim = 0.2 * GSL_DBL_MAX;
  const gsl_prec_t goal = GSL_MODE_PREC(mode);
  const double errtol = ( goal == GSL_PREC_DOUBLE ? 0.001 : 0.03 );
  const double prec   = gsl_prec_eps[goal];

  if(x < 0.0 || y < 0.0 || x + y < lolim) {
    DOMAIN_ERROR(result);
  }
  else if(GSL_MAX(x, y) < uplim) {
    const double c1 = 1.0 / 7.0;
    const double c2 = 9.0 / 22.0;
    double xn = x;
    double yn = y;
    double mu, sn, lamda, s;
    while(1) {
      mu = (xn + yn + yn) / 3.0;
      sn = (yn + mu) / mu - 2.0;
      if (fabs(sn) < errtol) break;
      lamda = 2.0 * sqrt(xn) * sqrt(yn) + yn;
      xn = (xn + lamda) * 0.25;
      yn = (yn + lamda) * 0.25;
    }
    s = sn * sn * (0.3 + sn * (c1 + sn * (0.375 + sn * c2)));
    result->val = (1.0 + s) / sqrt(mu);
    result->err = prec * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    DOMAIN_ERROR(result);
  }
}

int
gsl_sf_ellint_RD_e(double x, double y, double z, gsl_mode_t mode, gsl_sf_result * result)
{
  const gsl_prec_t goal = GSL_MODE_PREC(mode);
  const double errtol = ( goal == GSL_PREC_DOUBLE ? 0.001 : 0.03 );
  const double prec   = gsl_prec_eps[goal];
  const double lolim = 2.0/pow(GSL_DBL_MAX, 2.0/3.0);
  const double uplim = pow(0.1*errtol/GSL_DBL_MIN, 2.0/3.0);

  if(GSL_MIN(x,y) < 0.0 || GSL_MIN(x+y,z) < lolim) {
    DOMAIN_ERROR(result);
  }
  else if(locMAX3(x,y,z) < uplim) {
    const double c1 = 3.0 / 14.0;
    const double c2 = 1.0 /  6.0;
    const double c3 = 9.0 / 22.0;
    const double c4 = 3.0 / 26.0;
    double xn = x;
    double yn = y;
    double zn = z;
    double sigma  = 0.0;
    double power4 = 1.0;
    double ea, eb, ec, ed, ef, s1, s2;
    double mu, xndev, yndev, zndev;
    while(1) {
      double xnroot, ynroot, znroot, lamda;
      double epslon;
      mu = (xn + yn + 3.0 * zn) * 0.2;
      xndev = (mu - xn) / mu;
      yndev = (mu - yn) / mu;
      zndev = (mu - zn) / mu;
      epslon = locMAX3(fabs(xndev), fabs(yndev), fabs(zndev));
      if (epslon < errtol) break;
      xnroot = sqrt(xn);
      ynroot = sqrt(yn);
      znroot = sqrt(zn);
      lamda = xnroot * (ynroot + znroot) + ynroot * znroot;
      sigma  += power4 / (znroot * (zn + lamda));
      power4 *= 0.25;
      xn = (xn + lamda) * 0.25;
      yn = (yn + lamda) * 0.25;
      zn = (zn + lamda) * 0.25;
    }
    ea = xndev * yndev;
    eb = zndev * zndev;
    ec = ea - eb;
    ed = ea - 6.0 * eb;
    ef = ed + ec + ec;
    s1 = ed * (- c1 + 0.25 * c3 * ed - 1.5 * c4 * zndev * ef);
    s2 = zndev * (c2 * ef + zndev * (- c3 * ec + zndev * c4 * ea));
    result->val = 3.0 * sigma + power4 * (1.0 + s1 + s2) / (mu * sqrt(mu));
    result->err = prec * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    DOMAIN_ERROR(result);
  }
}

int
gsl_sf_ellint_RF_e(double x, double y, double z, gsl_mode_t mode, gsl_sf_result * result)
{
  const double lolim = 5.0 * GSL_DBL_MIN;
  const double uplim = 0.2 * GSL_DBL_MAX;
  const gsl_prec_t goal = GSL_MODE_PREC(mode);
  const double errtol = ( goal == GSL_PREC_DOUBLE ? 0.001 : 0.03 );
  const double prec   = gsl_prec_eps[goal];

  if(x < 0.0 || y < 0.0 || z < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x+y < lolim || x+z < lolim || y+z < lolim) {
    DOMAIN_ERROR(result);
  }
  else if(locMAX3(x,y,z) < uplim) {
    const double c1 = 1.0 / 24.0;
    const double c2 = 3.0 / 44.0;
    const double c3 = 1.0 / 14.0;
    double xn = x;
    double yn = y;
    double zn = z;
    double mu, xndev, yndev, zndev, e2, e3, s;
    while(1) {
      double epslon, lamda;
      double xnroot, ynroot, znroot;
      mu = (xn + yn + zn) / 3.0;
      xndev = 2.0 - (mu + xn) / mu;
      yndev = 2.0 - (mu + yn) / mu;
      zndev = 2.0 - (mu + zn) / mu;
      epslon = locMAX3(fabs(xndev), fabs(yndev), fabs(zndev));
      if (epslon < errtol) break;
      xnroot = sqrt(xn);
      ynroot = sqrt(yn);
      znroot = sqrt(zn);
      lamda = xnroot * (ynroot + znroot) + ynroot * znroot;
      xn = (xn + lamda) * 0.25;
      yn = (yn + lamda) * 0.25;
      zn = (zn + lamda) * 0.25;
    }
    e2 = xndev * yndev - zndev * zndev;
    e3 = xndev * yndev * zndev;
    s = 1.0 + (c1 * e2 - 0.1 - c2 * e3) * e2 + c3 * e3;
    result->val = s / sqrt(mu);
    result->err = prec * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    DOMAIN_ERROR(result);
  }
}

int
gsl_sf_ellint_RJ_e(double x, double y, double z, double p, gsl_mode_t mode, gsl_sf_result * result)
{
  const gsl_prec_t goal = GSL_MODE_PREC(mode);
  const double errtol = ( goal == GSL_PREC_DOUBLE ? 0.001 : 0.03 );
  const double prec   = gsl_prec_eps[goal];
  const double lolim =       pow(5.0 * GSL_DBL_MIN, 1.0/3.0);
  const double uplim = 0.3 * pow(0.2 * GSL_DBL_MAX, 1.0/3.0);

  if(x < 0.0 || y < 0.0 || z < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x + y < lolim || x + z < lolim || y + z < lolim || p < lolim) {
    DOMAIN_ERROR(result);
  }
  else if(locMAX4(x,y,z,p) < uplim) {
    const double c1 = 3.0 / 14.0;
    const double c2 = 1.0 /  3.0;
    const double c3 = 3.0 / 22.0;
    const double c4 = 3.0 / 26.0;
    double xn = x;
    double yn = y;
    double zn = z;
    double pn = p;
    double sigma = 0.0;
    double power4 = 1.0;
    double mu, xndev, yndev, zndev, pndev;
    double ea, eb, ec, e2, e3, s1, s2, s3;
    while(1) {
      double xnroot, ynroot, znroot;
      double lamda, alfa, beta;
      double epslon;
      gsl_sf_result rcresult;
      int rcstatus;
      mu = (xn + yn + zn + pn + pn) * 0.2;
      xndev = (mu - xn) / mu;
      yndev = (mu - yn) / mu;
      zndev = (mu - zn) / mu;
      pndev = (mu - pn) / mu;
      epslon = locMAX4(fabs(xndev), fabs(yndev), fabs(zndev), fabs(pndev));
      if(epslon < errtol) break;
      xnroot = sqrt(xn);
      ynroot = sqrt(yn);
      znroot = sqrt(zn);
      lamda = xnroot * (ynroot + znroot) + ynroot * znroot;
      alfa = pn * (xnroot + ynroot + znroot) + xnroot * ynroot * znroot;
      alfa = alfa * alfa;
      beta = pn * (pn + lamda) * (pn + lamda);
      rcstatus = gsl_sf_ellint_RC_e(alfa, beta, mode, &rcresult);
      if(rcstatus != GSL_SUCCESS) {
        result->val = 0.0;
	result->err = 0.0;
        return rcstatus;
      }
      sigma  += power4 * rcresult.val;
      power4 *= 0.25;
      xn = (xn + lamda) * 0.25;
      yn = (yn + lamda) * 0.25;
      zn = (zn + lamda) * 0.25;
      pn = (pn + lamda) * 0.25;
    }

    ea = xndev * (yndev + zndev) + yndev * zndev;
    eb = xndev * yndev * zndev;
    ec = pndev * pndev;
    e2 = ea - 3.0 * ec;
    e3 = eb + 2.0 * pndev * (ea - ec);
    s1 = 1.0 + e2 * (- c1 + 0.75 * c3 * e2 - 1.5 * c4 * e3);
    s2 = eb * (0.5 * c2 + pndev * (- c3 - c3 + pndev * c4));
    s3 = pndev * ea * (c2 - pndev * c3) - c2 * pndev * ec;
    result->val = 3.0 * sigma + power4 * (s1 + s2 + s3) / (mu * sqrt(mu));
    result->err = prec * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    DOMAIN_ERROR(result);
  }
}

/* [Carlson, Numer. Math. 33 (1979) 1, (4.1)] */
int
gsl_sf_ellint_F_e(double phi, double k, gsl_mode_t mode, gsl_sf_result * result)
{
  double sin_phi  = sin(phi);
  double sin2_phi = sin_phi*sin_phi;
  double x = 1.0 - sin2_phi;
  double y = 1.0 - k*k*sin2_phi;
  gsl_sf_result rf;
  int status = gsl_sf_ellint_RF_e(x, y, 1.0, mode, &rf);
  result->val = sin_phi * rf.val;
  result->err = GSL_DBL_EPSILON * fabs(result->val) + fabs(sin_phi*rf.err);
  return status;
}

/* [Carlson, Numer. Math. 33 (1979) 1, (4.2)] */
int
gsl_sf_ellint_E_e(double phi, double k, gsl_mode_t mode, gsl_sf_result * result)
{
  const double sin_phi  = sin(phi);
  const double sin2_phi = sin_phi  * sin_phi;
  const double x = 1.0 - sin2_phi;
  const double y = 1.0 - k*k*sin2_phi;
  if(x < GSL_DBL_EPSILON) {
    return gsl_sf_ellint_Ecomp_e(k, mode, result);
  }
  else {
    gsl_sf_result rf;
    gsl_sf_result rd;
    const double sin3_phi = sin2_phi * sin_phi;
    const int rfstatus = gsl_sf_ellint_RF_e(x, y, 1.0, mode, &rf);
    const int rdstatus = gsl_sf_ellint_RD_e(x, y, 1.0, mode, &rd);
    result->val  = sin_phi * rf.val - k*k/3.0 * sin3_phi * rd.val;
    result->err  = GSL_DBL_EPSILON * fabs(sin_phi * rf.val);
    result->err += fabs(sin_phi*rf.err);
    result->err += k*k/3.0 * GSL_DBL_EPSILON * fabs(sin3_phi * rd.val);
    result->err += k*k/3.0 * fabs(sin3_phi*rd.err);
    return GSL_ERROR_SELECT_2(rfstatus, rdstatus);
  }
}

/* [Carlson, Numer. Math. 33 (1979) 1, (4.3)] */
int
gsl_sf_ellint_P_e(double phi, double k, double n, gsl_mode_t mode, gsl_sf_result * result)
{
  const double sin_phi  = sin(phi);
  const double sin2_phi = sin_phi  * sin_phi;
  const double sin3_phi = sin2_phi * sin_phi;
  const double x = 1.0 - sin2_phi;
  const double y = 1.0 - k*k*sin2_phi;
  gsl_sf_result rf;
  gsl_sf_result rj;
  const int rfstatus = gsl_sf_ellint_RF_e(x, y, 1.0, mode, &rf);
  const int rjstatus = gsl_sf_ellint_RJ_e(x, y, 1.0, 1.0 + n*sin2_phi, mode, &rj);
  result->val  = sin_phi * rf.val - n/3.0*sin3_phi * rj.val;
  result->err  = GSL_DBL_EPSILON * fabs(sin_phi * rf.val);
  result->err += n/3.0 * fabs(sin3_phi*rj.err);
  return GSL_ERROR_SELECT_2(rfstatus, rjstatus);
}

/* [Carlson, Numer. Math. 33 (1979) 1, (4.4)] */
int
gsl_sf_ellint_D_e(double phi, double k, double n, gsl_mode_t mode, gsl_sf_result * result)
{
  const double sin_phi  = sin(phi);
  const double sin2_phi = sin_phi  * sin_phi;
  const double sin3_phi = sin2_phi * sin_phi;
  const double x = 1.0 - sin2_phi;
  const double y = 1.0 - k*k*sin2_phi;
  gsl_sf_result rd;
  const int status = gsl_sf_ellint_RD_e(x, y, 1.0, mode, &rd);
  result->val = sin3_phi/3.0 * rd.val;
  result->err = GSL_DBL_EPSILON * fabs(result->val) + fabs(sin3_phi/3.0 * rd.err);
  return status;
}

/* [Carlson, Numer. Math. 33 (1979) 1, (4.5)] */
int
gsl_sf_ellint_Kcomp_e(double k, gsl_mode_t mode, gsl_sf_result * result)
{
  if(k*k >= 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(k*k >= 1.0 - GSL_SQRT_DBL_EPSILON) {
    /* [Abramowitz+Stegun, 17.3.33] */
    const double y = 1.0 - k*k;
    const double a[] = { 1.38629436112, 0.09666344259, 0.03590092383 };
    const double b[] = { 0.5, 0.12498593597, 0.06880248576 };
    const double ta = a[0] + y*(a[1] + y*a[2]);
    const double tb = -log(y) * (b[0] * y*(b[1] + y*b[2]));
    result->val = ta + tb;
    result->err = 2.0 * GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else {
    return gsl_sf_ellint_RF_e(0.0, 1.0 - k*k, 1.0, mode, result);
  }
}

/* [Carlson, Numer. Math. 33 (1979) 1, (4.6)] */
int
gsl_sf_ellint_Ecomp_e(double k, gsl_mode_t mode, gsl_sf_result * result)
{
  if(k*k >= 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(k*k >= 1.0 - GSL_SQRT_DBL_EPSILON) {
    /* [Abramowitz+Stegun, 17.3.36] */
    const double y = 1.0 - k*k;
    const double a[] = { 0.44325141463, 0.06260601220, 0.04757383546 };
    const double b[] = { 0.24998368310, 0.09200180037, 0.04069697526 };
    const double ta = 1.0 + y*(a[0] + y*(a[1] + a[2]*y));
    const double tb = -y*log(y) * (b[0] + y*(b[1] + b[2]*y));
    result->val = ta + tb;
    result->err = 2.0 * GSL_DBL_EPSILON * result->val;
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result rf;
    gsl_sf_result rd;
    const double y = 1.0 - k*k;
    const int rfstatus = gsl_sf_ellint_RF_e(0.0, y, 1.0, mode, &rf);
    const int rdstatus = gsl_sf_ellint_RD_e(0.0, y, 1.0, mode, &rd);
    result->val = rf.val - k*k/3.0 * rd.val;
    result->err = rf.err + k*k/3.0 * rd.err;
    return GSL_ERROR_SELECT_2(rfstatus, rdstatus);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_ellint_Kcomp(double k, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_Kcomp_e(k, mode, &result));
}

double gsl_sf_ellint_Ecomp(double k, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_Ecomp_e(k, mode, &result));
}

double gsl_sf_ellint_F(double phi, double k, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_F_e(phi, k, mode, &result));
}

double gsl_sf_ellint_E(double phi, double k, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_E_e(phi, k, mode, &result));
}

double gsl_sf_ellint_P(double phi, double k, double n, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_P_e(phi, k, n, mode, &result));
}

double gsl_sf_ellint_D(double phi, double k, double n, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_D_e(phi, k, n, mode, &result));
}

double gsl_sf_ellint_RC(double x, double y, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_RC_e(x, y, mode, &result));
}

double gsl_sf_ellint_RD(double x, double y, double z, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_RD_e(x, y, z, mode, &result));
}

double gsl_sf_ellint_RF(double x, double y, double z, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_RF_e(x, y, z, mode, &result));
}

double gsl_sf_ellint_RJ(double x, double y, double z, double p, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_ellint_RJ_e(x, y, z, p, mode, &result));
}


/**
 *
 * @file core_cgelqt.c
 *
 *  PLASMA core_blas kernel
 *  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 c Tue Jan  7 11:44:45 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex32_t
 *
 *  CORE_cgelqt - computes a LQ factorization of a complex M-by-N tile A: A = L * Q.
 *
 *  The tile Q is represented as a product of elementary reflectors
 *
 *    Q = H(k)' . . . H(2)' H(1)', where k = min(M,N).
 *
 *  Each H(i) has the form
 *
 *    H(i) = I - tau * v * v'
 *
 *  where tau is a complex scalar, and v is a complex vector with
 *  v(1:i-1) = 0 and v(i) = 1; conjfg(v(i+1:n)) is stored on exit in
 *  A(i,i+1:n), and tau in TAU(i).
 *
 *******************************************************************************
 *
 * @param[in] M
 *          The number of rows of the tile A.  M >= 0.
 *
 * @param[in] N
 *         The number of columns of the tile A.  N >= 0.
 *
 * @param[in] IB
 *         The inner-blocking size.  IB >= 0.
 *
 * @param[in,out] A
 *         On entry, the M-by-N tile A.
 *         On exit, the elements on and below the diagonal of the array
 *         contain the M-by-min(M,N) lower trapezoidal tile L (L is
 *         lower triangular if M <= N); the elements above the diagonal,
 *         with the array TAU, represent the unitary tile Q as a
 *         product of elementary reflectors (see Further Details).
 *
 * @param[in] LDA
 *         The leading dimension of the array A.  LDA >= max(1,M).
 *
 * @param[out] T
 *         The IB-by-N triangular factor T of the block reflector.
 *         T is upper triangular by block (economic storage);
 *         The rest of the array is not referenced.
 *
 * @param[in] LDT
 *         The leading dimension of the array T. LDT >= IB.
 *
 * @param[out] TAU
 *         The scalar factors of the elementary reflectors (see Further
 *         Details).
 *
 * @param[out] WORK
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_cgelqt = PCORE_cgelqt
#define CORE_cgelqt PCORE_cgelqt
#endif
int CORE_cgelqt(int M, int N, int IB,
                PLASMA_Complex32_t *A, int LDA,
                PLASMA_Complex32_t *T, int LDT,
                PLASMA_Complex32_t *TAU,
                PLASMA_Complex32_t *WORK)
{
    int i, k, sb;

    /* Check input arguments */
    if (M < 0) {
        coreblas_error(1, "Illegal value of M");
        return -1;
    }
    if (N < 0) {
        coreblas_error(2, "Illegal value of N");
        return -2;
    }
    if ((IB < 0) || ( (IB == 0) && ((M > 0) && (N > 0)) )) {
        coreblas_error(3, "Illegal value of IB");
        return -3;
    }
    if ((LDA < max(1,M)) && (M > 0)) {
        coreblas_error(5, "Illegal value of LDA");
        return -5;
    }
    if ((LDT < max(1,IB)) && (IB > 0)) {
        coreblas_error(7, "Illegal value of LDT");
        return -7;
    }

    /* Quick return */
    if ((M == 0) || (N == 0) || (IB == 0))
        return PLASMA_SUCCESS;

    k = min(M, N);

    for(i = 0; i < k; i += IB) {
        sb = min(IB, k-i);

        LAPACKE_cgelq2_work(LAPACK_COL_MAJOR, sb, N-i,
                            &A[LDA*i+i], LDA, &TAU[i], WORK);

        LAPACKE_clarft_work(LAPACK_COL_MAJOR,
            lapack_const(PlasmaForward),
            lapack_const(PlasmaRowwise),
            N-i, sb,
            &A[LDA*i+i], LDA, &TAU[i],
            &T[LDT*i], LDT);

        if (M > i+sb) {
            LAPACKE_clarfb_work(
                LAPACK_COL_MAJOR,
                lapack_const(PlasmaRight),
                lapack_const(PlasmaNoTrans),
                lapack_const(PlasmaForward),
                lapack_const(PlasmaRowwise),
                M-i-sb, N-i, sb,
                &A[LDA*i+i],      LDA,
                &T[LDT*i],        LDT,
                &A[LDA*i+(i+sb)], LDA,
                WORK, M-i-sb);
        }
    }
    return PLASMA_SUCCESS;
}


/**
 *
 * @file core_dlag2s.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mathieu Faverge
 * @date 2010-11-15
 * @generated ds Tue Jan  7 11:44:47 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_double
 *
 *  CORE_PLASMA_dlag2s converts a double matrix, A, to a
 *  float matrix, B.
 *
 *******************************************************************************
 *
 * @param[in] m
 *          The number of rows of the matrices A and B. m >= 0.
 *
 * @param[in] n
 *          The number of columns of the matrices A and B. n >= 0.
 *
 * @param[in] A
 *          The double m-by-n matrix to convert.
 *
 * @param[in] lda
 *          The leading dimension of the array A. lda >= max(1,m).
 *
 * @param[out] B
 *          The float m-by-n matrix to convert.
 *
 * @param[in] ldb
 *          The leading dimension of the array B. ldb >= max(1,m).
 *
 * @param[out] info
 *         - 0 on successful exit.
 *         - 1 if an entry of the matrix A is greater than the SINGLE
 *         PRECISION overflow threshold, in this case, the content
 *         of B in exit is unspecified.
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_dlag2s = PCORE_dlag2s
#define CORE_dlag2s PCORE_dlag2s
#endif
void CORE_dlag2s(int m, int n,
                 const double *A, int lda,
                 float *B, int ldb, int *info)
{
    *info = LAPACKE_dlag2s_work(LAPACK_COL_MAJOR, m, n, A, lda, B, ldb);
}

/***************************************************************************//**
 *
 * @ingroup CORE_double
 *
 *  CORE_PLASMA_slag2d converts a float matrix, A, to a
 *  double matrix, B.
 *
 *******************************************************************************
 *
 * @param[in] m
 *          The number of rows of the matrices A and B. m >= 0.
 *
 * @param[in] n
 *          The number of columns of the matrices A and B. n >= 0.
 *
 * @param[in] A
 *          The float m-by-n matrix to convert.
 *
 * @param[in] lda
 *          The leading dimension of the array A. lda >= max(1,m).
 *
 * @param[out] B
 *          The double m-by-n matrix to convert.
 *
 * @param[in] ldb
 *          The leading dimension of the array B. ldb >= max(1,m).
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_slag2d = PCORE_slag2d
#define CORE_slag2d PCORE_slag2d
#endif
void CORE_slag2d(int m, int n,
                 const float *A, int lda,
                 double *B, int ldb)
{
    LAPACKE_slag2d_work(LAPACK_COL_MAJOR, m, n, A, lda, B, ldb);
}


#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
#include <immintrin.h>
#include <omp.h>
#include <Python.h>
#include <numpy/arrayobject.h>

#include <cblas.h>

static PyObject *smmprod_func(PyObject* self, PyObject* args)
{
    PyObject *A_obj = NULL;
    PyObject *A_arr = NULL;
    PyArrayObject *A = NULL;

    PyObject *B_obj = NULL;
    PyObject *B_arr = NULL;
    PyArrayObject *B = NULL;

    PyObject *Omega_obj = NULL;
    PyObject *Omega_row_arr = NULL;
    PyObject *Omega_col_arr = NULL;
    PyObject *Omega_seq = NULL;
    PyArrayObject *Omega_row = NULL;
    PyArrayObject *Omega_col = NULL;

    PyObject *Out_obj = NULL;
    PyObject *Out_arr = NULL;
    PyArrayObject *Out = NULL;

    int result = PyArg_ParseTuple(
        args,
        "OOOO",
        &A_obj,
        &B_obj,
        &Omega_obj,
        &Out_obj
    );

    if (!result) {
        return NULL;
    }

    A_arr = PyArray_FROM_OTF(A_obj, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    if (A_arr == NULL) {
        return NULL;
    }
    A = (PyArrayObject*) A_arr;

    B_arr = PyArray_FROM_OTF(B_obj, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    if (B_arr == NULL) {
        return NULL;
    }
    B = (PyArrayObject*) B_arr;

    Omega_seq = PySequence_Fast(Omega_obj, "expected a sequence");

    Omega_row_arr = PyArray_FROM_OTF(PySequence_Fast_GET_ITEM(Omega_seq, 0), NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    if (Omega_row_arr == NULL) {
        return NULL;
    }
    Omega_row = (PyArrayObject*) Omega_row_arr;

    Omega_col_arr = PyArray_FROM_OTF(PySequence_Fast_GET_ITEM(Omega_seq, 1), NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    if (Omega_col_arr == NULL) {
        return NULL;
    }
    Omega_col = (PyArrayObject*) Omega_col_arr;

    Out_arr = PyArray_FROM_OTF(Out_obj, NPY_DOUBLE, NPY_ARRAY_IN_ARRAY);
    if (Out_arr == NULL) {
        return NULL;
    }
    Out = (PyArrayObject*) Out_arr;

    int ndimA = PyArray_NDIM(A);
    if (ndimA != 2) {
        PyErr_SetString(PyExc_ValueError,
            "A should be 2-dimensional");
        return NULL;
    }

    int ndimB = PyArray_NDIM(B);
    if (ndimB != 2) {
        PyErr_SetString(PyExc_ValueError,
            "B should be 2-dimensional");
        return NULL;
    }

//    int ndimOmega = PyArray_NDIM(B);
//    if (ndimOmega != 2) {
//        PyErr_SetString(PyExc_ValueError,
//            "Omega should be 2-dimensional");
//        return NULL;
//    }

    npy_intp *dimsA = PyArray_DIMS(A);
    npy_intp *dimsB = PyArray_DIMS(B);
    npy_intp *dimsOmega_row = PyArray_DIMS(Omega_row);
    npy_intp *dimsOmega_col = PyArray_DIMS(Omega_col);

    if (dimsOmega_row[0] != dimsOmega_col[0]) {
        PyErr_SetString(PyExc_ValueError,
            "index arrays should have equal length");
        return NULL;
    }

    if (dimsA[1] != dimsB[0] ) {
        PyErr_SetString(PyExc_ValueError,
            "Inner dimensions of A and B do not match");
        return NULL;
    }

    double *bufferA = PyArray_DATA(A);
    double *bufferB = PyArray_DATA(B);
    double *bufferOmega_row = PyArray_DATA(Omega_row);
    double *bufferOmega_col = PyArray_DATA(Omega_col);
    double *bufferOut = PyArray_DATA(Out);

    //int m = dimsA[0];
    int k = dimsA[1];
    int n = dimsB[1];
    int j = dimsOmega_row[0];

    // Remove this pragma for single threaded execution
    #pragma omp parallel for
    for (int i=0; i<j; i++) {
        double *iA = bufferA + k * (int)(bufferOmega_row[i]);
        double *iB = bufferB + (int)(bufferOmega_col[i]);

        /*
         * The plain C version, without blas
        double val = 0;
        for (int x=0; x<k; x++) {
            val += *iA * *iB;
            iA += 1;
            iB += n;
        }
        *(bufferOut++) = val;
        */
        bufferOut[i] = cblas_ddot(k, iA, 1, iB, n);
    }

    Py_DECREF(A_arr);
    Py_DECREF(B_arr);
    Py_DECREF(Omega_row_arr);
    Py_DECREF(Omega_col_arr);
    Py_DECREF(Omega_seq);
    Py_DECREF(Out);
    Py_RETURN_NONE;
}

static PyMethodDef smmprod_methods[] =
{
     {"smmprod", smmprod_func, METH_VARARGS, "calculate fast smmprod in pure C"},
     {NULL, NULL, 0, NULL}
};

#if PY_MAJOR_VERSION >= 3
static struct PyModuleDef moduledef = {
    PyModuleDef_HEAD_INIT,
    "_smmprod",
    NULL,
    -1,
    smmprod_methods
};

PyObject *
PyInit__smmprod(void)
{
    import_array();
    PyObject *module = PyModule_Create(&moduledef);
    return module;
}
#else
void init_smmprod(void)
{
    import_array();
    Py_InitModule("_smmprod", smmprod_methods);
    return;
}

#endif