/**
 * scm_mex_core.c
 *
 * This file contains functions for calculation of Spatial channel model for
 * Multiple Input Multiple Output (MIMO) simulations. It also has a mex gateway function
 * for Matlab use.
 *
 * Compilation:
 *
 *    In Matlab type:
 *
 *          mex scm_mex_core.c
 *
 * Instructions for input arguments is found in scm_mec_core.m help file.
 *
 * 
 * @author Jussi Salmi, Helsinki University of Technology, Radio Laboratory, SMARAD Center of Excellence
 * @date 2004/09/29
 * Jari (Apr 17, 2005): Modified power normalization of polarized option.
 * Jussi (Jun 21, 2006): Corrected a bug in real/imaginary gain check in LOS option.
 * Refs:
 *		[1] 3GPP TR 25.996 V6.1.0 (2003-09)
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mex.h"
#define PI 3.14159265358979323846
#define DEFAULT_LUT_POINTS 16384 /* must be a power of two */
#define GENERAL 1
#define POLARIZED 2
#define LOS 3


/**
 * Function complex_multiply calculates the multiplication of two complex numbers.
 * The complex numbers must be in cartesian form of (a + ib)
 * (a + ib)(c + id) = (ac - bd) + i[(a + b)(c + d) - ac - bd] 
 * @param a real part of the first number
 * @param b imaginary part of the first number
 * @param c real part of the second number
 * @param d imaginary part of the second number
 * @param re_ans real part of the answer
 * @param im_ans imaginary part of the answer
 */
void complex_multiply(const double a, const double b, 
   const double c, const double d, double *re_ans, double *im_ans) {
      
   *re_ans = a*c-b*d;
   *im_ans = b*c+a*d;
}

/**
 * Function complex_multiply calculates the multiplication of three complex numbers.
 * @param ar real part of the first number
 * @param ai imaginary part of the first number
 * @param br real part of the second number
 * @param bi imaginary part of the second number
 * @param cr real part of the third number
 * @param ci imaginary part of the third number
 * @param re_ans real part of the answer
 * @param im_ans imaginary part of the answer
 */
void complex_multiply_3(const double ar, const double ai, 
   const double br, const double bi, const double cr, const double ci,
   double *re_ans, double *im_ans) {
      
   *re_ans = ar*br*cr-ar*bi*ci-ai*br*ci-ai*bi*cr;
   *im_ans = ar*br*ci+ar*bi*cr+ai*br*cr-ai*bi*ci;
}

/**
 * Function matrix_multiply calculates open a multiplication of [a][B][c], where
 * a and c are two-element complex vectors and B is a 2-by-2 complex matrix.
 * @param a1_re real part of a1
 * @param a1_im imaginary part of a1
 * @param a2_re real part of a2
 * @param a2_im imaginary part of a2
 * @param c1_re real part of c1
 * @param c1_im imaginary part of c1
 * @param c2_re real part of c2
 * @param c2_im imaginary part of c2
 * @param b11_re real part of b11
 * @param b11_im imaginary part of b11
 * @param b12_re real part of b12
 * @param b12_im imaginary part of b12
 * @param b21_re real part of b21
 * @param b21_im imaginary part of b21
 * @param b22_re real part of b22
 * @param b22_im imaginary part of b22
 * @param ans_real_part pointer to output real part
 * @param ans_imag_part pointer to output imaginary part
 */
void matrix_multiply(double a1_re,double a1_im, double a2_re, double a2_im,
   double c1_re, double c1_im, double c2_re, double c2_im,
   double b11_re, double b11_im, double b12_re, double b12_im,
   double b21_re, double b21_im, double b22_re, double b22_im,
   double *ans_real_part, double *ans_imag_part) {
   
   double real_part, imag_part;
   /* calculating the matrix multiplication open */
   complex_multiply_3(a1_re,a1_im,c1_re,c1_im,b11_re,b11_im,&real_part,&imag_part);
   *ans_real_part = real_part;
   *ans_imag_part = imag_part;
   complex_multiply_3(a2_re,a2_im,c1_re,c1_im,b21_re,b21_im,&real_part,&imag_part);
   *ans_real_part += real_part;
   *ans_imag_part += imag_part;
   complex_multiply_3(a1_re,a1_im,c2_re,c2_im,b12_re,b12_im,&real_part,&imag_part);
   *ans_real_part += real_part;
   *ans_imag_part += imag_part;
   complex_multiply_3(a2_re,a2_im,c2_re,c2_im,b22_re,b22_im,&real_part,&imag_part);
   *ans_real_part += real_part;
   *ans_imag_part += imag_part;
}

/**
 * Function sumloop calculates the repeating summation of subpaths.
 * @param m_i starting index of subpaths
 * @param stop final index +1
 * @param exp_helper table of t-independent terms in complex sinusoidials
 * @param exp_t_coeff table of t-dependent terms
 * @param real_multiplier real part of product of gain terms
 * @param imag_multiplier imaginary part of product of gain terms
 * @param t time instant
 * @param lm order of terms to be summed
 * @param real_sum pointer to real part of the sum
 * @param imag_sum pointer to imaginary part of the sum
 */
void sumloop(int m_i, int stop, const double *exp_helper,const double *exp_t_coeff,
   const double *real_multiplier, const double *imag_multiplier, double t, const double *lm, 
   double *real_sum, double *imag_sum) {

   int lm_i;
   double angle, a, b;

   while (m_i < stop) {
      lm_i = (int)lm[m_i]-1; /* order of terms, lm is indexed in matlab */
      angle = exp_helper[lm_i] + exp_t_coeff[lm_i]* t;
      complex_multiply(real_multiplier[lm_i], imag_multiplier[lm_i],
         cos(angle), sin(angle), &a, &b);
      *real_sum += a; /* real part of the multiplication */
      *imag_sum += b; /* imaginary part of the multiplication */
      m_i++;
   }
}

/**
 * Function sumloop_with_table calculates the repeating summation of subpaths using quantized cosine.
 * @param m_i starting index of subpaths
 * @param stop final index +1
 * @param exp_helper table of t-independent terms in complex sinusoidials
 * @param exp_t_coeff table of t-dependent terms
 * @param real_multiplier real part of product of gain terms
 * @param imag_multiplier imaginary part of product of gain terms
 * @param t time instant
 * @param lm order of terms to be summed
 * @param real_sum pointer to real part of the sum
 * @param imag_sum pointer to imaginary part of the sum
 * @param cos_table table of quantized cosine values
 * @param r2p coefficient for look-up table (num_of_points/(2*PI))
 * @param divider constant used by look-up table
 * @param halfpi (pi/2)
 */
void sumloop_with_table(int m_i, int stop, const double *exp_helper,const double *exp_t_coeff,
   const double *real_multiplier, const double *imag_multiplier, double t, const double *lm, 
   double *real_sum, double *imag_sum, const double *cos_table, const double r2p,
   const long int divider, const double halfpi) {
   
   int lm_i;
   double angle, a, b;

   while (m_i < stop) {
      lm_i = (int)lm[m_i]-1; /* order of terms, lm is indexed in matlab */
      /*printf("lm_i: %i\n", lm_i+1);*/
      angle = exp_helper[lm_i] + exp_t_coeff[lm_i]* t;
      complex_multiply(real_multiplier[lm_i], imag_multiplier[lm_i],
         cos_table[abs(angle*r2p)&divider], cos_table[abs((angle-halfpi)*r2p)&divider],
         &a, &b);
      *real_sum += a; /* real part of the multiplication */
      *imag_sum += b; /* imaginary part of the multiplication */
      m_i++;
   }
}

/**
 * Function scm_sum computes coefficients for 3GPP Spatial channel model [1 5.5.1.
 * The parameter angles must be in radians!
 * @param sin_look_up_points if nonzero, then look-up table for sin/cos is used. Use -1 for default number of points.
 * @param u the number of MS elements
 * @param s the number of BS elements
 * @param n number of multipaths
 * @param l number of midpaths
 * @param m number of subpaths for each n multipath
 * @param k number of individual links
 * @param *re_sq_G_BS real part of square root of BS Gain coefficients (size [k][s][n][m])
 * @param *im_sq_G_BS imaginary part of BS Gain coefficients (size [k][s][n][m])
 * @param *re_sq_G_MS real part of MS Gain coefficients (size [k][u][n][m])
 * @param *im_sq_G_MS imaginary part of MS Gain coefficients (size [k][u][n][m])
 * @param k_CONST wavenumber (2*pi/lambda)
 * @param *d_s distance of BS antenna s from ref. antenna (s=1), (size [s])
 * @param *d_u distance of MS antenna u from ref. antenna (u=1), (size [u])
 * @param *aod Angel of Departure (size [k][n][m])
 * @param *aoa Angel of Arrival (size [k][n][m])
 * @param *phase phase of the mth subpath of the nth path (size [k][n][m])
 * @param *v magnitude of the MS velocity vector (size [k])
 * @param *theta_v angle of the MS velocity vector (size [k])
 * @param *ts vector of time samples (size [k][tn])
 * @param tn number of time samples
 * @param *sq_Pn square root of Pn (size [k][n*l])
 * @param *ln number of subpaths per midpath (size [l])
 * @param *lm subpath indexing order for midpaths, Matlab indexing: 1-m (size [m])
 * @param GainsAreScalar has value 1 if gain is scalar, zero if not
 * @param *re_h pointer to real part of output h, h has to be initialized outside scm function (size [u][s][n][tn][k])
 * @param *im_h pointer to imag part of output h
 * @param *output_SubPathPhase pointer to output phases (size [k][n][m])
 * @return 0 if success, 1 if bad arguments
 */
int scm_sum(const long int sin_look_up_points, const int u, const int s, const int n, const int l, const int m, const int k, 
   const double *re_sq_G_BS, const double *im_sq_G_BS, const double *re_sq_G_MS, const double *im_sq_G_MS,
   const double k_CONST, const double *d_s, const double *d_u, const double *aod, const double *aoa,
   const double *phase, const double *v, const double *theta_v, const double *ts, const int tn,
   const double *sq_Pn, const double *ln, const double *lm, int GainsAreScalar, double *re_h, double *im_h, double *output_SubPathPhase) {

   int i, t_i, n_i, nl_i, l_i, m_i, u_i, s_i, k_i, m_count; /* running index variables */

   /* notation conversion variables from [][] to *(p[0]+var) */
   long int ksnm, kunm, knm, usnltk, kn, knl, kt, usnl, usnltk_i, ksn, nl, kun;

   int msreal, bsreal, both_real; /* boolean variables to check complexity of gains */
   double delta_t;
   double a, b, c, d, real_sum, imag_sum, kds, kdu, kv, gainScalar_re, gainScalar_im;
   double *real_multiplier;
   double *imag_multiplier;
   double *exp_helper; /* part of coefficient for exp(j...) in the sum */
   double *exp_t_coeff;
   double *cos_table;
   double r2p;
   double halfpi;
   double *one_over_sq_ln;
   
   long int num_of_points, divider, pows_of_two;

   real_multiplier = (double*)malloc(m*sizeof(double));
   imag_multiplier = (double*)calloc(m,sizeof(double));
   exp_t_coeff = (double*)malloc(m*sizeof(double));
   exp_helper = (double*)malloc(m*sizeof(double));

   /* check if gains are real valued */
   bsreal = 0;
   msreal = 0;
   both_real = 0;
   if (im_sq_G_BS == NULL)
      bsreal = 1;
   if (im_sq_G_MS == NULL)
      msreal = 1;
   if (bsreal && msreal)
      both_real = 1;

   /* check if gains are scalar */
   if (GainsAreScalar) {
      a = *re_sq_G_BS;
      c = *re_sq_G_MS;
      if (both_real) {
         gainScalar_re = a*c;
         gainScalar_im = 0;
      }
      else {
         if (bsreal)
            b = 0.0;
         else
            b = *im_sq_G_BS;
         if (msreal)
            d = 0.0;
         else
            d = *im_sq_G_MS;
         complex_multiply(a, b, c, d,
            &gainScalar_re, &gainScalar_im);
      }
   }
  
   nl = n*l;
   usnl = u*s*nl; /* for output pointing */
   ksn = k*s*n;  /* for parameter pointing */
   kun = k*u*n;
   
   
   /* calculate coefficient terms */
   one_over_sq_ln = (double*)malloc(l*sizeof(double));
   for(i=0;i<l;i++) {
      one_over_sq_ln[i] = 1/sqrt(ln[i]); 
   }

   /* checks if look-up table is used for sin/cos */
   if (sin_look_up_points) {
      if (sin_look_up_points == -1)
         num_of_points = DEFAULT_LUT_POINTS;
      else {
         pows_of_two = 2;
         while (pows_of_two < sin_look_up_points)
            pows_of_two = pows_of_two*2;
         num_of_points = pows_of_two;
         if (pows_of_two != sin_look_up_points)
            printf("Warning: Number of LU points is not a power-of-2: size changed to %li.\n", num_of_points);
      }

      divider = num_of_points-1;
      r2p = num_of_points/(2*PI);
      halfpi = PI*0.5;
      
      cos_table = (double*)malloc(num_of_points*sizeof(double));

      /* calculate the table */
      for (i=0;i<num_of_points;i++) { 
         cos_table[i] = cos((i+0.5)/r2p);
      }

      /* cycling links */
      for(k_i=0; k_i<k; k_i++) {
         if(tn>1) {
            delta_t = *(ts+k+k_i)-*(ts+k_i);
         }
         kv = k_CONST * v[k_i];
         /* cycling u, u is a given constant*/
         for(u_i=0; u_i < u; u_i++) {
            kdu = k_CONST * *(d_u+u_i);
            /* cyckling s, s is a given constant */
            for(s_i=0;s_i< s; s_i++) {
               kds = k_CONST * *(d_s+s_i);
               /* running through paths, n is a given constant*/
               nl_i = 0;
               for(n_i=0;n_i<n;n_i++) {
                  kn = k*n_i +k_i;
                  /* calculating m-variant t-invariant values */
                  for(m_i=0; m_i<m; m_i++) {                     
                     /* calculation of pointer parameters */
                     knm = k*n*m_i+kn;

                     /* calculation of sqrt(G_BS)*sqrt(G_MS): */
                     if (GainsAreScalar) {