modem/modem_OFDM2.cpp

/*
 * Copyright 2013, Per Zetterberg, KTH Royal Institute of Technology
 *
 * This program is free software: you can redistribut it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 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, see <http://www.gnu.org/licenses/>.
 */

/* This code should implemen, more or less, the same functions as 
 * 
 * mod_OFDM2.m and demod_OFDM2.m
 *
 *
 */

#include <itpp/itcomm.h>
#include <itpp/stat/misc_stat.h>
#include <math.h>
#include "modem_OFDM2.hpp"
#include <itpp/itbase.h>
#include <complex>
#include <itpp/base/sort.h>
#include <itpp/base/specmat.h>


using namespace itpp;
using namespace std;

OFDM2::OFDM2(void){
  pilot_power_factor=1;
  estimate_SINR=false;
  pulse_shape="0.0546,-0.0898,-0.0587,0.3134,0.5612,0.3133,-0.0587,-0.0898,0.0546";

  //pulse_shape="0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0";

  pulse_shape_delay=4;
  per_symbol_phase_derotation=false;
};


uint32_t OFDM2::number_of_modulation_symbols(void){
  return  ((Ns-known_symbol_pos.length())*length_ix);
};

uint32_t OFDM2::waveform_length(void){
  return (max(d_reorder)*Ns*(Nfft+Np));
};



void OFDM2::init(bool use_pilot_carriers, bool prepend_training,uint32_t prefix_length,uint32_t number_of_symbols, ivec known_symbol_postion_index, ivec reorder){
    
  
    pilot_carrier1=7; // Defined by IEEE 
    pilot_carrier2=Nfft-7; // Defined by IEEE

    //rightmost_carrier=16;
    //leftmost_carrier=Nfft-16;

    rightmost_carrier=19;
    leftmost_carrier=Nfft-19;

    rightmost_carrier_IEEE=rightmost_carrier;
    leftmost_carrier_IEEE=leftmost_carrier-Nfft;


    Ns=number_of_symbols;
    known_symbol_pos=known_symbol_postion_index;
    d_reorder=reorder;

    do_use_pilot_carriers=use_pilot_carriers;
    do_prepend_training=prepend_training;

    Np=prefix_length;

   
    if (do_use_pilot_carriers) 
      length_ix=2*rightmost_carrier-2;
    else
      length_ix=2*rightmost_carrier;

    length_ix_all=2*rightmost_carrier;

    for (int i1=0;i1<(int) Ns;i1++) {
      if (!(i1==known_symbol_pos(0))) {
        first_payload_symbol_index=i1;
        break;
      };
    };


    
    pilot_symbol=1.0+1.0i;
    h.set_size(Nfft);
    
    string temp_str;
    temp_str="1,1,-1,1,1,1,1,-1,1,1,1,-1,1,1,1,-1,-1,-1,1,1,1,1, ";
    temp_str+="-1,-1,-1,-1,-1,1,-1,-1,1,-1,1,1,-1,1,1,1,-1,1,1,1,1,-1,-1,-1, ";
    temp_str+="-1,1,1,1,1,1,1,-1,-1,-1,1,1,-1,1,-1,-1,-1,-1,-1,-1,1,-1,-1,1, ";
    temp_str+="1,1,1,-1,1,-1,-1,-1,-1,1,1,-1,1,-1,-1,-1,1,1,-1,1,-1,-1,-1, ";
    temp_str+="-1,1,1,-1,-1,1,-1,1,-1,1,1,1,1,1,-1,1,1,1,-1,1,-1,-1,1,-1,-1, ";
    temp_str+="1,1,-1,1,1,1,1,1,1,-1,1,-1,-1,-1,1,-1,-1,1,-1,1 ";
    train=temp_str.c_str();
    ix.set_size(length_ix);



    { uint32_t counter=0;
      for (uint32_t i1=1;i1<pilot_carrier1;i1++) {
        ix(counter++)=i1;
      };


      for (uint32_t i1=pilot_carrier1+1;i1<=rightmost_carrier;i1++) {
        ix(counter++)=i1;
      };
      

      rightmost_carrier_sub=counter-1;
      leftmost_carrier_sub=counter;

      for (uint32_t i1=leftmost_carrier;i1<pilot_carrier2;i1++) {
        ix(counter++)=i1;
      };
      
      for (uint32_t i1=pilot_carrier2+1;i1<Nfft;i1++){
        ix(counter++)=i1;
      };
    };
    


    ix_all.set_size(length_ix_all);
    {uint32_t counter=0;

      for (uint32_t i1=1;i1<=rightmost_carrier;i1++) {
        ix_all(counter++)=i1;
      };
      for (uint32_t i1=leftmost_carrier;i1<Nfft;i1++) {
        ix_all(counter++)=i1;
      };

    };

    ix_all_IEEE.set_size(length_ix_all);
    for (int i1=0;i1<(int) length_ix_all;i1++) {
      ix_all_IEEE(i1)=ix_all(i1);
      if (ix_all_IEEE(i1) > (int) Nfft/2)
        ix_all_IEEE(i1)-=Nfft;
    };  



    ix_sub.set_size(length_ix);
    {int counter=0;
      for (uint32_t i1=0;i1<(length_ix+2);i1++) {
        for (uint32_t i2=0;i2<length_ix;i2++) {
          if (ix_all(i1)==ix(i2)) {
            ix_sub(counter)=i1;
            counter++;
          };
        };
      };
    };
  
    for (uint32_t i1=0;i1<length_ix+2;i1++) {
      if (ix_all(i1)==(int32_t) pilot_carrier1) {
        pilot_carrier1_sub=i1;
      };

      if (ix_all(i1)==(int32_t) pilot_carrier2) {
        pilot_carrier2_sub=i1;
      };
    };

    if (!do_use_pilot_carriers) {
      ix=ix_all;
      for (uint32_t i1=0;i1<length_ix;i1++)
        ix_sub(i1)=i1;
    };



    closest_pilot.set_length(length_ix);
    closest_pilot_all.set_length(length_ix_all);
    for (uint32_t i1=0;i1<length_ix;i1++) {
      int Nffti=Nfft;
      int d1=ix_all(ix_sub(i1))-pilot_carrier1;
      if (d1> Nffti/2)
        d1=d1-Nffti;
      if (d1<-Nffti/2)
        d1=d1+Nffti;
      int d2=ix_all(ix_sub(i1))-pilot_carrier2;
      if (d2>Nffti/2)
        d2=d2-Nffti;
      if (d2<-Nffti/2)
        d2=d2+Nffti;

      d1=abs(d1);
      d2=abs(d2);
      if (d1<d2)
        closest_pilot(i1)=1;
      else
        closest_pilot(i1)=2;
    };


    for (uint32_t i1=0;i1<length_ix_all;i1++) {
      int Nffti=Nfft;
      int d1=ix_all(i1)-pilot_carrier1;
      if (d1> Nffti/2)
        d1=d1-Nffti;
      if (d1<-Nffti/2)
        d1=d1+Nffti;
      int d2=ix_all(i1)-pilot_carrier1;
      if (d2>Nffti/2)
        d2=d2-Nffti;
      if (d2<-Nffti/2)
        d2=d2+Nffti;

      d1=abs(d1);
      d2=abs(d2);
      if (d1<d2)
        closest_pilot_all(i1)=1;
      else
        closest_pilot_all(i1)=2;
    };



    int counter=0;
    ix_null.set_length(Nfft-length_ix_all);
    for (uint32_t i1=0;i1<Nfft;i1++) {
      bool is_in_ix_all=false;
      for (uint32_t i2=0;i2<length_ix_all;i2++) {
        if ((int) i1==ix_all(i2))
          is_in_ix_all=true;
      };
      if (!is_in_ix_all) {
        ix_null(counter)=i1;
        counter++;
      };

    };

    pz_init_fft(ix_all);

    cvec temp(Nfft);
    pilot_pattern(temp,ones_c(length_ix_all));
    pilot_pattern_vec=temp(ix_all);

    time_domain_pilot.set_length(Nfft+Np+pulse_shape.length()-1);
    time_domain_pilot.zeros();
    pz_ifft(temp,temp);

    for (uint32_t i1=0;i1<Nfft;i1++)
      time_domain_pilot(i1+Np)=temp(i1);
    for (uint32_t i1=0;i1<Np;i1++)
      time_domain_pilot(i1)=temp(i1+Nfft-Np);
  
    time_domain_filtered_pilot=filter(pulse_shape,1,time_domain_pilot);

    int no_interleavers=8;
    symbol_interleave_matrix.set_size(no_interleavers,length_ix);
    symbol_de_interleave_matrix.set_size(no_interleavers,length_ix);
    RNG_reset(101);
    ivec temp1(length_ix), temp2(length_ix);
    Sort<double> my_sort_f;
    Sort<int> my_sort_i;

    for (int i1=0;i1<no_interleavers;i1++) {
      temp1=my_sort_f.sort_index(0,(int) (length_ix-1),randu(length_ix));
      temp2=my_sort_i.sort_index(0,(int) (length_ix-1),temp1);
      
      symbol_interleave_matrix.set_row(i1,temp1);
      symbol_de_interleave_matrix.set_row(i1,temp2);      
    };



  } ;

void OFDM2::change_pulse_shape(cvec new_pulse_shape, int new_pulse_shape_delay) {
      
  pulse_shape=new_pulse_shape;
  pulse_shape_delay=new_pulse_shape_delay;

  pz_init_fft(ix_all);
  
  cvec temp(Nfft);
  pilot_pattern(temp,ones_c(length_ix_all));
  pilot_pattern_vec=temp(ix_all);

  time_domain_pilot.set_length(Nfft+Np+pulse_shape.length()-1);
  time_domain_pilot.zeros();
  pz_ifft(temp,temp);

  for (uint32_t i1=0;i1<Nfft;i1++)
    time_domain_pilot(i1+Np)=temp(i1);
  for (uint32_t i1=0;i1<Np;i1++)
    time_domain_pilot(i1)=temp(i1+Nfft-Np);
  
  time_domain_filtered_pilot=filter(pulse_shape,1,time_domain_pilot);  

};

void OFDM2::pilot_pattern(cvec &burst, cvec precoder) {


  burst(0)=0.0+0.0i;  burst(1)=-1.0-1.0i; burst(2)=-1.0-1.0i; 
  burst(3)=1.0+1.0i; burst(4)=-1.0-1.0i; 
  burst(5)=1.0+1.0i; burst(6)=-1.0-1.0i; burst(7)=1.0+1.0i; 
  burst(8)=1.0+1.0i; burst(9)=-1.0-1.0i; 
  burst(10)=-1.0-1.0i; burst(11)=-1.0+1.0i; 
  burst(12)=-1.0+1.0i; burst(13)=1.0+1.0i; 
  burst(14)=-1.0+1.0i;  burst(15)=-1.0+1.0i; 
  burst(16)=-1.0+1.0i; burst(17)=1.0+1.0i; 
  burst(18)=1.0+1.0i; burst(19)=1.0+1.0i; 
  burst(20)=0.0+0.0i; burst(21)=0.0+0.0i; 
  burst(22)=0.0+0.0i; burst(23)=0.0+0.0i; 
  burst(24)=0.0+0.0i; burst(25)=0.0+0.0i; 
  burst(26)=0.0+0.0i; burst(27)=0.0+0.0i; 
  burst(28)=0.0+0.0i; burst(29)=0.0+0.0i; 
  burst(30)=0.0+0.0i; burst(31)=0.0+0.0i; 
  burst(32)=0.0+0.0i; burst(33)=0.0+0.0i; 
  burst(34)=0.0+0.0i; burst(35)=0.0+0.0i; 
  burst(36)=0.0+0.0i; burst(37)=0.0+0.0i; 
  burst(38)=0.0+0.0i; burst(39)=0.0+0.0i; 
  burst(40)=0.0+0.0i; burst(41)=0.0+0.0i; 
  burst(42)=0.0+0.0i; burst(43)=0.0+0.0i; 
  burst(44)=0.0+0.0i; 
  burst(45)=0.0+0.0i; burst(46)=0.0+0.0i; 
  burst(47)=0.0+0.0i; burst(48)=0.0+0.0i; 
  burst(49)=0.0+0.0i; burst(50)=0.0+0.0i; 
  burst(51)=0.0+0.0i; burst(52)=0.0+0.0i; 
  burst(53)=0.0+0.0i; burst(54)=0.0+0.0i; 
  burst(55)=0.0+0.0i; burst(56)=0.0+0.0i; 
  burst(57)=0.0+0.0i; burst(58)=0.0+0.0i; 
  burst(59)=0.0+0.0i; burst(60)=0.0+0.0i; 
  burst(61)=-1.0+1.0i; burst(62)=1.0+1.0i; 
  burst(63)=-1.0-1.0i; burst(64)=-1.0+1.0i; 
  burst(65)=1.0+1.0i; burst(66)=-1.0-1.0i; 
  burst(67)=-1.0-1.0i; burst(68)=1.0+1.0i; 
  burst(69)=-1.0+1.0i;  burst(70)=-1.0-1.0i; burst(71)=-1.0+1.0i; 
  burst(72)=-1.0-1.0i; burst(73)=-1.0+1.0i; burst(74)=-1.0+1.0i; 
  burst(75)=-1.0-1.0i; burst(76)=1.0+1.0i; 
  burst(77)=-1.0-1.0i; burst(78)=-1.0+1.0i; 
  burst(79)=1.0+1.0i;


  for (uint32_t i1=0;i1<Nfft;i1++) {
    burst(i1)=burst(i1)*pilot_power_factor/sqrt(2);
  };


  for (uint32_t i1=0;i1<length_ix_all;i1++) {
    burst(ix_all(i1))=burst(ix_all(i1))*precoder(i1);
  };


  //for (uint32_t i1=0;i1<Nfft-length_ix_all;i1++) {
  //  burst(ix_null(i1))=burst(ix_null(i1))*precoder(0);
  //};

}; 


ivec OFDM2::start_of_known_symbol(void){
  ivec temp;
  temp.set_length(known_symbol_pos.length());
  for (int i1=0;i1<known_symbol_pos.length();i1++) {
    temp(i1)=d_reorder.get(known_symbol_pos(i1))*(Nfft+Np);
  };
  return temp;
};




void OFDM2::modulate(cvec &waveform,const cvec &symbols_in,  
                     const uint32_t offset,
double &rms_power,const cvec &precoder)

{

  //cvec symbol(Nfft), symbol_temp(Nfft);
    cvec symbol2(length_ix_all);
    cvec waveform_temp(Nfft);
    double power=0,temp;
    bool the_symbol_is_known;
    int i4; // Counter from symbols_in

    i4=0;
    //symbol.zeros();

    for (int i1=0;i1<(int) Ns;i1++) {
      
      the_symbol_is_known=false;
      for (int i2=0;i2<known_symbol_pos.length();i2++) {
         if (i1==known_symbol_pos(i2))
           the_symbol_is_known=true;
      };

          

      if (the_symbol_is_known) {
        
        //pilot_pattern(symbol,precoder);
        //symbol2=symbol(ix_all);
        symbol2=elem_mult(pilot_pattern_vec,precoder);
         
      } else {  

        for (uint32_t i3=0;i3<length_ix;i3++) {
          //symbol(ix(i3))=symbols_in(i4)*precoder(ix_sub(i3)); 
            symbol2(ix_sub(i3))=symbols_in(i4)*precoder(ix_sub(i3)); 
            i4++;
        };
        if (do_use_pilot_carriers) {
          //symbol[pilot_carrier1]=(1.0+1.0i)/sqrt(2);
          //symbol[pilot_carrier2]=(1.0+1.0i)/sqrt(2);
          //symbol[pilot_carrier1]*=precoder(pilot_carrier1_sub);
          //symbol[pilot_carrier2]*=precoder(pilot_carrier2_sub);

                symbol2[pilot_carrier1_sub]=(1.0+1.0i)/sqrt(2);
                symbol2[pilot_carrier2_sub]=(1.0+1.0i)/sqrt(2);
                symbol2[pilot_carrier1_sub]*=precoder(pilot_carrier1_sub);
                symbol2[pilot_carrier2_sub]*=precoder(pilot_carrier2_sub);


        };
        //symbol2=symbol(ix_all);
      };

      
      //waveform_temp=ifft(symbol,Nfft);
      //pz_ifft(symbol,waveform_temp);
      //waveform_temp=Wifft_sub*symbol(ix_all);
      waveform_temp=Wifft_sub*symbol2;

      //waveform_temp=concat(waveform_temp.mid(Nfft-Np,Np),waveform_temp);
      //waveform_temp=filter(pulse_shape,1,waveform_temp);


      if (rms_power!=-10) {
        //temp=norm(waveform_temp.mid(Np,Nfft));
        //power=power+temp*temp;
        temp=norm(waveform_temp);
        power=power+temp*temp;
      };

      int replace_pos=d_reorder(i1)*(Nfft+Np)+offset-pulse_shape_delay;

      if (replace_pos<0) {
        std::cout << "writing outside of waveform in OFDM2::modulate \n";
        exit(1);
      };
      
      for (int i2=0;i2<waveform_temp.size();i2++) {
         waveform(Np+i2+replace_pos)+=waveform_temp(i2);
      };
      for (int i2=0;i2<(int) Np;i2++){
        waveform(i2+replace_pos)+=waveform_temp(Nfft-Np+i2);
      };

    }; 
    if (rms_power!=-10) {
      power=power/(Ns*Nfft);
      rms_power=sqrt(power);
    };
    

};


void OFDM2::modulate_multi_antenna(cmat &waveform, const cvec &symbols_in,  
                     const uint32_t offset,
                                   vec &rms_power,const cmat &V, double scaling,int Nss)
{




  //cvec waveform_temp(Nfft);
    cvec waveform_temp(length_Wifft_sub_filtered);

    int i4; // Counter from symbols_in
    int no_ant=waveform.rows();
    cmat symbol2(length_ix_all,no_ant);


    i4=0;
    //int Nss=((double) symbols_in.length())/((double) length_ix)+0.999;

    /* Known symbol */
    for (int i2=0;i2<no_ant;i2++) {
          symbol2.set_col(i2,elem_mult(pilot_pattern_vec,V.get_row(i2))*
                          scaling);
    };
    int replace_pos=d_reorder(0)*(Nfft+Np)+offset-pulse_shape_delay;
    for (int i2=0;i2<no_ant;i2++) {

        waveform_temp=Wifft_sub_filtered*symbol2.get_col(i2);

        for (int i3=0;i3<length_Wifft_sub_filtered;i3++){
          waveform(i2,i3+replace_pos)+=waveform_temp(i3);
        };
    };


    if (do_use_pilot_carriers) {

         for (int i2=0;i2<no_ant;i2++) {
           symbol2(pilot_carrier1_sub,i2)=(1.0+1.0i)/sqrt(2);
           symbol2(pilot_carrier2_sub,i2)=(1.0+1.0i)/sqrt(2);
           symbol2(pilot_carrier1_sub,i2)*=V(i2,pilot_carrier1_sub)*scaling;
           symbol2(pilot_carrier2_sub,i2)*=V(i2,pilot_carrier2_sub)*scaling;
         };
    };

    for (int i1=1;i1<(int) Nss+1;i1++) {
      

       for (uint32_t i3=0;i3<length_ix;i3++) {
         for (int i2=0;i2<no_ant;i2++) {
           symbol2(ix_sub(i3),i2)=symbols_in(i4)*V(i2,ix_sub(i3))*scaling;
         };
         i4++;
       };
       #if 0
       if (do_use_pilot_carriers) {

         for (int i2=0;i2<no_ant;i2++) {
           symbol2(pilot_carrier1_sub,i2)=(1.0+1.0i)/sqrt(2);
           symbol2(pilot_carrier2_sub,i2)=(1.0+1.0i)/sqrt(2);
           symbol2(pilot_carrier1_sub,i2)*=V(i2,pilot_carrier1_sub)*scaling;
           symbol2(pilot_carrier2_sub,i2)*=V(i2,pilot_carrier2_sub)*scaling;
         };

       };
       #endif

      int replace_pos=d_reorder(i1)*(Nfft+Np)+offset-pulse_shape_delay;

      for (int i2=0;i2<no_ant;i2++) {

        waveform_temp=Wifft_sub_filtered*symbol2.get_col(i2);       
        for (int i3=0;i3<length_Wifft_sub_filtered;i3++){
          waveform(i2,i3+replace_pos)+=waveform_temp(i3);
        };
      };
    };    
};

void OFDM2::modulate_multi_antenna(cmat &waveform, const cmat &symbols_in,  
                                   const uint32_t offset, vec &rms_power, const cmat V[], double scaling,ivec stream_ix_this_node, int symbols_ix_start, 
int num_symbols) const

{

    
    cvec waveform_temp(length_Wifft_sub_filtered);

    int i4; // Counter from symbols_in
    int no_ant=waveform.rows();
    int no_streams=symbols_in.rows();

    cmat symbol2(length_ix_all,no_ant);
    cvec symbol(length_ix_all);
    int replace_pos;

    if (num_symbols==0)
      num_symbols=Ns-1;

    if (symbols_ix_start==0) {
      for (int i1=0;i1<no_streams;i1++) {

        replace_pos=stream_ix_this_node(i1)*(Nfft+Np)+offset-pulse_shape_delay;

        for (int i2=0;i2<no_ant;i2++) {

          symbol=elem_mult(pilot_pattern_vec,(V[i1]).get_row(i2))*scaling;
          waveform_temp=Wifft_sub_filtered*symbol;        

          for (int i3=0;i3<length_Wifft_sub_filtered;i3++){
            waveform(i2,i3+replace_pos)+=waveform_temp(i3);
          };
       };
     };
    };



    std::complex<double> pilot_symbol=(1.0+1.0i)/sqrt(2);
    pilot_symbol*=scaling;

    i4=symbols_ix_start*length_ix;
    for (int i1=symbols_ix_start+1;i1<=(int) (symbols_ix_start+num_symbols);i1++) {
      

      replace_pos=d_reorder(i1)*(Nfft+Np)+offset-pulse_shape_delay;
      symbol2.zeros();
      for (uint32_t i3=0;i3<length_ix;i3++) {

        for (int i5=0;i5<no_streams;i5++) {
           for (int i2=0;i2<no_ant;i2++) {
             symbol2(ix_sub(i3),i2)+=symbols_in(i5,i4)*(V[i5](i2,ix_sub(i3)))*scaling;
           }; // i2
        }; // i5
        i4++;
      }; // i3
      for (int i5=0;i5<no_streams;i5++) {          
        for (int i2=0;i2<no_ant;i2++) {
            symbol2(pilot_carrier1_sub,i2)+=(V[i5](i2,pilot_carrier1_sub))*pilot_symbol;
            symbol2(pilot_carrier2_sub,i2)+=(V[i5](i2,pilot_carrier2_sub))*pilot_symbol;
        };  // i2
  
      }; // i5

      for (int i2=0;i2<no_ant;i2++) {
         waveform_temp=Wifft_sub_filtered*symbol2.get_col(i2);
         for (int i3=0;i3<length_Wifft_sub_filtered;i3++){
           waveform(i2,i3+replace_pos)+=waveform_temp(i3);
        };
      };
    }; // i1 

    if (!(rms_power(0)==-10)) {
       rms_power.zeros();
       i4=0;
      for (int i1=symbols_ix_start+1;i1<=(int) (symbols_ix_start+num_symbols);i1++) {      
         replace_pos=d_reorder(i1)*(Nfft+Np)+offset;
         for (int i2=0;i2<(int) no_ant;i2++) {
           for (int i3=0;i3<(int) (Nfft);i3++) {
             rms_power(i2)=rms_power(i2)+abs(waveform(i2,i3+replace_pos))*abs(waveform(i2,i3+replace_pos));
             i4++;
           };       
         };  
       };
      rms_power=sqrt(rms_power/i4*no_ant);
    };

};




void OFDM2::insert_pilot(cvec &waveform, uint32_t symbol_position, uint32_t offset, 
cvec precoder) {
  cvec symbol(Nfft);
  cvec waveform_temp(Nfft);
  symbol.zeros();
  pilot_pattern(symbol,precoder);
  //waveform_temp=ifft(symbol,Nfft);
  pz_ifft(symbol,waveform_temp);

  waveform.replace_mid(symbol_position*(Nfft+Np)+Np+offset,waveform_temp);
  waveform.replace_mid(symbol_position*(Nfft+Np)+offset,waveform_temp.mid(Nfft-Np,Np));
};


void OFDM2::insert_pilot(cmat &waveform, uint32_t symbol_position, 
                         uint32_t offset, int antenna_ix,double scaling){

  int i2=symbol_position*(Nfft+Np)+offset-pulse_shape_delay;
  for (int i1=0;i1<time_domain_filtered_pilot.length();i1++) {
    waveform(antenna_ix,i2)+=scaling*time_domain_filtered_pilot(i1);
    i2++;
  };
  
};


void OFDM2::estimate_channel(cvec waveform,double  &noise_variance_normalized, 
                             cvec &h, uint32_t start_sample) {
  double noise_variance;
  estimate_channel(waveform,noise_variance_normalized,h,start_sample,noise_variance);

};



void OFDM2::estimate_channel(cvec waveform,double  &noise_variance_normalized,
                             cvec &h, uint32_t start_sample, double &noise_variance) {

    cvec pilot(Nfft);
    int i1,counter;
    complex <double> hhat, actual_meas, ideal_meas, error;
    double error_div_h;
    cvec signal(Nfft);
    cvec h_rotated(Nfft);
    complex<double> rotation;



    //signal=fft(waveform.mid(0,Nfft));
    pz_fft(waveform.mid(start_sample,Nfft),signal);

    pilot.set_length(Nfft);
    pilot_pattern(pilot,ones_c(length_ix_all));
    h=signal(ix_all);    
    h/=pilot(ix_all);    


    noise_variance_normalized=0;
    noise_variance=0;
    counter=0;

    for (i1=1;i1<ix_all.length()-1;i1++) {

      if (((ix_all(i1)-ix_all(i1-1))==1) && ((ix_all(i1+1)-ix_all(i1))==1)) {

        hhat=0.5*abs(h(i1-1))+0.5*abs(h(i1+1));
        complex<double> t;
        t=h(i1+1)/h(i1-1);
        double a=arg(h(i1-1))+0.5*arg(t);
        hhat=polar(abs(hhat),a);
        ideal_meas=hhat*pilot(ix_all(i1));

        actual_meas=signal(ix_all(i1)); 
        error=ideal_meas-actual_meas;
                
        noise_variance+=abs(error)*abs(error);
        error_div_h=abs(error)/abs(h(i1));
        noise_variance_normalized+=error_div_h*error_div_h;
        counter++;
      }
    };
    noise_variance_normalized/=counter;
    noise_variance/=counter;



 };
 


void OFDM2::init_multi_antenna(uint32_t no_ant, int32_t buffer_size, ivec &interferers_known_pos_in,
double noise_variance) {
 



  x.set_size(no_ant,length_ix_all);

  
  //pc1.set_size(no_ant,Ns-known_symbol_pos.length()); // Signal received on pilot_carrier1 all antennas
  //pc2.set_size(no_ant,Ns-known_symbol_pos.length()); // Signal received on pilot_carrier2 all antennas
  a1c.set_size(no_ant,Ns-known_symbol_pos.length()); // Signal received on pilot_carrier1 all antennas
  a2c.set_size(no_ant,Ns-known_symbol_pos.length()); // Signal received on pilot_carrier2 all antennas
  a1c.ones();
  a2c.ones();


  he.set_size(no_ant,length_ix_all);
  interferers_known_pos=interferers_known_pos_in;
  for (int i1=0;i1<interferers_known_pos.length();i1++) {
    
    hi[i1].set_size(no_ant,length_ix_all);
  };


  d_noise_variance_receiver=noise_variance;
  int num_payload_symbols=0;
  bool the_symbol_is_known;
  for (int i1=0;i1<(int) Ns;i1++) {
    the_symbol_is_known=false;
    for (int i2=0;i2<known_symbol_pos.length();i2++) {
         if (i1==known_symbol_pos(i2))
           the_symbol_is_known=true;
    };

    if (!(the_symbol_is_known)) {
       num_payload_symbols++;
    };    
  };

  x_c.set_size(length_ix_all,num_payload_symbols);
  x_cp.set_size(length_ix_all,num_payload_symbols);


}


void OFDM2::demodulate_multi_antenna(const cmat &waveform,vec &soft_bit, uint32_t start_sample, int modulation_order,int num_symbols_to_extract){

    cvec waveform_temp(Nfft);
    double noise_variance, noise_variance_norm;
    vec noise_variance_vec; // Noise variance estmimated during pilot.
    vec noise_variance_SINR; // Noise variance estmimated when there is
                             // no signal. Only for benchmarking
    vec soft_temp;
    cvec symbolsc(Nfft);
    //bool the_symbol_is_known;
    //int32_t num_payload_symbols;
    double D=0.0, DI=0.0;
    
    noise_variance_vec.set_length(waveform.rows());
    // Estimate noise variance for analysis purposes



    if (estimate_SINR) {
      noise_variance_SINR.set_length(waveform.rows());
      int Nsymb;
      int start_ix=noise_estimation_start_ix;
      int stop_ix;
      
      for (int i2=0;i2<waveform.rows();i2++) {
        noise_variance_SINR(i2)=0;
        start_ix=noise_estimation_start_ix;
        stop_ix=start_ix+Nfft;
        Nsymb=0;
        while (stop_ix<noise_estimation_stop_ix) {
          waveform_temp=(waveform.get_row(i2)).mid(start_ix,Nfft);
          pz_fft(waveform_temp,symbolsc);
          noise_variance_SINR(i2)+=sum(sqr(symbolsc(ix_all)))/length_ix_all;
          start_ix=stop_ix+1;
          stop_ix=start_ix+Nfft;
          Nsymb++;
        };
        noise_variance_SINR(i2)=noise_variance_SINR(i2)/Nsymb;
      };
    };
    

    if (modulation_order==0) return;

    

    // Estimate the vector channel of desired signal and interferers
    for (int i2=0;i2<he.rows();i2++) {
       double dummy;
       

       int s=start_of_known_symbol()(0)+start_sample;

       waveform_temp=(waveform.get_row(i2)).mid(s,Nfft);
       estimate_channel(waveform_temp,noise_variance_norm,
                        h,0,noise_variance);

       //if (i2==0)
         //std::cout<< "waveform_temp=" << waveform_temp << "\n";


       noise_variance_vec(i2)=noise_variance;
       he.set_row(i2,h);

       for (int i1=0;i1<interferers_known_pos.length();i1++) {
         s=start_sample+interferers_known_pos(i1)*(Nfft+Np);
         

         waveform_temp=(waveform.get_row(i2)).mid(s,Nfft);
         estimate_channel(waveform_temp,dummy,h,0);
         hi[i1].set_row(i2,h);
       };
    };


    noise_variance=mean(noise_variance_vec);

    if (modulation_order==1) return;


    cmat Rs; // Structured covariance matrix
    //cmat Ru; // Un-structured covariance matrix
    cmat temp;
    cvec temp1;
    complex<double> hc;
    cvec w(waveform.rows());
    

    Rs.set_size(waveform.rows(),waveform.rows());
    //Ru.set_size(waveform.rows(),waveform.rows());

    temp.set_size(waveform.rows(),waveform.rows());
    temp1.set_length(waveform.rows());

    int aw=20; // Averaging window for the angles
    int aw_i=0; // Index into averaging window.
    cvec p10(waveform.rows()), p20(waveform.rows());
    std::complex<double> v1,v2,v3;
    int i10=0;
    cmat p1r(waveform.rows(),aw); // running average
    cmat p2r(waveform.rows(),aw); // running average
    p1r.zeros();
    p2r.zeros();


    int i4;
    int i3;

    // Make an average over the aw first symbols

    i10=0;
    p10.zeros();
    p20.zeros();
    int is=0; // Number of collected samples

    {
      int i1=0;
      while (is<aw) {
        if (!(i1==known_symbol_pos(0))) {
          for (i3=0;i3<waveform.rows();i3++) {
            i4=(Nfft+Np)*d_reorder(i1)+start_sample;
            v1=0;
            v2=0;
            for (int i5=0;i5<(int) Nfft;i5++) {
              v1=v1+Wfft(pilot_carrier1,i5)*waveform(i3,i4);
              v2=v2+Wfft(pilot_carrier2,i5)*waveform(i3,i4);
              i4++;
            }; // i5
            //if ((i3==0) && (is<2))
            //  std::cout << "v1=" << v1 ;
            p10(i3)=p10(i3)+v1;
            p20(i3)=p20(i3)+v2;
            
          };
          is++;
        };
        i1++;
      };
    };

    i10=0;
    std::complex<double> c1,c2;
    for (int i1=0;i1<(int) num_symbols_to_extract+1;i1++) {


        if (!(i1==known_symbol_pos(0))) {
         for (i3=0;i3<waveform.rows();i3++) {
           i4=(Nfft+Np)*d_reorder(i1)+start_sample;
           v1=0;
           v2=0;
           for (int i5=0;i5<(int) Nfft;i5++) {
             v1=v1+Wfft(pilot_carrier1,i5)*waveform(i3,i4);
             v2=v2+Wfft(pilot_carrier2,i5)*waveform(i3,i4);
             i4++;
           }; // i5

           p1r(i3,aw_i)=v1;
           p2r(i3,aw_i)=v2;

           //if ((i10==0) && (i3==0))
           //  std::cout << "v1=" << v1 << "\n";


           if (i10>=aw-1) {
             

             c1=p10(i3)/mean(p1r.get_row(i3));
             c1=c1/abs(c1);
             c2=p20(i3)/mean(p2r.get_row(i3));
             c2=c2/abs(c2);
             a1c(i3,i10)=c1;
             a2c(i3,i10)=c2;

             //std::cout << "p1r.get_row(i3)=" << p1r.get_row(i3) << std::endl;
             //std::cout << "c1=" << c1 << std::endl;



           } else {
             c1=1;
             c2=1;
             a1c(i3,i10)=c1;
             a2c(i3,i10)=c2;
           };
                   
         }; // i3
         aw_i++;
         aw_i=aw_i % aw;

         i10++;
       }; // known_symbol_pos
    }; // i1
    
 
    cmat *ac;
    
    for (uint32_t i2=0;i2<length_ix_all;i2++) {

      //Ru.zeros();
      //for (int i1=0;i1<num_payload_symbols;i1++) {
      //        temp1=(x[i1]).get_col(i2);
      //        Ru+=outer_product(temp1,temp1,true);
      //};
      Rs=outer_product(he.get_col(i2),he.get_col(i2),true);
      for (int i1=0;i1<interferers_known_pos.length();i1++) {
        Rs=Rs+outer_product(hi[i1].get_col(i2),hi[i1].get_col(i2),true);
      };
      //Rs=Rs+diag(noise_variance_vec);
      complex<double> cnoise_variance=d_noise_variance_receiver;
      Rs=Rs+eye_c(Rs.rows())*cnoise_variance;
      w=inv(Rs)*he.get_col(i2);
      w=w/norm(w,2); 

      if (false) { //(estimate_SINR) {
        double d;
        DI+=abs(elem_mult_sum(conj(w),(Rs+
                         diag(noise_variance_SINR-noise_variance_vec))*w));
        d=abs(elem_mult_sum(conj(w),he.get_col(i2)));
        D+=d*d;
      };
      
      h(i2)=elem_mult_sum(he.get_col(i2),conj(w));


      if (closest_pilot_all(i2)==1)
        ac=&a1c;
      else 
        ac=&a2c;


      i10=0;
      for (int i1=0;i1<(int) num_symbols_to_extract+1;i1++) {
                      
        if (i1!=known_symbol_pos(0)) {

          
          #if 1
          int i4;
          v2=0;
          for (int i3=0;i3<waveform.rows();i3++) {
            i4=(Nfft+Np)*d_reorder(i1)+start_sample;
            v1=0;
            for (int i5=0;i5<(int) Nfft;i5++) {
              v1=v1+Wfft(ix_all(i2),i5)*waveform(i3,i4);
              i4++;
            };
            v2=v2+v1*conj(w(i3))*((*ac)(i3,i10));
            //v2=v2+v1*conj(w(i3));
          };
          x_c(i2,i10)=v2;
          #endif

          i10++;
          
        };

        
       
        
      };

    };

    cpe00.set_length(3);
    v1=0; v2=0;
    for (int i1=0;i1<aw;i1++) {
      v1=v1+x_c(pilot_carrier1_sub,i1);
      v2=v2+x_c(pilot_carrier2_sub,i1);
    };
    v1=pilot_symbol/v1;
    v2=pilot_symbol/v2;
    v1=v1/abs(v1);
    v2=v2/abs(v2);
    cpe00(1)=v1;
    cpe00(2)=v2;     

    if (estimate_SINR) {
       SINR=D/(DI-D);
    };

    if (modulation_order==2) return;

    #if 0    
    cvec symbol;
    symbol.set_length(length_ix_all);
    soft_temp.set_length(length_ix);
    int bits_per_symbol=log2(modulation_order);

    // Re-estimate noise variance
    double noise_variance_temp;
    noise_variance=0;
    for (int i1=0;i1<num_payload_symbols;i1++) {
       symbol=x_c.get_col(i1);
       noise_variance_temp=0;
       soft_output(symbol,noise_variance_temp,modulation_order,soft_temp);
       noise_variance+=noise_variance_temp;
    };
    noise_variance/=num_payload_symbols;


    // Soft-value extraction
    for (int i1=0;i1<num_payload_symbols;i1++) {
       symbol=x_c.get_col(i1);
       soft_output(symbol,noise_variance,modulation_order,soft__temp);
       soft_bit.replace_mid(i1*length_ix*bits_per_symbol,soft_temp);
    };
    #endif
};

void OFDM2::extract_multi_antenna(const cmat &waveform,
                         uint32_t start_sample,int num_symbols_to_extract){

  // When calling this function the following members will be set
  // x_c : A matrix which contains the combined signal (#subcarrier,#symbol)
  // h : Channel estimate including combiner #subcarriers
  // x[#symbols]: The raw complex samples. Each matrix (#subcarrier,#symbol).
  // he: Channel estimate for the desired signal (#antenna,#subcarrier)
  // hi[#interferers]: Channel estimate of interfering signals.
  
  vec dummy;

  demodulate_multi_antenna(waveform,dummy,start_sample,2,
                           num_symbols_to_extract);
};

void OFDM2::init_compressed_feedback(int b_psis,int b_phis, uint32_t num_ms_ant, uint32_t num_tx_antennas, int Ng, double noise_variance_per_subcarrier, bool IA_compression) {
  d_b_psis=b_psis;
  d_b_phis=b_phis;
  d_num_ms_ant=num_ms_ant;
  d_num_tx_antennas=num_tx_antennas;
  d_Ng=Ng;
  d_IA_compression=IA_compression;

  d_noise_variance_per_subcarrier=noise_variance_per_subcarrier;
  psis_res=pi/(pow2(b_psis+1));
  // psis_codebook=(0.5+linspace(0,pow2(b_psis)-1,pow2(b_psis)))*psis_res;
  phis_res=pi/(pow2(b_phis-1));
  //phis_codebook=(0.5+linspace(0,pow2(b_phis)-1,pow2(b_phis)))*phis_res;

  n_psis_codebook=pow2(b_psis);
  n_phis_codebook=pow2(b_phis);

  //std::cout << "phis_codebook=" << phis_codebook << std::endl;
  //std::cout << "phis_res=" << phis_res << std::endl;

  //for (int i1=0;i1<phis_codebook.size();i1++) {
  //  if (phis_codebook(i1)>pi)
  //    phis_codebook(i1)=phis_codebook(i1)-2*pi;
  //};


  // scwcf : subcarriers with compressed feedback (IEEE notation),
  // scwds : subcarriers with delta SNR feedback
  // scwcf_low: The subcarriers from scwcf_low(i1) to scwcf_high(i)
  // scwcf_high: use primarily scwcf(i1) for channel estimation.
  // scwcf_has_ds: scwcf_has_ds(i1)=1 if there is an i2 such 
  //               scwcf(i1)=scwds(i2).
  // scwcf_ds_low: Closest delta SNR measurement downwards or equal 
  // scwcf_ds_high Closest delta SNR measurement upwards or equal (or -1000 if not existent)
  // scwcf_ix_all_sub: Index in ix_all 
  // scwcf_ix_all_low: Index in ix_all corresponding to scwcf_low
  // scwcf_ix_all_high: Index in ix_all corresponding to scwcf_high


  scwcfs[0]="-19, -18, -17, -16, -15, -14, -13, -12, -11, -10, -9, -8,"
             "-6, -5, -4, -3, -2, -1, 1, 2, 3, 4, 5, 6, 8, 9, 10, 11," 
             "12, 13, 14, 15, 16, 17, 18, 19";
  scwcf_lows[0]="-19, -18, -17, -16, -15, -14, -13, -12, -11, -10, -9, -8,"
             "-7, -5, -4, -3, -2, -1, 1, 2, 3, 4, 5, 6, 7, 9, 10, 11," 
             "12, 13, 14, 15, 16, 17, 18, 19";
  scwcf_highs[0]="-19, -18, -17, -16, -15, -14, -13, -12, -11, -10, -9, -8,"
             "-6, -5, -4, -3, -2, -1, 1, 2, 3, 4, 5, 6, 8, 9, 10, 11," 
             "12, 13, 14, 15, 16, 17, 18, 19";

  scwdss[0]="-18, -16, -14, -12, -10, -8, -6, -4, -2, -1, 1, 2, 4, 6,"  
             "8, 10, 12,14, 16, 18";


 scwcfs[1]=    "-18, -16, -14, -12, -10, -8, -6, -4, -2, -1, 1, 2, 4, 6,"  
             "8, 10, 12, 14, 16, 18";
 scwdss[1]="-16, -12, -8, -4, -1, 1, 4, 8, 12, 16";     
        
 scwcf_lows[1]="-19, -17, -15, -13, -11, -9, -7, -5, -3, -1, 1, 2, 3, 5,"
          "7, 9, 11, 13,  15, 17";
 scwcf_highs[1]="-18, -16, -14, -12, -10, -8, -6, -4, -2, -1, 1, 2, 4, 6,"
          "8, 10, 12, 14,  16, 19";


 scwdss[3]="-12, -8, -4, -1, 1, 4, 12";
 scwcfs[3]="-16, -12, -8, -4, -1, 1, 4, 8, 12, 16";
 scwcf_lows[3]="-19, -14,-10, -6, -2, 1, 3, 6, 10, 14";
 scwcf_highs[3]="-15, -11, -7, -3, -1, 2, 5, 9, 13, 19";

 scwcfs[7]=    "-16, -8,  -1, 1, 8, 16";
 scwcf_lows[7]="-19,-12,  -4, 1, 5, 12";
 scwcf_highs[7]="-13, -5,  -1, 4, 11,19";
 scwdss[7]="-8, -1, 1, 8";


 scwcfs[15]="-16,-1,1,16";
 scwcf_lows[15]="-19,-8,1,9";
 scwcf_highs[15]=" -9,-1,8,19";
 scwdss[15]="-1,1,";



 scwcfs[16]="-10 10";
 scwcf_lows[16]="-19 1";
 scwcf_highs[16]="-1 19";
 scwdss[16]="-10 10";

 scwcfs[17]="1";
 scwcf_lows[17]="-19";
 scwcf_highs[17]="19";
 scwdss[17]="1,";


   ivec Nss="1,2,4,8,16,17,18";
   int Nsi=0;
   for (int Nss_i=0;Nss_i<Nss.length();Nss_i++) {
     Nsi=Nss(Nss_i)-1;


     scwcf_ix_all_lows[Nsi]=-1000*ones_i(scwcfs[Nsi].length());
     scwcf_ix_all_highs[Nsi]=-1000*ones_i(scwcfs[Nsi].length());
     scwcf_ix_alls[Nsi]=-1000*ones_i(scwcfs[Nsi].length());
     scwcf_ds_lows[Nsi]=-1000*ones_i(scwcfs[Nsi].length());
     scwcf_ds_highs[Nsi]=-1000*ones_i(scwcfs[Nsi].length());

     for (int i1=0;i1<scwdss[Nsi].size();i1++) {
       if ((scwdss[Nsi](i1)<leftmost_carrier_IEEE) | (scwdss[Nsi](i1)>rightmost_carrier_IEEE)) {
         scwdss[Nsi].del(i1,i1);
       }
     };
          
     for (int i1=0;i1<scwcfs[Nsi].size();i1++) {



       bool has_ds=false;
       int ie=scwcfs[Nsi](i1);
       int i4=ie;
       if (i4<0)
         i4=i4+Nfft;
     
     
       int i4_low=scwcf_lows[Nsi](i1);
       while (i4_low<leftmost_carrier_IEEE)
         i4_low++;
     
       while (i4_low>rightmost_carrier_IEEE)
         i4_low--;
     
       

       if (i4_low<0)
         i4_low=i4_low+Nfft;

       int i4_high=scwcf_highs[Nsi](i1);
       while (i4_high<leftmost_carrier_IEEE)
         i4_high++;
       while (i4_high>rightmost_carrier_IEEE)
         i4_high--;

       if (i4_high<0)
         i4_high=i4_high+Nfft;

       if (scwcfs[Nsi].length()==1) {
         i4_low=ix_all(0);
         i4_high=ix_all(length_ix_all-1);
       };

       for (int i2=0;i2<(int) length_ix_all;i2++) {
         if (ix_all(i2)==i4) {
           scwcf_ix_alls[Nsi](i1)=i2;
         };
         if (ix_all(i2)==i4_low) {
           scwcf_ix_all_lows[Nsi](i1)=i2;
         };
         if (ix_all(i2)==i4_high) {
           scwcf_ix_all_highs[Nsi](i1)=i2;
         };

       };


       int i2;
       for (i2=0;i2<scwdss[Nsi].size();i2++) {
         if (scwcfs[Nsi](i1)==scwdss[Nsi](i2)) {
           has_ds=true;
           break;
         };
       };

       scwcf_has_dss[Nsi].ins(scwcf_has_dss[Nsi].length(),has_ds);
       if (has_ds) {
         scwcf_ds_lows[Nsi](i1)=i2;
         scwcf_ds_highs[Nsi](i1)=i2;
       } else {
        int i2_low=-1000; 
        for (i2=0;i2<scwdss[Nsi].size();i2++) {
          if (scwdss[Nsi](i2)<scwcfs[Nsi](i1)) {
            i2_low=i2;
          };
        };

        scwcf_ds_lows[Nsi](i1)=i2_low;
        int i2_high=-1000;
        for (i2=scwdss[Nsi].size()-1;i2>=0;i2--) {
          if (scwdss[Nsi](i2)>scwcfs[Nsi](i1)) {
            i2_high=i2;
          };
        };
        scwcf_ds_highs[Nsi](i1)=i2_high;
       };

     };
   
     


     d_num_delta_SNRs[Nsi]=0;
     for (int i1=0;i1<scwdss[Nsi].size();i1++) {
       if ((scwdss[Nsi](i1)>=leftmost_carrier_IEEE) && (scwdss[Nsi](i1)<=rightmost_carrier_IEEE)) {       
         d_num_delta_SNRs[Nsi]++;
       };
     };

     
   };


};


void OFDM2::unpackV(ivec &psis,ivec &phis, cmat &V){
  int i_psis=0;
  int i_phis=0;
  double phi, psi, c, s;
  std::complex<double> t1,t2;


  V.zeros();
  for (int i1=0;i1<d_num_ms_ant;i1++){
    V(i1,i1)=1;
  };
      
  for (int l1=d_num_ms_ant-1;l1>=0;l1--) {     
     for (int l2=d_num_tx_antennas  -1;l2>=l1+1;l2--) {
       

       //psi=psis_codebook(psis(i_psis));
        psi=psis_res*(psis(i_psis)+0.5);


        c=cos(psi);
        s=-sin(psi);

        t1=V(l1,l1);
        t2=V(l2,l1);
        V(l1,l1)=c*t1+s*t2;
        V(l2,l1)=-s*t1+c*t2;            
        for (int i4=l1+1;i4<(int) d_num_ms_ant;i4++) {   
          t1=V(l1,i4);
          t2=V(l2,i4);
          V(l1,i4)=c*t1+s*t2;
          V(l2,i4)=-s*t1+c*t2;
        };           
        i_psis++;
     };

     for (int l3=d_num_tx_antennas-2;l3>=l1;l3--) {
       //phi=phis_codebook(phis(i_phis));
       phi=phis(i_phis);
       i_phis++;
       if (phi>n_phis_codebook/2)
         phi=phi-n_phis_codebook;
       phi=(phi+0.5)*phis_res;


       if (d_IA_compression) {
          if (l1==0) {
            if (l3==0) {
              phi=0;
            };
            if (l3==d_num_tx_antennas/3) {
              psi=0;
            };
          };
       };  

       std::complex<double> c=std::complex<double> (cos(phi),-sin(phi));
       for (int l4=0;l4<(int) d_num_ms_ant;l4++) {
         V(l3,l4)=c*V(l3,l4);
       };
     };

  };  
  
};

void OFDM2::packV(ivec &psis,ivec &phis, const cmat &V){

  cmat Vp=V;
  // Number of phis angles
  // Nphis=\sum_{i1=0}^{Nr-1} (Nc-1-i1) = (Nc-1)Nr-\sum_{i1=0}^{Nr-1} i1
  // = (Nc-1)Nr-Nr/2*(0+Nr-1)=(Nc-1)Nr-Nr^2/2+Nr/2=NcNr-Nr^2/2-Nr/2
 
  // Number of psis angles
  // Npsis=\sum_{i1=0}^{Nr-1} (Nc-(i1+1))  = Nr/2*(Nc-1+Nc-(Nr-1+1))
  // = Nr/2*(Nc-1+Nc-Nr)=Nr/2*(2Nc-1-Nr)=NcNr-Nr^2/2-Nr/2


  if (d_IA_compression) {

    #if 1
    double phi=arg(Vp(0,0));
    std::complex<double> c=std::complex<double> (cos(phi),-sin(phi));
    for (int i3=0;i3<d_num_ms_ant;i3++) {
      for (int i4=0;i4<d_num_tx_antennas/3;i4++) {
        Vp(i4,i3)=c*Vp(i4,i3);
      };
    };
    phi=arg(Vp(d_num_tx_antennas/3,0));
    c=std::complex<double> (cos(phi),-sin(phi));
    for (int i3=0;i3<d_num_ms_ant;i3++) {
      for (int i4=0;i4<d_num_tx_antennas/3;i4++) {
        Vp(i4+d_num_tx_antennas/3,i3)=c*Vp(i4+d_num_tx_antennas/3,i3);
      };
    };
    #endif


  };


  // Rotation angles
  for (int i1=0;i1<d_num_ms_ant;i1++){ // Column    

     vec phisa=-angle(Vp.get_col(i1));

     #if 0
     for (int i2=i1;i2<d_num_tx_antennas-1;i2++) {
          //result.phis.push_back(min_index(abs(phis(i2)-phis_codebook)));
         phis.ins(phis.length(),min_index(abs(phisa(i2)-phis_codebook)));
         //std::cout << "phis=" << phis(phis.length()-1) << "\n";
     };
     #endif
     #if 1
     int min_ixa;
     for (int i2=i1;i2<d_num_tx_antennas-1;i2++) {
       //std::cout << "i2=" << i2 << std::endl;
       if (phisa(i2)<0) {
         min_ixa=(phisa(i2)/phis_res-1.0);
         min_ixa=min_ixa+n_phis_codebook;//phis_codebook.length();
       }
       else {
         min_ixa=(phisa(i2)/phis_res);
       };
         
       //std::cout << "min_ixa=" << min_ixa << std::endl;
       phis.ins(phis.length(),min_ixa);
     }


     #endif



      
     for (int l2=i1;l2<d_num_tx_antennas-1;l2++) {
        std::complex<double> c=std::complex<double> 
          (cos(phisa(l2)),sin(phisa(l2)));      
        for (int l1=0;l1<d_num_ms_ant;l1++){ // Column
          Vp(l2,l1)=Vp(l2,l1)*c;
        };
     };     


      
    for (int i2=i1+1;i2<d_num_tx_antennas;i2++) {
      
      double psi_i2_i1;
                
      if (abs(Vp(i1,i1))<1e-5)
         psi_i2_i1=pi/2;
      else
         psi_i2_i1=atan(real(Vp(i2,i1)/Vp(i1,i1)));
        
      
      #if 0
      psis.ins(psis.length(),min_index(abs(psi_i2_i1-psis_codebook)));
      #endif
      
      #if 1
      int min_ix=(psi_i2_i1/psis_res);
      psis.ins(psis.length(),min_ix);
      #endif

        double c=cos(psi_i2_i1);
        double s=sin(psi_i2_i1);
        std::complex<double> t1,t2;
        
        t1=Vp(i1,i1);
        t2=Vp(i2,i1);
        Vp(i1,i1)=c*t1+s*t2;
        Vp(i2,i1)=-s*t1+c*t2;            
        for (int i4=i1+1;i4<(int) d_num_ms_ant;i4++) {   
           t1=Vp(i1,i4);
           t2=Vp(i2,i4);
           Vp(i1,i4)=c*t1+s*t2;
           Vp(i2,i4)=-s*t1+c*t2;
        };

      }; // for i2

    }; // for i1


};

void OFDM2::uncompress_feedback(cmat H[],const OFDM2::compressed_feedback_IEEE80211ac &feedback) {

  uint32_t num_of_tx_antennas;
  uint32_t num_ms_ant;



  num_ms_ant=d_num_ms_ant;
  num_of_tx_antennas=d_num_tx_antennas;


  /* Unpack average (bar) SNRs first */
  vec bar_SNR(num_ms_ant);
  for (int i1=0;i1<(int) num_ms_ant;i1++) {
    bar_SNR(i1)=(feedback.bar_SNRs[i1]+128)*0.25-10;
  };
  /* Combine with delta SNRs to get SNR per carrier with interpolation */



  //mat SNR(num_ms_ant,scwds.length());
  mat SNR(num_ms_ant,scwdss[d_Ng-1].length());
  vec SNR_sub(num_ms_ant); // SNR on a certain subcarrier in dB
  //vec SNR_sub_start(num_ms_ant); // SNR on subcarrier for which delta_SNR is defined.
  //vec SNR_sub_stop(num_ms_ant); // SNR on subcarrier for which delta_SNR is defined.
  int ix_delta_SNRs=0; // Reading index from feedback.delta_SNRs
  

  for (int i1=0;i1<d_num_delta_SNRs[d_Ng-1];i1++) {
  //for (int i1=0;i1<d_num_delta_SNR;i1++) {
    for (int i2=0;i2<(int) num_ms_ant;i2++) {  // Streams per subcarrier
      SNR(i2,i1)=feedback.delta_SNRs[ix_delta_SNRs++]+bar_SNR(i2);
    };
  };


      
  /* Unpack phis and psis abd combine with interpolation*/
  
  int Na_phis=feedback.Na_phis;
  int Na_psis=feedback.Na_psis;
  ivec phis(Na_phis);
  ivec psis(Na_psis);

  cmat V(num_of_tx_antennas,num_ms_ant);
  int counter=0;

  //for (int i1=0;i1<scwcf.length();i1++) { 
  for (int i1=0;i1<scwcfs[d_Ng-1].length();i1++) { 

    //int ie=scwcf(i1);
    int ie=scwcfs[d_Ng-1](i1);

     if ((ie<leftmost_carrier_IEEE) || (ie>rightmost_carrier_IEEE)) {
        continue;
     };       

          
     for (int i2=0;i2<Na_phis;i2++) {
         phis(i2)=feedback.phis(Na_phis*counter+(Na_phis-1-i2));
     };

     for (int i2=0;i2<Na_psis;i2++) {
         psis(i2)=feedback.psis(Na_psis*counter+(Na_psis-1-i2));
     };

     counter++;
     

     unpackV(psis,phis,V);
          
     if (scwcf_has_dss[d_Ng-1](i1)) {
     //if (scwcf_has_ds(i1)) {
        for (int i2=0;i2<(int) num_ms_ant;i2++) {
          //SNR_sub(i2)=SNR(i2,scwcf_ds_low(i1));
          SNR_sub(i2)=SNR(i2,scwcf_ds_lows[d_Ng-1](i1));
        };
     } else {
                   
       //if (scwcf_ds_low(i1)==-1000) {
       if (scwcf_ds_lows[d_Ng-1](i1)==-1000) {
          
          for (int i2=0;i2<(int) num_ms_ant;i2++) {
            //SNR_sub(i2)=SNR(i2,scwcf_ds_high(i1));
            SNR_sub(i2)=SNR(i2,scwcf_ds_highs[d_Ng-1](i1));
          };
        };
       //if (scwcf_ds_high(i1)==-1000) {
       if (scwcf_ds_highs[d_Ng-1](i1)==-1000) {
          for (int i2=0;i2<(int) num_ms_ant;i2++) {
            //SNR_sub(i2)=SNR(i2,scwcf_ds_low(i1));
            SNR_sub(i2)=SNR(i2,scwcf_ds_lows[d_Ng-1](i1));
          };
        };
       #if 0
       if ((scwcf_ds_high(i1)>-1000) && (scwcf_ds_low(i1)>-1000)) {       
          double d=(scwcf(i1)-scwds(scwcf_ds_low(i1)))/(scwds(scwcf_ds_high(i1))-scwds(scwcf_ds_low(i1)));
          for (int i2=0;i2<(int) num_ms_ant;i2++) {
            SNR_sub(i2)=SNR(i2,scwcf_ds_low(i1))+d*(SNR(i2,scwcf_ds_high(i1))-SNR(i2,scwcf_ds_low(i1)));
          };      
        };
       #endif
       if ((scwcf_ds_highs[d_Ng-1](i1)>-1000) && (scwcf_ds_lows[d_Ng-1](i1)>-1000)) {       
          double d=(scwcfs[d_Ng-1](i1)-scwdss[d_Ng-1](scwcf_ds_lows[d_Ng-1](i1)))/(scwdss[d_Ng-1](scwcf_ds_highs[d_Ng-1](i1))-scwdss[d_Ng-1](scwcf_ds_lows[d_Ng-1](i1)));
          for (int i2=0;i2<(int) num_ms_ant;i2++) {
            SNR_sub(i2)=SNR(i2,scwcf_ds_lows[d_Ng-1](i1))+d*(SNR(i2,scwcf_ds_highs[d_Ng-1](i1))-SNR(i2,scwcf_ds_lows[d_Ng-1](i1)));
          };      
        };

     };


     cmat Ht(num_ms_ant,num_of_tx_antennas);
     for (int i2=0;i2<(int) num_ms_ant;i2++) {    
       double e=sqrt(exp(log(10)*0.1*SNR_sub(i2))*
                     d_noise_variance_per_subcarrier);
         for (int i4=0;i4<(int) num_of_tx_antennas;i4++) {
           Ht(i2,i4)=e*conj(V(i4,i2));
         };      
     };

          

     for (int i2=0;i2<(int) num_ms_ant;i2++) {
        for (int i4=0;i4<(int) num_of_tx_antennas;i4++) {
          //for (int i5=scwcf_ix_all_low(i1);i5<=scwcf_ix_all_high(i1);i5++) {
          for (int i5=scwcf_ix_all_lows[d_Ng-1](i1);i5<=scwcf_ix_all_highs[d_Ng-1](i1);i5++) {
             H[i4](i2,i5)=Ht(i2,i4);
           };
        };
     };


   };   
      


};

OFDM2::compressed_feedback_IEEE80211ac OFDM2::compress_feedback(cmat H[]) {
  OFDM2::compressed_feedback_IEEE80211ac result;
  
  int num_ms_ant=d_num_ms_ant;
  int num_of_tx_antennas=d_num_tx_antennas;

  cmat Ht(num_ms_ant,num_of_tx_antennas);
  cmat U;
  vec s;
  cmat V;
  vec bar_SNR(num_ms_ant);
  ivec bar_SNR_i(num_ms_ant);
  bar_SNR.zeros();
  //mat delta_SNR(d_num_delta_SNR,num_ms_ant);
  //imat delta_SNR_i(scwcf.size(),num_ms_ant);
  mat delta_SNR(d_num_delta_SNRs[d_Ng-1],num_ms_ant);
 
  int num_SNR_estimates=0;

  int id=0; // Report Delta SNR subcarrier counter
  bool first_iteration=true;

  

  //for (int i1=0;i1<scwcf.length();i1++) { 
  for (int i1=0;i1<scwcfs[d_Ng-1].length();i1++) { 
  
    //int ie=scwcf(i1); // Index IEEE subcarrier numbering    
    int ie=scwcfs[d_Ng-1](i1); // Index IEEE subcarrier numbering    

    if ((ie<leftmost_carrier_IEEE) || (ie>rightmost_carrier_IEEE)) {
      continue;
    };  


    // Store MIMO matrix for subchannel ix in Ht
    for (int i3=0;i3<num_ms_ant;i3++) {
       for (int i2=0;i2<num_of_tx_antennas;i2++) {        
         //Ht(i3,i2)=(H[i2])(i3,scwcf_ix_all(i1));       
         Ht(i3,i2)=(H[i2])(i3,scwcf_ix_alls[d_Ng-1](i1));        
       };
    };
  

    #if 0
     cmat Ht="(-0.2130,0.1766),(-1.0431,-0.4140),(-0.4381,2.0034),"   
      "(0.9835,-0.4320),(1.1437,-0.3601),(0.9726,1.4158);"
       "(-0.8657,0.9707),(-0.2701,-0.4383),(-0.4087,0.9510),"
       "(-0.2977,0.6489),(-0.5316,0.7059),(-0.5223,-1.6045)";
    #endif

    // Ht=USV'
    // HtV=US  (MxN)*(N,M) (M,M)
    // 

    #if 0
    svd(Ht,U,s,V);  
    V=V.get_cols(0,num_ms_ant-1);
    #endif

    #if 1
    eig_sym(hermitian_transpose(Ht)*Ht,s,V);
    V=V.get_cols(num_of_tx_antennas-num_ms_ant,num_of_tx_antennas-1);
    #endif

   
    // Make last row real:
    #if 0
    cvec v_row=V.get_row(num_of_tx_antennas-1);
    v_row=elem_div(conj(v_row),to_cvec(abs(v_row)));
    V=V*diag(v_row);
    #endif
    //std::complex<double> c=std::complex<double> (cos(phi),-sin(phi));
    #if 1
    std::complex<double> c;
    for (int i2=0;i2<V.cols();i2++) {
      c=V(num_of_tx_antennas-1,i2);
      c=conj(c)/abs(c);
      for (int i3=0;i3<V.rows();i3++) {
        V(i3,i2)=V(i3,i2)*c;
      };
    };
    #endif




    // SNR per "stream"    
    // Ht=U

    //vec SNR_subc=diag(abs(hermitian_transpose(U)*Ht*V));
    vec SNR_subc=diag(abs(Ht*V));
    SNR_subc=elem_mult(SNR_subc,SNR_subc);
    SNR_subc=SNR_subc/d_noise_variance_per_subcarrier;
    SNR_subc=10*log10(SNR_subc);
    bar_SNR=bar_SNR+SNR_subc; // Average over streams
    num_SNR_estimates++;


    // Compress V
    packV(result.psis,result.phis,V);

 
 
    // Save delta_SNR if applicable 
    //if (scwcf_has_ds(i1)) { 
    if (scwcf_has_dss[d_Ng-1](i1)) { 
       delta_SNR.set_row(id,SNR_subc);
       id=id+1;
     };

     if (first_iteration) {
       result.Na_psis=result.psis.size();
       result.Na_phis=result.phis.size();
       first_iteration=false;
     };
            

  }; // for i1


   

  // Average SNR per stream encoding
  bar_SNR=bar_SNR/((double) num_SNR_estimates);
  double max_bar_SNR=max(bar_SNR);
  double compensate_SNR;
  if (max_bar_SNR>51.120) 
    compensate_SNR=51.120-max_bar_SNR;
  else
    compensate_SNR=0;

  //compensate_SNR=0;

 
  for (int i1=0;i1<num_ms_ant;i1++) {    
     bar_SNR(i1)=bar_SNR(i1)+compensate_SNR;
  };


  bar_SNR_i=to_ivec(floor(4*bar_SNR+40+0.5));
  for (int i1=0;i1<num_ms_ant;i1++) {    
    if (bar_SNR_i(i1)<0)
      bar_SNR_i(i1)=-128;
    else if (bar_SNR_i(i1)>127+128)
      bar_SNR_i(i1)=127;
    else
      bar_SNR_i(i1)=bar_SNR_i(i1)-128;    
    
    result.bar_SNRs.push_back(bar_SNR_i(i1));
  };


  //for (int i3=0;i3<d_num_delta_SNR;i3++) {    
  for (int i3=0;i3<d_num_delta_SNRs[d_Ng-1];i3++) {    
     for (int i4=0;i4<num_ms_ant;i4++) {
        delta_SNR(i3,i4)=delta_SNR(i3,i4)-bar_SNR(i4)+compensate_SNR;
        int snr_ix=floor(delta_SNR(i3,i4)+0.5);
        if (snr_ix>7)
          snr_ix=7;
        else if (snr_ix<-8)
          snr_ix=-8;
        result.delta_SNRs.push_back(snr_ix);
      };
  };

  return result;
}


void OFDM2::extract_multi_antenna(const cmat &waveform,
         uint32_t start_sample, cvec &symbols_out, int num_symbols_to_extract){

  // As the short version but also including equalization and estimation of the noise
  // in the equalized samples
  
  vec dummy;
  cvec signal(length_ix_all);
  cvec cpec(3);
  std::complex<double> cpec1,cpec2;

demodulate_multi_antenna(waveform,dummy,start_sample,2,num_symbols_to_extract);


  uint32_t i3=0;  
  //std::cout << "h=" << h << "; \n";
  //std::cout << "x_c=" << x_c << "; \n";
  cpec=cpe00;

  for (int i1=0;i1<x_c.cols();i1++) {

    elem_div_out( x_c.get_col(i1),h,signal);

    if (do_use_pilot_carriers) {
          // Common phase error correction (CPEC)
          //cpec=pilot_symbol/(signal(pilot_carrier1_sub)/h(pilot_carrier1_sub)+
          //         signal(pilot_carrier2_sub)/h(pilot_carrier2_sub));
      
       if (per_symbol_phase_derotation) {
           cpec1=pilot_symbol/signal(pilot_carrier1_sub);
           cpec2=pilot_symbol/signal(pilot_carrier2_sub);       
           cpec1=cpec1/abs(cpec1);
           cpec2=cpec2/abs(cpec2);
           cpec1=1;
           cpec2=1;
           cpec(1)=cpec1;
           cpec(2)=cpec2;
        }; 
         
         //aw1(aw_ix)=signal(pilot_carrier1_sub);
         //aw2(aw_ix)=signal(pilot_carrier2_sub);
         
         
    };


    for (uint32_t i2=0;i2<length_ix;i2++) {
      symbols_out(i3)=signal(ix_sub(i2))*cpec(closest_pilot(i2));
      i3=i3+1;
    };
  };

  
};




void OFDM2::demodulate(const cvec &waveform,cvec &symbols_out, uint32_t start_sample, const cvec &h,  double &rms_signal)
{
   double power,temp;
    cvec symbolsc(Nfft);
    vec soft_temp;
    bool the_symbol_is_known;
    cvec signal(Nfft), demodulator_input;
    int i4; // Counter in symbols_out
    std::complex<double> cpec;
    int s;

    demodulator_input.set_length(length_ix);

    i4=0;
    power=0.0;
    cpec=1.0;
    
    for (int i1=0;i1<(int) Ns;i1++) {
        
      
       the_symbol_is_known=false;
       for (int i2=0;i2<known_symbol_pos.length();i2++) {
         if (i1==known_symbol_pos(i2))
           the_symbol_is_known=true;
       };

      if (!the_symbol_is_known) {

        s=(Nfft+Np)*d_reorder(i1)+start_sample;
        
        symbolsc=waveform.mid(s,Nfft);
        temp=norm(symbolsc);
        power=power+temp*temp;

        pz_fft(symbolsc,signal);

        //std::cout << "h1=" << h(pilot_carrier1_sub) << "\n";
        //std::cout << "s1=" << signal(pilot_carrier1) << "\n";
        //std::cout << "h2=" << h(pilot_carrier2_sub) << "\n";
        //std::cout << "s2=" << signal(pilot_carrier2) << "\n";
        //std::cout << "pilot_symbol=" << pilot_symbol << "\n";

        if (do_use_pilot_carriers) {
          // Common phase error correction (CPEC)
          //cpec=pilot_symbol/(signal(pilot_carrier1_sub)/h(pilot_carrier1_sub)+
          //         signal(pilot_carrier2_sub)/h(pilot_carrier2_sub));

          cpec=pilot_symbol/(signal(pilot_carrier1)/h(pilot_carrier1_sub)+
                     signal(pilot_carrier2)/h(pilot_carrier2_sub));

          cpec=cpec/abs(cpec);

        };
            
        elem_div_out(signal(ix_all),h,demodulator_input);


        if (do_use_pilot_carriers) 
          demodulator_input=demodulator_input*cpec;


        for (uint32_t i5=0;i5<length_ix;i5++) {
          symbols_out(i4)=demodulator_input(ix_sub(i5));
          i4++;
        };
      };  
    };
    power=power/((Ns-known_symbol_pos.length())*Nfft);
    rms_signal=sqrt(power);
}


void OFDM2::synchronize(cvec &waveform, uint32_t &start_pos,double &f_offset) {
    cvec de_spread(train.size());
    
    uint32_t Nlags;
    double max_value;
    int start_pos_int, max_index;

    Nlags=waveform.length()-train.length()+1;
    vec FrequencyIx(Nlags), Criterions(Nlags);

    for (uint32_t i1=0; i1<Nlags;i1 ++) {
      de_spread=elem_mult(waveform.mid(i1,train.length()),conj(train));
      max_value=max(abs(fft(de_spread,Nfft_sync)),max_index);
      FrequencyIx(i1)=max_index;
      Criterions(i1)=max_value;
    };

    max_value=max(Criterions,start_pos_int);
    start_pos=start_pos_int;
    f_offset=FrequencyIx(start_pos)/Nfft_sync;
    if (f_offset>0.5)
      f_offset=f_offset-1;

    f_offset=f_offset*25e6;
    
 
}

void OFDM2::synchronize(cvec &waveform, uint32_t &start_pos) {
    cvec de_spread(train.size());
    
    uint32_t Nlags;
    double max_value;
    int start_pos_int;

    Nlags=waveform.length()-train.length()+1;
    vec Criterions(Nlags);

    for (uint32_t i1=0; i1<Nlags;i1 ++) {
      de_spread=elem_mult(waveform.mid(i1,train.length()),conj(train));
      max_value=sum(abs(de_spread));
      Criterions(i1)=max_value;
    };

    max_value=max(Criterions,start_pos_int);
    start_pos=start_pos_int;
    
 
}

void OFDM2::interleave_symbols(cvec &symbols, int index) {

  cvec symbol;
  
  index=index % symbol_interleave_matrix.rows();
  symbol.set_length(length_ix);
  
  for (uint32_t i1=0;i1<Ns-known_symbol_pos.length();i1++) {
    symbol=symbols.mid(i1*length_ix,length_ix);
    symbol=symbol(symbol_interleave_matrix.get_row(index));
    symbols.set_subvector(i1*length_ix,symbol);
  };
 
};

void OFDM2::de_interleave_symbols(cvec &symbols, int index) {

  cvec symbol;
  
  index=index % symbol_de_interleave_matrix.rows();
  symbol.set_length(length_ix);
  
  for (uint32_t i1=0;i1<Ns-known_symbol_pos.length();i1++) {
    symbol=symbols.mid(i1*length_ix,length_ix);
    symbol=symbol(symbol_de_interleave_matrix.get_row(index));
    symbols.set_subvector(i1*length_ix,symbol);
  };
  

};