// Basically a data storage class for unstructured signals that
// Uses the POCS method to reconstruct them.

// Does reconsruction on irregularly sampled signal using POCS
// The reconstruction is done on a regualrly sampled grid 
// sampled at the sampling frequency of the irreg samples

#include <fstream.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <time.h>
#include <tk.h>
#include <tcl.h>
#include <project.h>

#define PRINT_ORIGINAL_SIGNAL 1
#define PRINT_RECONSTRUCTED_SIGNAL 2
#define PRINT_ALL 3


#define CONV_FRACTION  0.05
#define SINC_LOBES 10

class pocs
{
  float* orig_samples;
    int orig_length;

  float* irr_samples;
    int* irr_xi;
    int irr_length;

  float *sinc_val;

  float B; // bandwidth, frequency within [-2piB, 2piB]
  float Fs;
  
  int max_iter;
  float tol;

    int final_length;

  float *final;
  float *err;
  float mean_err;
  float time_taken;
  
  int Debug;
  int DebugLevel;
  
public:
  
  pocs();
  pocs(float* original_samples,   int original_length,
       float* irreg_samples,   int *irreg_locations,   int irreg_length,
       double tolerance, int maxiterations, double bandwidth, double sampl_freq);
  
  void reset(float* original_samples,   int original_length,float* irreg_samples,
	       int *irreg_locations,   int irreg_length,double tolerance, int maxiterations, 
	     double bandwidth, double sampl_freq); 
  void   project_i(  int i);       // Projects fk onto Ci using signal(xi)
  result next_iter(void); 
  // Will be bound to the tcl button.  
  void  CheckConvergence();
  void  GenDiffSignal();
  void  DebugOn(){Debug = 1;}
  void  DebugOff(){Debug = -1;}
  void  SetDebugLevel(int i);
  void gen_sinc(void);

  int iter;
  int Done;

  friend ostream& operator << (ostream&,pocs&);
};
//////////////////////////////////////////////////////////////////////
pocs::pocs()
{
  Debug = -1;
  DebugLevel = PRINT_RECONSTRUCTED_SIGNAL;
  Done = -1;  
  time_taken =0;
  
  sinc_val = NULL;
  irr_samples = NULL;
  irr_xi = NULL;
  final = NULL;
  err = NULL;
  orig_length = final_length = irr_length = 0;
  mean_err = 100;
}
//////////////////////////////////////////////////////////////////////
pocs::pocs(float* original_samples,   int original_length,
	   float* irreg_samples,   int *irreg_locations,   int irreg_length,
	   double tolerance, int maxiterations, double bandwidth, double sampl_frequency)
{
    int i;

  B = bandwidth;
  Fs = sampl_frequency;
  max_iter = maxiterations;
  if(B <=0 || Fs <=0)
    {
      cerr<<" Invalid Bandwidth or Sampling frequency\n POCS destructing";
      exit(1);
    }
  

  orig_length = original_length;
  orig_samples = original_samples;

  irr_samples = irreg_samples;
  irr_xi = irreg_locations;
  irr_length = irreg_length;

  final_length = orig_length;

  final = new float[final_length];
  err   = new float[final_length];
  mean_err = 100;
  // Initialize the fk arrays
  
  for(i=0; i<final_length;i++)
    final[i] = 0;
  iter = 0;

  gen_sinc();
}
//////////////////////////////////////////////////////////////////////
void pocs::reset(float* original_samples,   int original_length,
		 float* irreg_samples,   int *irreg_locations,  int irreg_length,
		 double tolerance, int maxiterations, double bandwidth, double sampl_frequency)
{
    int i;

  B = bandwidth;
  Fs = sampl_frequency;
  max_iter = maxiterations;
  if(B <=0 || Fs <=0)
    {
      cerr<<" Invalid Bandwidth or Sampling frequency\n POCS destructing";
      exit(1);
    }
  

  orig_length = original_length;
  orig_samples = original_samples;

  irr_samples = irreg_samples;
  irr_xi = irreg_locations;
  irr_length = irreg_length;

  final_length = orig_length;

  if(final != NULL) delete final;
  if(err != NULL) delete err;
  
  final = new float[final_length];
  err   = new float[final_length];
  mean_err = 100;
  // Initialize the fk arrays
  
  for(i=0; i<final_length;i++)
    final[i] = 0;
  iter = 0;

  gen_sinc();
}
//////////////////////////////////////////////////////////////////////
// Using the samp_no property, namely the samp_no'th sample, project// 
// the approximation fk onto {Ci : i = samp_no}                     //
//////////////////////////////////////////////////////////////////////
void pocs::project_i(  int samp_no)
{

  float sample_diff;
    int jj,loc;
  
  loc = irr_xi[samp_no];
  sample_diff = irr_samples[samp_no] - final[loc];

  // minimise the local influence of the error to reduce time taken

  for( jj=0; jj <final_length;jj++)
    final[jj]+=sample_diff*sinc_val[abs(loc -jj)];

  if(Debug == 1)
    {
	cout<<" Iteration : "<<iter<<'\n';
	if(DebugLevel == PRINT_ORIGINAL_SIGNAL ||
	   DebugLevel == PRINT_ALL)
	  {
	    for(   int i=0; i<orig_length; i++)
	      cout<<i<<'\t'<<orig_samples[i]<<'n';
	    cout<<"\n\n Reconstructed signal\n";
	  }
	else
	  { 
	    for(  int i=0; i<final_length; i++) 
	      cout<<i<<'\t'<<final[i]<<'n';
	  }
    }
}
//////////////////////////////////////////////////////////////////////
result pocs::next_iter()
{
  clock_t time1,time2;

  time1=clock();
  for(  int iter_step =0; iter_step<irr_length;iter_step++)
    project_i(iter_step);
  time2 = clock();
  iter++;  
  time_taken+=(time2-time1);
  
  this->GenDiffSignal();
  this->CheckConvergence();
  
  if(iter >max_iter)
    Done = 1;
  return result(final, final_length, iter, time2-time1,
		time_taken, mean_err, err, Done);
}
//////////////////////////////////////////////////////////////////////
ostream& operator <<(ostream& os, pocs& p)
{
  if(p.Debug ==1)
    {
      os<<"Input Signal Length:  "<<p.orig_length<<'\n';
      os<<"Input Signal Bandwidth:  "<<p.B<<'\n';
    }
  
  os<<"Iteration:  "<<p.iter<<'\n';
  if(p.Debug == 1)
    {
      if(p.DebugLevel == PRINT_ORIGINAL_SIGNAL ||
	 p.DebugLevel == PRINT_ALL)
	for(  int i=0; i<p.orig_length; i++)
	  os<<i<<'\t'<<p.orig_samples[i]<<'\n';
      
      for(  int i=0;i<p.final_length;i++)
	os<<i<<'\t'<<p.final[i]<<'\n';
     }
  return os;
}
//////////////////////////////////////////////////////////////////////
void pocs::SetDebugLevel(int i)
{
  if(i>3 || i<1)
    {
      cerr<<"\n Illegal DebugLevel value\n";
      i = PRINT_RECONSTRUCTED_SIGNAL;
    }
  DebugLevel = i;
}
//////////////////////////////////////////////////////////////////////
void pocs::GenDiffSignal()
{
  for(  int i=0; i< final_length; i++)
    err[i] = final[i] - orig_samples[i];
}
//////////////////////////////////////////////////////////////////////
void pocs::CheckConvergence()
{
  float mean_conv = 0;
  float tot =0;
    int i;
  
  for(i=0; i<final_length; i++)
    {
      mean_conv += pow( err[i],2);
      tot+= orig_samples[i]*orig_samples[i];
    }
  
  mean_conv = sqrt(mean_conv/orig_length);
  tot = sqrt(tot/orig_length);
  
  if( tot !=0)mean_conv =(mean_conv/tot)*100;
  else mean_conv = 100;
  
  
  if(mean_conv <= tol) Done = 1;
  if( fabs(mean_err - mean_conv) < 1e-4) Done = 1;
  if( fabs(mean_conv) > fabs(mean_err)) 
    {cerr<<"Diverging in POCS, quitting \n"; Done = 1;}
  mean_err = mean_conv;
}
//////////////////////////////////////////////////////////////////////
void pocs::gen_sinc(void)
{
  float mul;

  if( sinc_val != NULL) delete sinc_val;
  sinc_val = new float[final_length];
  sinc_val[0] = 1;
  mul = 1.0/Fs;

  for(  int i=1; i< final_length; i++)
    sinc_val[i] = sin(2*M_PI*B*i*mul)/(2*M_PI*B*i*mul);
}
//////////////////////////////////////////////////////////////////////