#include <iostream.h>
#include <iomanip.h>
#include <fstream.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <sys/types.h>
#include <project.h>

#define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr

class marv
{
public:
  int reg_sig_len;
  float* reg_sig;
  int irreg_sig_len;
  float* irreg_sig;
  int* irreg_locs;
  int recon_sig_len;
  float* recon_sig;
  float* wt;
  int filt_len;
  float* filt;
  float* err;
  float max_err;
  
  int Done;
  int Fs;
  int Bw;
  int iter_num;
  int max_iter;
  float thresh;
  float relax;
  float total_time;
  
  marv();
  void reset(float* rs,  int rsl,float* is,  int* il,  int isl,double  tsh,int mi,double bw,double  fs);
  ~marv();

  void fft(float data[], unsigned int nn, int isign);
  void SetParam(int mi,float tol,float rlx);
  void SetWeights();
  void SetFilter(  int* index);
  float* GetReconstSig();
  float* GetErrorSig();
    int* GetBW(float* data,  int len,float frac);
  result  next_iter();
  void initialise();
  friend ostream& operator << (ostream& os, marv& p);
};


void marv::fft(float data[], unsigned int nn, int isign)
{
	int n,mmax,m,j,istep,i;
	float wtemp,wr,wpr,wpi,wi,theta;
	float tempr,tempi;

	n=nn << 1;
	j=1;
	for (i=1;i<n;i+=2) {
		if (j > i) {
			SWAP(data[j],data[i]);
			SWAP(data[j+1],data[i+1]);
		}
		m=n >> 1;
		while (m >= 2 && j > m) {
			j -= m;
			m >>= 1;
		}
		j += m;
	}
	mmax=2;
	while (n > mmax) {
		istep=mmax << 1;
		theta=isign*(6.28318530717959/mmax);
		wtemp=sin(0.5*theta);
		wpr = -2.0*wtemp*wtemp;
		wpi=sin(theta);
		wr=1.0;
		wi=0.0;
		for (m=1;m<mmax;m+=2) {
			for (i=m;i<=n;i+=istep) {
				j=i+mmax;
				tempr=wr*data[j]-wi*data[j+1];
				tempi=wr*data[j+1]+wi*data[j];
				data[j]=data[i]-tempr;
				data[j+1]=data[i+1]-tempi;
				data[i] += tempr;
				data[i+1] += tempi;
			}
			wr=(wtemp=wr)*wpr-wi*wpi+wr;
			wi=wi*wpr+wtemp*wpi+wi;
		}
		mmax=istep;
	}
}

void marv::SetParam(int mi,float tol,float rlx)
{
    max_iter=mi;
    thresh=tol;
    relax=rlx;    
}

void marv::SetWeights()
{
  for(  int i=0;i<irreg_sig_len;i++)
    wt[i]=1.0;
}

float* marv::GetReconstSig()
{
    return recon_sig;
}

float* marv::GetErrorSig()
{
    return err;
}

void marv::SetFilter(  int* index)
{
      int i;
    for(i=0;i<filt_len;i++)
	filt[i]=0.0;
    for(i=0;i<index[0];i++)
	{
	    filt[i]=1.0;
	}
  for(i=index[1];i<recon_sig_len;i++)
	{
	    filt[i]=1.0;
	}
}

result marv::next_iter()
{
    float iter_time=0.0;
    float mean_err;
    if(Done== -1)
 	{
	    float* xd;
	    xd=new float[recon_sig_len];
	    float* data;
	    data=new float[2*recon_sig_len];
	   
	    float err_sum,xa_sum;
	      int i;
	    clock_t time1,time2;
	    iter_num++;
	    //if((iter_num%5)==0)
	    cout <<" Iter Number "<<iter_num << endl;
	    for(i=0;i<recon_sig_len;i++)
		{
		    xd[i]=0.0;
		    err[i]=0.0;
		}
	    time1=clock();
	    for(i=0;i<irreg_sig_len;i++)
		{
		    xd[irreg_locs[i]]=(irreg_sig[i]-recon_sig[irreg_locs[i]])*wt[i];
		}
	    for(i=0;i<recon_sig_len;i++)
		{
		    data[2*i]=xd[i];
		    data[2*i+1]=0.0;
		}
	    fft(data-1,recon_sig_len,1);
	    for(i=0;i<recon_sig_len;i++)
		{
		    data[2*i]=data[2*i]*filt[i];
		    data[2*i+1]=data[2*i+1]*filt[i];
		}
	    fft(data-1,recon_sig_len,-1);
	    for(i=0;i<recon_sig_len;i++)
		{
		    recon_sig[i]=relax*data[2*i]/recon_sig_len+recon_sig[i];
		}
	    time2=clock();
	    iter_time=(float)(time2-time1);
	    total_time=total_time+iter_time;
	    err_sum=0;xa_sum=0;
	    for(i=0;i<reg_sig_len;i++)
		{
		    err[i]=recon_sig[i]-reg_sig[i];
		    err_sum=err_sum+err[i]*err[i];
		    xa_sum=xa_sum+reg_sig[i]*reg_sig[i];
		}
	    err_sum=sqrt(err_sum);
	    mean_err=(err_sum/sqrt(xa_sum))*100.0;
	    float pct=2;
	    if(max_err!=0.0) pct=fabs(100*(mean_err-max_err)/max_err);
	    if((mean_err<thresh/100.0) || (iter_num>max_iter) || (pct<.0001))
		{
		  cout << "Total no of Iter = " << iter_num << endl;
		  Done=1;
		}
	    if((max_err!=0.0) && (max_err<mean_err))
		{
		  cerr << "Diverging in MARV, quitting\n";
		  Done=1;
		}
	    max_err=mean_err;
	}    
    return result(recon_sig,recon_sig_len,iter_num,iter_time,total_time,mean_err,err,Done);
}

  int* marv::GetBW(float* data,  int len,float frac)
{
  float energy;
  float temp;
    int i;
    int *bw1=new   int[2];
  energy=0.0;
  for(i=1;i<len/2;i++)
    {
      energy=energy+data[2*i]*data[2*i];
    }
  temp=0.0;
  for(i=1;i<len/2;i++)
    {
      temp=temp+data[2*i]*data[2*i];
      if(frac<=(temp/energy)) break;
    }
  bw1[0]=i;
  energy=0.0;temp=0.0;
  for(i=len-1;i>=len/2;i--)
    {
      energy=energy+data[2*i]*data[2*i];
    }
  temp=0.0;
  for(i=len-1;i>=len/2;i--)
    {
      temp=temp+data[2*i]*data[2*i];
      if(frac<=(temp/energy)) break;
    }
  bw1[1]=i;
  return bw1;
};

void marv::initialise()
{
      int i,*index=new   int[2];
 
	
    float* data;
    data=new float[2*recon_sig_len];
    for(i=0;i<recon_sig_len;i++)
	{
	    data[2*i]=reg_sig[i];
	    data[2*i+1]=0.0;
	}
    fft(data-1,recon_sig_len,1);    
    index=GetBW(data,recon_sig_len,0.99);
    cout << "filt index =" << index[0] << " " << index[1]  << endl;
    SetWeights();
      int temp=(  int) (filt_len*Bw/Fs);
    if(temp>index[0])
	{
	    index[0]=temp;
	    index[1]=filt_len-temp;
	}
    cout << "filt index =" << index[0] << " " << index[1]  << endl;
    SetFilter(index);
    for(i=0;i<recon_sig_len;i++)
	{
	    recon_sig[i]=0.0;
	}
    iter_num=0;
 }

ostream& operator << (ostream& os, marv& p)
{
    for(  int i=0;i<p.recon_sig_len;i++)
	os << i << '\t' << p.recon_sig[i] << '\n';
    return os;
};

marv::marv()
 {
   Done=-1;
   iter_num=0;
   relax=1.0;
   recon_sig=NULL;
   wt=NULL;
   filt=NULL;
   err=NULL;
 }


void marv::reset(float* rs,  int rsl,float* is,  int* il,  int isl,double  tsh,int mi,double bw,double  fs)
{
    reg_sig_len=rsl;
    reg_sig=rs;
    irreg_sig_len=isl;
    irreg_sig=is;
    irreg_locs=il;
    recon_sig_len=rsl;
    
    Done=-1;
    Fs=(  int)(fs);
    Bw=(  int)(bw);
    iter_num=0;
    max_iter=mi;
    relax=1.0;
    thresh=(float)(tsh);    
    filt_len=recon_sig_len;
     total_time=0.0;

     if(recon_sig!=NULL) delete recon_sig;
    if(wt!=NULL) delete wt;
    if(filt!=NULL) delete filt;
    if(err!=NULL) delete err;
    recon_sig=new float[recon_sig_len];
    wt=new float[irreg_sig_len];
    filt=new float[filt_len];
    err=new float[recon_sig_len];
    initialise();
 };

marv::~marv()
{
    if(recon_sig!=NULL) delete recon_sig;
    if(wt!=NULL) delete wt;
    if(filt!=NULL) delete filt;
    if(err!=NULL) delete err;
}