/**
 * Acquisition of GPS C/A Code Signal
 *    using TUSI (7.8) Acquisition by Circular Correlation
 * 
 * coded by fenrir.
 */
#include <iostream>
#include <strstream>

#include <vector>
#include <algorithm>
#include <bitset>

#include "GPS.h"
#include "util/util.h"
#include "param/complex.h"
#include "param/fft.h"

using namespace std;

typedef double real_t;
typedef Complex<real_t> complex_t;

#define IF_FREQ  4.309E6 // Hz
#define CLK_FREQ 5.714E6 // Hz
#define OUT_FREQ ((IF_FREQ < (CLK_FREQ / 2)) ? IF_FREQ : (CLK_FREQ - IF_FREQ))

/*
 * 5714         sampled data = 1     ms, resolution 1000  Hz => 21 step (+-10KHz)
 * 4096(2^{14}) sampled data = 0.717 ms, resolution 1395  Hz => 15 step (+-10KHz)
 * 8192(2^{15}) sampled data = 1.434 ms, resolution 697.5 Hz => 29 step (+-10KHz)
 */
 
#define DATA_SIZE_AQ 5714
//#define DATA_SIZE_AQ 8192
//#define DATA_SIZE_AQ 64

#define FREQ_RANGE_AQ 10E3

#define FREQ_STEP (CLK_FREQ / DATA_SIZE_AQ)

typedef struct{
  template <class T>
  Complex<T> operator()(const T &value){return Complex<T>(value);}
  template <class T>
  Complex<T> operator()(const Complex<T> &value){return value.conjugate();}
} Func_conj;

typedef struct{
  template <class T>
  T operator()(const T &value1, const T &value2){return value1 * value2;}
} Func_multi;

typedef struct{
  template <class T>
  T operator()(const T &value){return value;}
  template <class T>
  T operator()(const Complex<T> &value){return value.abs();}
} Func_abs;

int main(int argc, char *argv[]){
  
  char buf[64];
  strstream line(buf, sizeof(buf), strstream::in);
  
  vector<int> data;
  
  while(!cin.eof()){
    cin.getline(buf, sizeof(buf));
    buf[cin.gcount()] = '\0';
    //cout << buf << endl;
    
    int value;
    line.seekg(0);
    
    line >> value;
    data.push_back(value);
  }
  
  cout << "DATA Length: " << data.size() << endl;
  
  vector<real_t> data_aq(DATA_SIZE_AQ);
  copy(data.begin(), data.begin() + DATA_SIZE_AQ, data_aq.begin());
  //copy(data_aq.begin(), data_aq.end(), ostream_iterator<real_t>(cout, " "));
  //cout << endl;
  
  // 1. Perform the FFT on the data_aq, x(n) -> X(k)
  vector<complex_t> data_aq_fd(FFT::fft(data_aq));
  //copy(data_aq_fd.begin(), data_aq_fd.end(), ostream_iterator<complex_t>(cout, " "));
  //cout << endl;
  
  // 2. Take the conjugate, X(k) -> {X(k)}^{*}
  vector<complex_t> data_aq_star_fd(data_aq_fd.size());
  transform(data_aq_fd.begin(), data_aq_fd.end(), 
              data_aq_star_fd.begin(), 
              Func_conj());
  //copy(data_aq_star_fd.begin(), data_aq_star_fd.end(), ostream_iterator<complex_t>(cout, " "));
  //cout << endl;
  
  for(int sid = 1; sid < 37; sid++){
    
    cout << "SID: " << sid << endl;
    
    // 3. Generate local codes using eq.(7.1), l_{si}(n)
    // 3-(1) Generate chips.
    typedef struct chip_time_t{
      int chip;
      real_t time;
      chip_time_t(const int &_chip, const real_t &_time) : chip(_chip), time(_time) {}
    } ChipTime_t;
    vector<ChipTime_t> local_chips;
    
    CA_Code ca_code(sid);
    int ca_code_index(0);
    for(real_t time = 0; local_chips.size() < DATA_SIZE_AQ; time += (1. / CLK_FREQ)){
      int current_ca_code_index((int)(time / CA_Code::LENGTH_1CHIP));
      while(current_ca_code_index > ca_code_index){
        ca_code_index++;
        ca_code.next();
      }
      local_chips.push_back(ChipTime_t(ca_code.get_multi(), time));
    }
    
    for(int i = -(int)floor(FREQ_RANGE_AQ / FREQ_STEP); i <= (int)floor(FREQ_RANGE_AQ / FREQ_STEP); i++){
     
      // 3-(2) Multiply carrier(IF).
      vector<complex_t> local_signal_td;
      real_t f_i(OUT_FREQ + FREQ_STEP * i);
      cout << "FREQ index: " << i << "; FREQ: " << f_i << endl;
      for(vector<ChipTime_t>::iterator it = local_chips.begin();
            it < local_chips.end();
            it++){
        local_signal_td.push_back(iexp(2. * M_PI * f_i * (*it).time) * (*it).chip);
      }
      //copy(local_signal_td.begin(), local_signal_td.end(), ostream_iterator<complex_t>(cout, " "));
      //cout << endl;
      
      // 4. Perform FFT on local codes, l_{si}(n) -> L_{si}(k)
      vector<complex_t> local_signal_fd(FFT::fft(local_signal_td));
      
      // 5. Multiply {X(k)}^{*} and L_{si}(k), {X(k)}^{*} * L_{si}(k) -> R_{si}(k)
      vector<complex_t> correlation_fd(data_aq_star_fd.size());
      transform(data_aq_star_fd.begin(), data_aq_star_fd.end(),
                  local_signal_fd.begin(), 
                  correlation_fd.begin(), 
                  Func_multi());
                  
      // 6. Take the inverse FFT, R_{si}(k) -> r_{si}(n)
      vector<complex_t> correlation_td(FFT::ifft(correlation_fd));
      //copy(local_signal_fd.begin(), local_signal_fd.end(), ostream_iterator<complex_t>(cout, " "));
      //cout << endl;
      
      // 7. calc absolute values, |r_{si}(n)|
      vector<real_t> correlation_abs(correlation_td.size());
      transform(correlation_td.begin(), correlation_td.end(), 
                  correlation_abs.begin(), 
                  Func_abs());
      copy(correlation_abs.begin(), correlation_abs.end(), ostream_iterator<real_t>(cout, "\n"));
      cout << endl;
      
      vector<real_t>::iterator max_it(max_element(correlation_abs.begin(), correlation_abs.end()));
      cout << "Max: " << *max_it << " at " << (max_it - correlation_abs.begin()) << "." << endl;
    }
  }
    
  return 0;
}