/*
 * fmcw.c
 *
 *  Created on: 1 December 2017
 *          by: Daniel Sjoberg
 *
 *      compile with "gcc ABE_ADCDACPi.c fmcw.c -lm -o fmcw"
 *      run with "./fmcw"
 */

#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <complex.h>
#include <sys/time.h>
#include <unistd.h>

#include "ABE_ADCDACPi.h"  // AB Electronics library for AD/DA 


#define c0 299792458.0     // Speed of light in vacuum 
#define q 8                // Exponent in number of data bins 
#define N (1<<q)           // Number of data bins, N=2^q 
#define Npad 8             // Factor to expand data vector with
#define Nave 1             // Number of pulses to average between
#define Ngraph 80          // Number of output characters

#ifndef PI
# define PI	3.14159265358979323846264338327950288
#endif


void RemoveFunction(double complex *f, double complex *f0)
{
  int n;
  double complex A, B, C, D, a, b, tmp;
  A = 0.0 + 0.0*I;
  B = 0.0 + 0.0*I;
  C = 0.0 + 0.0*I;
  D = 0.0 + 0.0*I;
  for(n=0; n<N; n++) {
    A = A + f0[n]*f0[n];
    B = B + f0[n];
    C = C + f0[n]*f[n];
    D = D + f[n];
  }
  tmp = A*N - B*B;
  a = (N*C - B*D)/tmp;
  b = (-B*C + A*D)/tmp;
  for(n=0; n<N; n++) {
    f[n] = f[n] - a*f0[n] - b;
  }
  return;
}

void fft(double complex *v, int n, double complex *tmp )
{
  if(n>1) {			/* otherwise, do nothing and return */
    int k, m;
    double complex z, w, *vo, *ve;
    ve = tmp; vo = tmp + n/2;
    for(k=0; k<n/2; k++) {
      ve[k] = v[2*k];
      vo[k] = v[2*k+1];
    }
    fft( ve, n/2, v );		/* FFT on even-indexed elements of v[] */
    fft( vo, n/2, v );		/* FFT on odd-indexed elements of v[] */
    for(m=0; m<n/2; m++) {
      w = cos(2*PI*m/(double)n) - sin(2*PI*m/(double)n) * I;
      z = w * vo[m];
      v[m] = ve[m] + z;
      v[m + n/2] = ve[m] - z;
    }
  }
  return;
}

/* Compute threshold value for detection and index for maximum */
void detect(double complex *a, double *threshold, int *max_idx)
{
  double average, max_value;
  int n;
  average = 0.0;
  max_value = 0.0;
  *max_idx = 0;
  for(n=0; n<N*Npad; n++) {
    average = average + cabs(a[n])/N;
    if(cabs(a[n]) > max_value) {
      max_value = cabs(a[n]);
      *max_idx = n;
    }
  }
  *threshold = average*5;
  return;
}


int main(int argc, char **argv){
  double Isig, Qsig;
  double complex a1[N*Npad], a2[N*Npad], scratch[N*Npad], ramp[N], testv1[N], testv2[N];
  double f1, f2, f0, lambda0, B, V0, Rgate;
  double R, v, A1, A2;
  int m, n, p, Npulses;
  struct timeval t0start, t0stop, tstart, tstop;
  double T, T0, T1, T2;
  double fgate, df, delta_f1, delta_f2;
  int gate_idx, max_idx1, max_idx2;
  double threshold1, threshold2;
  char graphstring1[Ngraph+1], graphstring2[Ngraph+1];

  /* Set radar parameters */
  f1 = 24e9;           // Start frequency 
  f2 = 24.45e9;        // Stop frequency 
  f0 = (f1 + f2)/2;    // Center frequency 
  lambda0 = c0/f0;     // Center wavelength 
  B = f2 - f1;         // Absolute bandwidth 
  V0 = 2.048;          // Maximum output voltage from DAC 
  Rgate = 1.0;         // Range gate; blank out values below this 
  Npulses = 100;       // Number of pulses used for timing the program
  
  /* Initialize data */
  for(n=0; n<N; n++) {
    ramp[n] = (double)n/(double)N + 0.0*I;
  }
  for(n=0; n<Ngraph; n++) {
    graphstring1[n] = '.';
    graphstring2[n] = '.';
  }
  graphstring1[Ngraph] = '\0';  // Add trailing null character to string
  graphstring2[Ngraph] = '\0';  // Add trailing null character to string
  /* Fill testv[] with a function of known FFT, for testing purposes */
  for(n=0; n<N; n++) {
    testv1[n] = 0.125*(cos(2*PI*n/(double)N) - sin(2*PI*n/(double)N) * I);
    testv2[n] = 0.125*(cos(2*2*PI*n/(double)N) + sin(2*2*PI*n/(double)N) * I);
  }

  /* Initialize ADCDAC */
  if (open_dac() != 1){    // Open the DAC spi channel 
    exit(1);               // If the SPI bus fails to open exit the program 
  }
  if (open_adc() != 1){    // Open the DAC spi channel 
    exit(1);               // If the SPI bus fails to open exit the program 
  }
  set_dac_gain(1);         // This sets  the voltage range of 0 to 2.048V 
  set_dac_voltage(0.0, 2); // Enable radar module 

  /* Main loop */
  gettimeofday(&t0start, NULL); // Start T0 timer 
//  for(p=0; p<Npulses; p++) {
  while(1) {
    /* First set the whole vectors a1 and a2 to zero */
    for(n=0; n<N*Npad; n++) {
      a1[n] = 0.0 + 0.0*I;
      a2[n] = 0.0 + 0.0*I;
    }
    for(m=0; m<Nave; m++) {
      gettimeofday(&tstart, NULL); // Start T1 timer
      for(n=0; n<N; n++) {
        set_dac_voltage(V0*(double)n/(double)N, 1);
        Isig = read_adc_voltage(1, 0);
        Qsig = read_adc_voltage(2, 0);
        a1[n] = a1[n] + (Isig + Qsig * I)/(double)Nave;
        /*
        a1[n] = testv1[n];
        usleep(2);
        */
      }
      gettimeofday(&tstop, NULL);   // Stop T1 timer
      T1 = (tstop.tv_sec - tstart.tv_sec) * 1000.0;      // sec to ms 
      T1 += (tstop.tv_usec - tstart.tv_usec) / 1000.0;   // us to ms 
      gettimeofday(&tstart, NULL);  // Start T2 timer
      for(n=0; n<N; n++) {
        set_dac_voltage(V0*(double)(N - n - 1)/(double)N, 1);
        Isig = read_adc_voltage(1, 0);
        Qsig = read_adc_voltage(2, 0);
        a2[n] = a2[n] + (Isig + Qsig * I)/(double)Nave;
        /*
        a2[n] = testv2[n];
        usleep(2);
        */
      }
      gettimeofday(&tstop, NULL);    // Stop T2 timer
      T2 = (tstop.tv_sec - tstart.tv_sec) * 1000.0;      // sec to ms 
      T2 += (tstop.tv_usec - tstart.tv_usec) / 1000.0;   // us to ms
      T = (T1 + T2)/2000; // Sweep time in seconds
    }

    RemoveFunction(a1, ramp);
    RemoveFunction(a2, ramp);
    fft(a1, Npad*N, scratch);
    fft(a2, Npad*N, scratch);

    /* Gate out the near environment, determined by Rgate */
    df = 1.0/(Npad*T);
    fgate = 2*B/(c0*T)*Rgate;
    gate_idx = round(fgate/df);
    for(n=0; n<gate_idx; n++) {
      a1[n] = 0.0;
      a2[n] = 0.0;
      a1[Npad*N - n - 1] = 0.0;
      a2[Npad*N - n - 1] = 0.0;
    }

    /* Compute detection thresholds and location of maximum */
    detect(a1, &threshold1, &max_idx1);
    detect(a2, &threshold2, &max_idx2);
    if(max_idx1 < Npad*N/2) {
      delta_f1 = max_idx1*df;
    } else {
      delta_f1 = -(Npad*N - max_idx1)*df;
    }
    if(max_idx2 < Npad*N/2) {
      delta_f2 = max_idx2*df;
    } else {
      delta_f2 = -(Npad*N - max_idx2)*df;
    }
    // printf("threshold1 = %f, threshold2 = %f, max_idx1 = %d, max_idx2 = %d\n", threshold1, threshold2, max_idx1, max_idx2);
    // printf("delta_f1 = %g, delta_f2 = %g\n", delta_f1, delta_f2);

    /* Compute range, velocity, and amplitude */
    R = c0*T/(4*B)*(delta_f2 - delta_f1);
    v = c0/(4*f0)*(delta_f1 + delta_f2);
    A1 = cabs(a1[max_idx1]);
    A2 = cabs(a2[max_idx2]);
    printf("R = %.5f,\tv = %.5f,\tA1 = %.5f,\tA2 = %.5f,\tdf1 = %.5f\n", R, v, A1, A2, delta_f1);

    /* Output graphical representation of signal */
    for(n=0; n<Ngraph; n++) {
      if(cabs(a1[Npad*N-n-1]) > threshold1) {
	graphstring1[n] = '*';
      } else {
	graphstring1[n] = '.';
      }
      if(cabs(a2[Npad*n+1]) > threshold2) {
	graphstring2[n] = '*';
      } else {
	graphstring2[n] = '.';
      }
    }
//    printf("%s\n", graphstring1);
//    printf("%s\n", graphstring2);
  }
  gettimeofday(&t0stop, NULL); // Stop T0 timer
  T0 = (t0stop.tv_sec - t0start.tv_sec) * 1000.0;      // sec to ms 
  T0 += (t0stop.tv_usec - t0start.tv_usec) / 1000.0;   // us to ms 
  printf("%d pulses in %f ms, PRR = %f pulses/second\n", Npulses, T0, 100/T0*1000);

  /*
  for(n=0; n<N; n++) {
    printf("a1[%d] = %f + j%f,\t a2[%d] = %f + j%f\n", n, creal(a1[n]), cimag(a1[n]), n, creal(a2[n]), cimag(a2[n]));
  }
  */

  close_dac();
  return (0);
}