#include <stdio.h>

#include "node.h"
#include "hash.h"
#include "radius.h"
#include "batch_ipc.h"


/* Current hash */
Hash curWifi;
/* Calibrated hash Tx key */
Hash clbrWifi;
/* Calibrated hash Rx key */
Hash clbrWifiRx;


int leastSqK(WifiPair* pair){
int stat;
WifiPair* pCur;
WifiPair rsPair;
Iterator it;
float sumS=0, sumLogR=0, sumR=0, sumRS=0, sumRlogR=0, sumR2=0, sum1dR=0;
int count=0;

 if( !pair )
   return E_BADINPUT;

  /* Find calibrated records */
  memset( &rsPair, 0, sizeof(WifiPair) );
  rsPair.type = C_SIGNAL | C_RADIUS;
  memcpy( &rsPair.rx.mac, &pair->rx.mac, sizeof(rsPair.rx.mac));


  if( (stat = getFirstHashFilter(&clbrWifi, &rsPair, isEqRxType, &it)) )
    if( stat == E_END )
      return 0;
    else
      return stat;

  /* Iterarte throu all calibrated (S, R set) records for a passed MAC and calculate least squares of k coefficient and S0 */
  while(++count){

    pCur = (WifiPair*)it.ptr;

    sumS += pCur->S;	
    sumR += pCur->R;
    sumLogR += log10(pCur->R);
    sumRS += pCur->S * pCur->R;
    sumRlogR += pCur->R * log10(pCur->R);
    sumR2 += pCur->R * pCur->R;
    sum1dR += 1/pCur->R;

    if( (stat = getNextHashFilter(&clbrWifi, &rsPair, isEqRxType, &it)) )
      if( stat == E_END )
	break;
      else
	return stat;		
  }

  if( sumR/count - sumR2/sumR ){
    pair->kPow = ((sumRS + 20*sumRlogR)/sumR - (sumS + 20*sumLogR)/count)\
      / (10 * log10(exp(1))*(sumR/count - sumR2/sumR));
    pair->kS0 = (sumS + 20*sumLogR + 10*pair->kPow*log10(exp(1))*sumR)/count;

    pair->sigmaK = ( ( ( 
			(sumS+20*sumLogR+20*count*log10(exp(1)))*sumR -\
			(sumRS+20*sumRlogR)*count
		       )/((sumR)*(sumR)) -\
		       20*log10(exp(1))*sum1dR/count 
		     ) *\
		     (sumR/count - sumR2/sumR) -\
		     ((sumRS+20*sumRlogR)/sumR-(sumS+20*sumLogR)/count) *\
		     (1-(2*sumR*sumR-sumR2*count)/(sumR*sumR))
		   )
      / (10*log10(exp(1))*(sumR/count - sumR2/sumR)*(sumR/count - sumR2/sumR));
  }
  else
    return E_CANT_CALCULATE;


return 0;
}



int leastSqN(WifiPair* pair){
int stat;
WifiPair* pCur;
WifiPair rsPair;
Iterator it;
float sumS=0, sumLogR=0, sumR=0, sumRS=0, sumRlogR=0, sumSlogR=0, sumLogR2=0, sum1dR=0;
 float sigmaRmean=MEAN_SIGMA_R_CALIBRATION, sigmaSmean=0;
int count=0;

 if( !pair )
   return E_BADINPUT;

  /* Find calibrated records */
  memset( &rsPair, 0, sizeof(WifiPair) );
  rsPair.type = C_SIGNAL | C_RADIUS;
  memcpy( &rsPair.rx.mac, &pair->rx.mac, sizeof(rsPair.rx.mac));


  /* Iterarte throu all calibrated (S, R set) records for a passed Rx MAC and calculate least squares of n coefficient and S0 */
  if( (stat = getFirstHashFilter(&clbrWifi, &rsPair, isEqRxType, &it)) )
    if( stat == E_END )
      return 0;
    else
      return stat;

  while(++count){

    pCur = (WifiPair*)it.ptr;

    sumS += pCur->S;	
    sumR += pCur->R;
    sumLogR += log10(pCur->R);
    sumRS += pCur->S * pCur->R;
    sumRlogR += pCur->R * log10(pCur->R);
    sumSlogR += pCur->S * log10(pCur->R);
    sumLogR2 += log10(pCur->R) * log10(pCur->R);
    sum1dR += 1/pCur->R;

    sigmaSmean += pCur->sigmaS;

    if( (stat = getNextHashFilter(&clbrWifi, &rsPair, isEqRxType, &it)) )
      if( stat == E_END )
	break;
      else
	return stat;		
  }

  sigmaSmean /= count;

  /* Calculate deviations of n and S0 */
  if( sumLogR/count - sumRlogR/sumR ){
    pair->nPow = (sumRS/sumR - sumS/count)\
      / (10. * (sumLogR/count - sumRlogR/sumR));
    pair->nS0 = (sumS + 10. * pair->nPow * sumLogR) / count;

    pair->sigmaN = ( ((sumS*sumR - sumRS*count)/(sumR*sumR))*\
		    (sumLogR/count - sumRlogR/sumR)-\
		    (sumRS/sumR - sumS/count)*\
		    (sum1dR*log10(exp(1))/count-\
		     ((sumLogR + count*log10(exp(1)))*sumR - sumRlogR*count)/\
		     (sumR*sumR)))/\
      (10.*(sumLogR/count - sumRlogR/sumR)*(sumLogR/count - sumRlogR/sumR)) *
      sigmaRmean;

    pair->sigmaNS0 = sqrt( pow(10.*pair->nPow*log10(exp(1))/count*sum1dR, 2) * 
			   sigmaRmean*sigmaRmean + sigmaSmean*sigmaSmean +
			   pow(10.* sumLogR/count, 2)*pair->sigmaN*pair->sigmaN
			 );
  }
  else
    return E_CANT_CALCULATE;

return 0;
}



int updateCalibration(Hash* hash, WifiPair* pair){
int stat;
WifiPair* pCur;
WifiPair rsPair;
Iterator it;

 if( !hash || !pair )
   return E_BADINPUT;

  /* Find calibrated records */
  memset( &rsPair, 0, sizeof(WifiPair) );
  memcpy( &rsPair.rx.mac, &pair->rx.mac, sizeof(rsPair.rx.mac));
  rsPair.type = C_SIGNAL;


  /* Iterarte throu all calibrated records for passed MAC and update calibrated values */
  if( (stat = getFirstHashFilter(hash, &rsPair, isEqRxType, &it)) )
    if( stat != E_END )
      return stat;

  while(!stat){

    pCur = (WifiPair*)it.ptr;

    pCur->kPow = pair->kPow;
    pCur->kS0 = pair->kS0;

    pCur->nPow = pair->nPow;
    pCur->nS0 = pair->nS0;
    pCur->sigmaN = pair->sigmaN;
    pCur->sigmaNS0 = pair->sigmaNS0;

    pCur->type |= C_NPOW;


    /* Calculate distance and deviation */
    pCur->Rcalc = RcalcN(pCur);
    pCur->sigmaRproj = sqrt( pow( dRdN(pCur,pCur->Rcalc), 2.)*
			     (pow(pCur->sigmaS,2.)+pow(pCur->sigmaNS0,2.)) + 
			     pow(dRdS0(pCur,pCur->Rcalc) * pCur->sigmaN, 2) 
			   );

    pCur->type |= C_RADIUS_CALC;


    if( (stat = upsertHash( hash, pCur )) )
      return stat;


    if( (stat = getNextHashFilter(hash, &rsPair, isEqRxType, &it)) )
      if( stat != E_END && stat != E_NOTFOUND )
	return stat;		
  }

return 0;
}





int main(int argc, char* argv[]){
  int stat;
  WifiPair pair;
  WifiPair* pCur;
  Iterator it;

  float k, n;


	/* Initialize current hash */
	curWifi.hFn = getTxHashFn;
	curWifi.eqFn = isEqTxRx;
	curWifi.newFn = newWifiPair;
	curWifi.cpFn = ctorWifiPairLoad;
	
	/* Initialize calibrated hash */
	clbrWifi.hFn = getTxHashFn;
	clbrWifi.eqFn = isEqTxRxTime;
	clbrWifi.newFn = newWifiPair;
	clbrWifi.cpFn = ctorWifiPairLoad;

	/* Initialize calibrated Rx hash */
	clbrWifiRx.hFn = getRxHashFn;
	clbrWifiRx.eqFn = isEqRx;
	clbrWifiRx.newFn = newWifiPair;
	clbrWifiRx.cpFn = ctorWifiPair;

	if( (stat = initLoad()) )
		return stat;

	/* Create hash with key on Rx */
	if( (stat = copyHash( &clbrWifi, &clbrWifiRx) ))
	    return stat;


	/* Iterarte throu unique hash keys (rx MACs)  */
	if( (stat = getFirstHash(&clbrWifiRx, &it)) )
	  if( stat != E_END )
	    return stat;

	while(!stat){

	  pCur = (WifiPair*)it.ptr;


	  if( (stat = leastSqK( pCur )) )
	    return stat;
	  if( (stat = leastSqN( pCur )) )
	    return stat;
	  
	  if( (stat = updateCalibration( &clbrWifi, pCur )) )
	    return stat;
	  if( (stat = updateCalibration( &curWifi, pCur )) )
	    return stat;


	  if( (stat = getNextHash(&curWifi, &it)) )
	    if( stat == E_END )
	      break;
	    else
	      return stat;		
	}


	outputResult(0);

return 0;
}

