#include <malloc.h>
#include <utility>
#include <string.h>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <string>
#include <iterator>
#include <math.h>
#include <sys/timeb.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_fft_complex.h>

using namespace std;

#define REAL double

//Debugging

#define DEBUG yes
//Definition of SI variables

#define SI_G      6.67428E-11
#define SI_c      2.99792458E8
#define SI_c2     (SI_c*SI_c)
#define SI_ly     9460730472580800.0
#define SI_pc     (3.26156*SI_ly)
#define PI        3.141592653589793

// Definition for the Eart-Sun geometry

#define FFT_FORWARD  0
#define FFT_BACKWARD 1
#define FFT_INVERSE  2

#define SunEarthDistance    150000000000   //150 Mkm
#define EarthRadius         6353000        //6353 km
#define EarthRotPeriod      (24*3600)      //1 day
#define Day                 (24*3600)      //1 day
#define EarthOrbitingPeriod (365*24*3600)  //1 year
#define Year                (365*24*3600)  //1 year
#define EarthInclination    0.40927962     //23.45 degree

REAL sinEarthTilt = sin(EarthInclination);
REAL cosEarthTilt = cos(EarthInclination);

// Definition for the Hough transformation

#define Tobs       4096
#define Tcoh       4096
#define Tbin       1
#define N          (Tobs/Tcoh) //Number of segments
#define M          (Tcoh/Tbin) //Number of subsegments

#define SampleRate 4096        //Detector output's sampling frequency
#define Flow       40          //40 Hz lower cutoff frequency
#define FNyq       2048        //The Nyquist freuqncy
#define fthresold  200         //The PSD threshold
#define df         (1./Tcoh*SampleRate)   //The frequency resolution

/*
#define Tobs       33554432    //2**25 sample, 2.27 hour
#define Tcoh       4194304     //2**22 sample, 17 min
#define Tbin       65536       //2*16 sample,  16 sec 
#define N          (Tobs/Tcoh) //Number of segments
#define M          (Tcoh/Tbin) //Number of subsegments
*/

#define SkyResolution 100      //Tiling for sky the map

//Defines for the logging
enum   hLogLevels  {hALL = 6, hDEBUG = 5, hINFO = 4, hWARN =3, hERR = 2, hCRIT = 1, hNONE = 0 };
int    hLogLevel = hALL;
char * hlogmessage = (char*) malloc(100);

// The test data

REAL * data;
REAL * datafft;
int  * datathr;


char * hLogLevelNames[7] = {"NONE","CRIT","ERR","WARN","INFO","DEBUG","ALL"};
void hLog(int debuglevel, const char * message) {
    if (debuglevel <= hLogLevel) fprintf(stderr,"%s: %s\n",hLogLevelNames[debuglevel], message);
}

//Structure for pulsar parameters
typedef struct pulsarparams {
	REAL f0,f1,f2,f3;      //Base frequency and derivatives
	REAL latti,longi;      //Celestial coordinates in SSB
};

pulsarparams mypulsar;
//Hough map definition
//typedef int houghmap[SkyResolution][SkyResolution];

//Creating a of Hough-map
int hm[N][SkyResolution][SkyResolution];

// Gaussian noise generator, used the Muller-Box method

inline REAL RandReal()
{
    return rand() / ((REAL)RAND_MAX + 1);
}

void GenerateGaussianNoise(int length, REAL* out)
{
    REAL u1, u2, x, y;
    for(int i = 0; i < length; i = i + 2)
    {
        u1 = RandReal();
        u2 = RandReal();
        x = sqrt(-2.0 * log(u1)) * cos(2.0 * M_PI * u2);
        y = sqrt(-2.0 * log(u1)) * sin(2.0 * M_PI * u2);
        out[2*i]       = x;
        out[2*(i+1)]   = y;
        out[2*(i+1)+1] = 0;
	out[2*i+1]     = 0;
    }
}

//Function to emulate a pulsar signal for (testing purpose)
void AddPulsarSignal(REAL lat, REAL long, REAL freq,
                     REAL t, REAL length, REAL amp,
                                           REAL * buffer) {
	for (int i = 0; i < length*SampleRate; i++) {
         buffer[2*i+(int)(t*SampleRate)]+=amp*sin(freq*2*M_PI*i/SampleRate);
        }

}


//Structure to represent detector position on the Earth
typedef struct gw_detcoor {
	REAL longi, latti;         //coordinates of the detector in Earth ref. system
	REAL x,y,z;                //coordiantes of the detecot in the SSB
	REAL v,vx,vy,vz;           //speed of the detector in the SSB
	REAL vlatti,vlongi;        //celestial coordinate of the speed vector in SSB
};

gw_detcoor VirgoPos;

//Function calculating detector position and speed vector direction
// at a specific time.

int GetDetPosition(gw_detcoor * detcoor, REAL t) {

	REAL tmpx,tmpy,tmpz;
	REAL tmplongi;

	//Detector speed in Earth reference system
	REAL DetSpeed = 2*M_PI*EarthRadius*cos(detcoor->latti)/EarthRotPeriod;

	//First do the daily rotation around the z axis (North Pole)
	tmplongi = detcoor->longi + t/EarthRotPeriod*2*M_PI;

	// Then calculate the coordinates in the Earth reference system
        detcoor->x = cos(detcoor->latti)*cos(tmplongi)*EarthRadius;
	detcoor->y = cos(detcoor->latti)*sin(tmplongi)*EarthRadius;
	detcoor->z = sin(detcoor->latti)*EarthRadius;


	// The speed vector is perpendicular to the position and
        // perpendicular to the z axis.
        detcoor->vx = cos(tmplongi+M_PI/2.)*DetSpeed;
	detcoor->vy = sin(tmplongi+M_PI/2.)*DetSpeed;
	detcoor->vz = 0;

        // Some debug stuff to check the transformations
        sprintf(hlogmessage,"Detector x,y,z: %f %f %f", detcoor->x,detcoor->y,detcoor->z);
        hLog(hDEBUG,hlogmessage);
        sprintf(hlogmessage,"Detector vz,vy,vz: %f %f %f", detcoor->vx,detcoor->vy,detcoor->vz);
        hLog(hDEBUG,hlogmessage);
        sprintf(hlogmessage,"Scalar product of v and pos: %f",detcoor->vx*detcoor->x+detcoor->vy*detcoor->y+detcoor->vz*detcoor->z);
        hLog(hDEBUG,hlogmessage);

        //Rotation of speed and position vector due to earth tilt respect to SSB y axis (23.5)
	tmpx = detcoor->x*cosEarthTilt+detcoor->z*sinEarthTilt;
	tmpz = -detcoor->x*sinEarthTilt+detcoor->z*cosEarthTilt;
	detcoor->x = tmpx;
	detcoor->z = tmpz;

	tmpx = detcoor->vx*cosEarthTilt+detcoor->vz*sinEarthTilt;
	tmpz = -detcoor->vx*sinEarthTilt+detcoor->vz*cosEarthTilt;
	detcoor->vx = tmpx;
	detcoor->vz = tmpz;

	//Rotation of speed and position vector due to earth orbiting around the Sun
	REAL OrbitPhase = t/EarthOrbitingPeriod*2*M_PI;
	REAL cosOrbitPhase=cos(OrbitPhase);
	REAL sinOrbitPhase=sin(OrbitPhase);

	tmpx = detcoor->x*cosOrbitPhase - detcoor->y*sinOrbitPhase;
	tmpy = detcoor->x*sinOrbitPhase + detcoor->y*cosOrbitPhase;
	detcoor->x = tmpx;
	detcoor->y = tmpy;

	tmpx = detcoor->vx*cosOrbitPhase - detcoor->vy*sinOrbitPhase;
	tmpy = detcoor->vx*sinOrbitPhase + detcoor->vy*cosOrbitPhase;
	detcoor->vx = tmpx;
	detcoor->vy = tmpy;

	//Adding the Sun-Earth distance to the position vector.
	tmpx = cos(OrbitPhase)*SunEarthDistance;
	tmpy = sin(OrbitPhase)*SunEarthDistance;
	detcoor->x = detcoor->x+tmpx;
	detcoor->y = detcoor->y+tmpy;

	//adding the Earth speed to the speed vector
	REAL EarthSpeed = 2*M_PI*SunEarthDistance/EarthOrbitingPeriod;
	REAL EarthSpeedx = EarthSpeed*cos(OrbitPhase+M_PI/2.);
	REAL EarthSpeedy = EarthSpeed*sin(OrbitPhase+M_PI/2.);
	detcoor->vx += EarthSpeedx;
	detcoor->vy += EarthSpeedy;

	//Absolute value of the speed vector
	detcoor->v=sqrt(detcoor->vx*detcoor->vx+detcoor->vy*detcoor->vy+detcoor->vz*detcoor->vz);

	//Calculating the celestial coordiantes of the speed in SSB
	detcoor->vlatti = asin(detcoor->vz/detcoor->v);
	detcoor->vlongi = atan(detcoor->vy/detcoor->vx);

        sprintf(hlogmessage,"SSB vlong,vlatt: %f %f", detcoor->vlongi,detcoor->vlatti);
        hLog(hDEBUG,hlogmessage);

	if ((detcoor->vx < 0) && (detcoor->vy > 0)) {
            detcoor->vlongi += M_PI;
        } else if ((detcoor->vy < 0) && (detcoor->vx < 0)) {
            detcoor->vlongi += M_PI;
        } else if ((detcoor->vy < 0 ) && (detcoor->vx > 0)) {
            detcoor->vlongi += 2*M_PI;
        }

        sprintf(hlogmessage,"SSB vlong,vlatt2: %f %f", detcoor->vlongi,detcoor->vlatti);
        hLog(hDEBUG,hlogmessage);

	return 0;
}

int main()  {

//Variable declaration
REAL t, t0;

//Initializing data for Virgo (real values)
VirgoPos.longi = 0.750491;	// 43.2 degree
VirgoPos.latti = 0.17453;	// 10.31 degree

//Initializing data for Virgo (test values)
VirgoPos.longi = 2.05049;
VirgoPos.latti = 0.77453;

data    = new REAL[Tobs*2];
datafft = new REAL[Tobs*2];
datathr = (int *) calloc(N*Tobs*sizeof(int),1);

// Initializing randum number generator
srand ( time(NULL) );

//Filling up the data and datafft arrays with random noise
GenerateGaussianNoise(Tobs, data);


//Adding a pulsar signal to the data
mypulsar.latti=30/(2*M_PI);
mypulsar.longi=30/(2*M_PI);
mypulsar.f0=100;

AddPulsarSignal(mypulsar.latti, mypulsar.longi, mypulsar.f0,
                     0.0, 1.0 , 0.2, data);

//Copying the original data to the FFT buffer
for (int i = 0; i < Tobs*2; i++) {
 datafft[i] = data[i];
}


//Fourier transforming the input data in Tcoh segments
for (int i = 0; i < N; i++ ) {
  gsl_fft_complex_radix2_forward((REAL *)&datafft[i*Tcoh*2],(size_t) 1,(size_t) Tcoh);
}

//Thresholding the FFTzed data
for (int i = 0; i<N; i++) {
 for (int j = 0; j <Tcoh/2; j++) {
   REAL aux1 = sqrt(pow(datafft[i*Tcoh*2+2*j],2)+pow(datafft[i*Tcoh*2+2*j+1],2));
   if ( aux1 > fthresold ) {
    datathr[i*Tcoh+j] = 1;
    //printf("DEBUG: Over threshold : %d,%f\n",j,aux1);
   }
 }
}

REAL fsearch = 100.005;          //The search frequency
REAL finst;                      //Instantenous frequency
REAL cosphi,phi;                 //The angle between v and n

// Let's start everything at some given time
t0 = 24*3600*270;
t0 = 24*3600*300;
//Creating the Hough-map from point above the threshold
//Loop over Tcoh periods
for (int i=0; i< N;i++) {

 //Let's calculate the speed of the detector at the beginning of each Tcoh
 t = i*Tcoh/SampleRate+t0;
 GetDetPosition(&VirgoPos,t);

 //Now loop over the actual Tcoh segment. Only the half
 //containing valuable info, sys the FFT buffer is symmetric
 for (int j=0;j<Tcoh/2;j++) {

     //If something is above the threshold
     // we have to solve the Master equation for the measured frequency, mypulsar.f0 and v.
     if (datathr[i*Tcoh+j] > 0) {
        //Let's see if there is an inverze cosine or not
        REAL cosphi = (fsearch-j*df)/fsearch*SI_c/VirgoPos.v;

	if (( cosphi <= 1.0 ) && (cosphi >= -1.0)) {
           phi = acos(cosphi);
           //Drawing the circle in the Hough map
           for (REAL iota = 0; iota < 2*M_PI ; iota=iota+0.01) {
               REAL tmplat,tmplong;
               int  binx,biny;
               tmplat=VirgoPos.vlatti+cos(iota)*phi;
               tmplong=VirgoPos.vlongi+sin(iota)*phi;
               //Handling overpositioned points on the Hough map
               if (tmplat > M_PI/2) {tmplat = tmplat - M_PI;}
               if (tmplat < -M_PI/2) {tmplat = tmplat + M_PI;}
               if (tmplong > 2*M_PI) {tmplong = tmplong  - 2*M_PI;}
               if (tmplong < 0) {tmplong = tmplong + 2*M_PI;}

               //Coverting angle to bin on the Houg map
               binx = (int)(SkyResolution/(2*M_PI)*tmplong);
               biny = (int)(SkyResolution/M_PI*tmplat+SkyResolution/2);

               //Filling the Hough map of the ith Tcoh segment;
               hm[i][binx][biny]=1;
           };
        }
     } // end of thresholding loop
 } // end of Tcoh loop
} // end of Tobs loop

//Checking the results
//Printing the arrays
//for (int i = 0; i < Tcoh; i++) {
// printf("%d %f %f %f %f\n", i, data[2*i],data[2*i+1],datafft[2*i],datafft[2*i+1]);
//}

//Printing the 1st Hough Map
for (int i=0; i <SkyResolution;i++) {
 for (int j =0; j <SkyResolution;j++) {
   printf("%f %f %d\n",((double)i)/SkyResolution*2*M_PI,
                       ((double)j)/SkyResolution*M_PI-M_PI/2,hm[0][i][j]);
 }
}


return 0;
}