#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <time.h>
#include "kiss_fft.h"
#include "filtre.h"
#include <stdbool.h>
#include <math.h>
#include "hps_0.h"
#include <sys/mman.h>
#include <pthread.h>
// Includes pour le memory map
#include "C:/intelFPGA_pro/17.1/embedded/ip/altera/hps/altera_hps/hwlib/include/hwlib.h"
#include "C:/intelFPGA_pro/17.1/embedded/ip/altera/hps/altera_hps/hwlib/include/soc_cv_av/socal/socal.h"
#include "C:/intelFPGA_pro/17.1/embedded/ip/altera/hps/altera_hps/hwlib/include/soc_cv_av/socal/hps.h"
#include "C:/intelFPGA_pro/17.1/embedded/ip/altera/hps/altera_hps/hwlib/include/soc_cv_av/socal/alt_gpio.h"
// Defines pour le memory map
#define HW_REGS_BASE ( ALT_STM_OFST )
#define HW_REGS_SPAN ( 0x04000000 )
#define HW_REGS_MASK ( HW_REGS_SPAN - 1 )
#define VAL_ULTRASON_MOY 2000

void* pwm(void* arg);
int distUltrason[1];

int main(){
  // Création de la configuration et des buffers in et out pour s(t) et S(f)
  const kiss_fft_cfg config = kiss_fft_alloc(NFFT, 0, NULL, NULL);
  const kiss_fft_cfg configInv = kiss_fft_alloc(NFFT, 1, NULL, NULL);
  kiss_fft_cpx* in = (kiss_fft_cpx*)malloc(NFFT*sizeof(kiss_fft_cpx));
  kiss_fft_cpx* out = (kiss_fft_cpx*)malloc(NFFT*sizeof(kiss_fft_cpx));
  kiss_fft_cpx* inv = (kiss_fft_cpx*)malloc(NFFT*sizeof(kiss_fft_cpx));
  int i, cpt, sec;
  for(i = 0; i < NFFT; i++){
      in[i].r = 0.0;
	  inv[i].r = 0.0;
    }
	
  // buffers du memory map
  volatile int *adc=NULL;
  volatile unsigned long *spi=NULL;
  volatile int *pio=NULL;
  void *virtual_base;
  int fd;
  int data = 0; 
  
  // memory map 
  if( ( fd = open( "/dev/mem", ( O_RDWR | O_SYNC ) ) ) == -1 ) {
    printf( "ERROR: could not open \"/dev/mem\"...\n" );
    return( 1 );
  }
  virtual_base = mmap( NULL, HW_REGS_SPAN, ( PROT_READ | PROT_WRITE ), MAP_SHARED, fd, HW_REGS_BASE );	
  if( virtual_base == MAP_FAILED ) {
    printf( "ERROR: mmap() failed...\n" );
    close( fd );
    return(1);
  }
  adc = virtual_base + ( ( unsigned long  )( ALT_LWFPGASLVS_OFST + ADC_0_BASE ) & ( unsigned long)( HW_REGS_MASK ) );
  spi = virtual_base + ( (unsigned long) (ALT_LWFPGASLVS_OFST + SPI_0_BASE ) & (unsigned long) ( HW_REGS_MASK) );
  pio = virtual_base + ( (unsigned long) (ALT_LWFPGASLVS_OFST + PIO_BOUTON_BASE ) & (unsigned long) ( HW_REGS_MASK) );
  
  //Thread pour le capteur ultrason
  pthread_t ultrason;
  pthread_create(&ultrason, NULL, pwm, (void*)adc);
  
  // Structures de contrôle du nanosleep
  struct timespec tim;
  tim.tv_sec = 0;
  tim.tv_nsec = (long)1L;
  int dodo;

  while(1){
	acquisition(adc, &data);
	if(((*pio) & 1) || ((*pio)>>1 & 1) || ((*pio)>>2 & 1)){
		dac(spi, inv[cpt].r);
	}else{
		dac(spi, (data));	
	}
    in[cpt].r = (float)data;
    
    // Calcul de la FFT depuis in vers out
	cpt ++;
	if(cpt == NFFT){
		kiss_fft(config, in, out);
		// Calcul de la FFT inverse
		if((*pio) & 1){
			passeBas(out, (*(adc+2)-115));
		}
		if((*pio)>>1 & 1){
			passeHaut(out, (*(adc+4)-115));
		}
		if((*pio)>>2 & 1){
			modulation(out, (*distUltrason)*100);
			passeHaut(out, ((*distUltrason)-1)*100);
		}
		kiss_fft(configInv, out, inv);
		for(i = 0; i < NFFT; i++){
			inv[i].r /= NFFT;
		}
		cpt = 0;
	}
	if(sec >= 40000){
		//affichage(out);
		afficherFmax(out);
		if((*pio) & 1){
			printf("Passe bas à Fc = %d\n", *(adc+2)-115);
		}
		if((*pio)>>1 & 1){
			printf("Passe haut à Fc = %d\n", *(adc+4)-115);
		}
		if((*pio)>>2 & 1){
			printf("Modulation à Fp = %d\n", *distUltrason*100);
		}
		sec = 0;
	}
	sec++;
	//nanosleep(&tim , NULL);
	for(dodo =2550; dodo > 0; dodo --){
		
	}
  }
  // Libération des espaces mémoire alloués
  free(configInv);
  free(config);
  free(in);
  free(out);
  free(inv);
  return 0;
}

void* pwm(void* arg){
	int cptPwm = 0;
	int flag;
	int* adc = (int*) arg;
	while(1){
		if(*(adc+6) > VAL_ULTRASON_MOY){
			cptPwm ++;
			flag = 1;
		}else if((*(adc+6) < VAL_ULTRASON_MOY) && (flag == 1)){
			//printf("Nombre de cycles état haut: %d\n", cptPwm/140);
			*distUltrason = cptPwm/140;
			cptPwm = 0;
			flag = 0;
		}
	}
}