#include "machdefs.h"

//---- System et stdc++ include files
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <string.h>
#include <iostream>
#include <fstream>
#include <complex>

#include <typeinfo>
#include <string>
#include <vector>
#include <map>
#include <functional>
#include <list>

//---- Sophya include files
#include "sopnamsp.h"
#include "basetools.h"
#include "systools.h"
#include "sutils.h"
#include "ntools.h"
#include "array.h"
#include "histats.h"

//---- Spiapp include files
#include "nobjmgr.h"
#include "servnobjm.h"

//---- Include files from additionnal modules

//---- function to compare bits on double 
int_8 BitCmp64(double v,int_8 flg) 
{return ((int_8)((v<0.) ? v-0.1 : v+0.1))&flg;} 

//---- function for Adding and displaying Objects  
void Keep_Object(AnyDataObj & obj, string const & nom) 
{ 
  string name = nom; 
  NamedObjMgr om; 
  if (om.GetObj(name)) 
    cerr << "KeepObj()/Warning Already kept object " << endl; 
  else om.AddObj(obj, name); 
} 

void Display_Object(AnyDataObj & obj, string const & opt, string const & nom) 
 { 
  string name = nom; 
  NamedObjMgr om; 
  if (!om.GetObj(name)) 
    om.AddObj(obj, name); 
  om.DisplayObj(name, opt); 
} 

//---- function for getting and setting ObjectManager variables  
void Set_ObjMgrVar(MuTyV v, string const & nom) 
{ 
  NamedObjMgr om; 
  om.SetVar(nom, (string)v); 
} 

MuTyV Get_ObjMgrVar(const char * nom) 
{ 
  string name = nom; NamedObjMgr om; 
  MuTyV v = om.GetVar(name); 
  return v; 
} 

//---- Macro for Objects and variables saving
#define KeepObj(obj) Keep_Object(obj, #obj)
#define GetOMVar(var) Get_ObjMgrVar( #var )
#define SetOMVar(var) Set_ObjMgrVar(var, #var )

//---- Macro Displaying objects and command execution
#define DispObj(obj, att) Display_Object(obj, att, #obj); 

#define ExecCmd(cmd) srvo.ExecuteCommand(cmd); 



//-------------------------------------------------//
//----------------- User Functions ----------------//
//-------------------------------------------------//

extern "C" {
  int doFilter( vector<string>& args );
}

template<class RandAccessIter>
double median(RandAccessIter begin, RandAccessIter end) {
  std::size_t size = end - begin;
  std::size_t middleIdx = size/2;
  RandAccessIter target = begin + middleIdx;
  std::nth_element(begin, target, end);
    
  if(size % 2 != 0){ //Odd number of elements
    return *target;
  }else{            //Even number of elements
    double a = *target;
    RandAccessIter targetNeighbor= target-1;
    std::nth_element(begin, targetNeighbor, end);
    return (a+*targetNeighbor)/2.0;
  }
}


void medianFiltering(const TMatrix<r_4> mtx, 
		     sa_size_t halfwidth,
		     TMatrix<r_4>& vec) {
  
  sa_size_t nr = mtx.NRows();
  sa_size_t nc = mtx.NCols();
  sa_size_t chMin = 0;
  sa_size_t chMax = nc-1;
  
  for (sa_size_t ir=0; ir<nr; ir++){
    for (sa_size_t ic=0; ic<nc; ic++) {
      sa_size_t chLow = ic-halfwidth;
      chLow = (chLow >= chMin) ? chLow : chMin;
      sa_size_t chHigh = ic+halfwidth;
      chHigh = ( chHigh <= chMax ) ? chHigh : chMax;
      TVector<r_4> tmp(mtx(Range(ir),Range(chLow,chHigh)),false);
      vector<r_4> val;
      tmp.FillTo(val);
      vec(ir,ic) = median(val.begin(),val.end());
    }
  }
}


int doFilter( vector<string>& args )
{
// Some definitions to help using spiapp;
NamedObjMgr omg;
Services2NObjMgr& srvo = *omg.GetServiceObj();

//-------------- Object List --------------
//Number of objects = 2
string ___nomobj;

___nomobj = "specv0";
TVector< r_4 > * ___specv0 = dynamic_cast< TVector< r_4 >  * >(omg.GetObj(___nomobj));
if(___specv0==NULL) throw NullPtrError("CxxExecutor::PutObject: Non existing object specv0... please update file");
TVector< r_4 > & specv0 = (*___specv0);

___nomobj = "specv1";
TVector< r_4 > * ___specv1 = dynamic_cast< TVector< r_4 >  * >(omg.GetObj(___nomobj));
if(___specv1==NULL) throw NullPtrError("CxxExecutor::PutObject: Non existing object specv1... please update file");
TVector< r_4 > & specv1 = (*___specv1);



//--------------------------------------------//
//----------------- User Code ----------------//
//--------------------------------------------//

 sa_size_t NUMBER_OF_CHANNELS = 2;
 sa_size_t NUMBER_OF_FREQ = 8192;
 TMatrix<r_4> specMtxInPut(NUMBER_OF_CHANNELS,NUMBER_OF_FREQ);
 specMtxInPut.Row(0)=specv0;
 specMtxInPut.Row(1)=specv1;

 TMatrix<r_4> specMtxOutPut(NUMBER_OF_CHANNELS,NUMBER_OF_FREQ);
 
 sa_size_t halfWidth=10;
 medianFiltering(specMtxInPut,halfWidth,specMtxOutPut);

 TVector<r_4>sfilt0=specMtxOutPut.Row(0).CompactAllDimensions();
 TVector<r_4>sfilt1=specMtxOutPut.Row(1).CompactAllDimensions();
 KeepObj(sfilt0);
 KeepObj(sfilt1);
 
 
 return 0;
}