#include <stdio.h>

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


/* Current hash, Rx+Tx key */
Hash curWifi;
/* Calibrated hash, Rx+Tx+Time key */
Hash clbrWifi;
/* Calibrated hash, unique Rx key */
Hash clbrWifiRx;
/* Calibrated hash, unique Tx key */
Hash clbrWifiTx;



int triangulateRx(Hash* hash, ThFn eqFn, WifiPair* pair){
int stat;
WifiPair *pCur, *pCur2;
WifiPair rsPair;
Iterator it, it2;
 float sumHx2=0, sumHy2=0, sumHxHy=0, sumHxC=0, sumHyC=0;
 float hx, hy, c, det;

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

  /* Find records by "pair" sample*/
  memcpy( &rsPair, pair, sizeof(rsPair));

  /* Iterarte throu all records specified by eqFn and pair */
  if( (stat = getFirstHashFilter(hash, &rsPair, eqFn, &it)) )
    return stat;

  while(1){

    pCur = (WifiPair*)it.ptr;

    /* Iterarte throu all records specified by eqFn and pair from current it down */
    memcpy(&it2, &it, sizeof(Iterator));
    while(1){

      if( (stat = getNextHashFilter(hash, &rsPair, eqFn, &it2)) )
	if( stat == E_END || stat == E_NOTFOUND )
	  break;
	else
	  return stat;		

      pCur2 = (WifiPair*)it2.ptr;

      hx = pCur->tx.X - pCur2->tx.X; 
      hy = pCur->tx.Y - pCur2->tx.Y;
      c = (pCur2->R*pCur2->R - pCur->R*pCur->R + 
	   pCur->tx.X*pCur->tx.X - pCur2->tx.X*pCur2->tx.X +
	   pCur->tx.Y*pCur->tx.Y - pCur2->tx.Y*pCur2->tx.Y)/2;

      sumHx2 += hx*hx;
      sumHy2 += hy*hy;
      sumHxHy += hx*hy;
      sumHxC += hx*c;
      sumHyC += hy*c;
    }

    if( (stat = findNextHash(hash, &rsPair, eqFn, &it)) )
      if( stat == E_END || stat == E_NOTFOUND )
	break;
      else
	return stat;		
  }

  det = sumHx2*sumHy2 - sumHxHy*sumHxHy;
  if(det){
    rsPair.rx.X = (sumHxC*sumHy2 - sumHyC*sumHxHy)/det;
    rsPair.rx.Y = (sumHyC*sumHx2 - sumHxC*sumHxHy)/det;


    /* Iterarte throu all records specified by eqFn and pair */
    if( (stat = getFirstHashFilter(hash, &rsPair, eqFn, &it)) )
      return stat;

    while(1){

      pCur = (WifiPair*)it.ptr;
      
      pCur->rx.X = rsPair.rx.X;
      pCur->rx.Y = rsPair.rx.Y;

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


      if( (stat = getNextHashFilter(hash, &rsPair, eqFn, &it)) )
	if( stat == E_END || stat == E_NOTFOUND )
	  break;
	else
	  return stat;		
    }
  }
  else
    return E_CANT_CALCULATE;

  return 0;
}



int triangulateTx(Hash* hash, ThFn eqFn, WifiPair* pair){
int stat;
WifiPair *pCur, *pCur2;
WifiPair rsPair;
Iterator it, it2;
 float sumHx2=0, sumHy2=0, sumHxHy=0, sumHxC=0, sumHyC=0;
 float hx, hy, c, det;

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

  /* Find records by "pair" sample*/
  memcpy( &rsPair, pair, sizeof(rsPair));

  /* Iterarte throu all records specified by eqFn and pair */
  /* Use findFirst only if eqFn has one of the parameters of hash function */ 
  if( (stat = findFirstHash(hash, &rsPair, eqFn, &it)) )
    return stat;

  while(1){

    pCur = (WifiPair*)it.ptr;

    /* Iterarte throu all records specified by eqFn and pair from current it down */
    memcpy(&it2, &it, sizeof(Iterator));
    while(1){

      if( (stat = findNextHash(hash, &rsPair, eqFn, &it2)) )
	if( stat == E_END || stat == E_NOTFOUND )
	  break;
	else
	  return stat;		

      pCur2 = (WifiPair*)it2.ptr;

      hx = pCur->rx.X - pCur2->rx.X; 
      hy = pCur->rx.Y - pCur2->rx.Y;
      c = (pCur2->R*pCur2->R - pCur->R*pCur->R + 
	   pCur->rx.X*pCur->rx.X - pCur2->rx.X*pCur2->rx.X +
	   pCur->rx.Y*pCur->rx.Y - pCur2->rx.Y*pCur2->rx.Y)/2;

      sumHx2 += hx*hx;
      sumHy2 += hy*hy;
      sumHxHy += hx*hy;
      sumHxC += hx*c;
      sumHyC += hy*c;
    }

    if( (stat = findNextHash(hash, &rsPair, eqFn, &it)) )
      if( stat == E_END || stat == E_NOTFOUND )
	break;
      else
	return stat;		
  }

  det = sumHx2*sumHy2 - sumHxHy*sumHxHy;
  if( det ){
    rsPair.tx.X = (sumHxC*sumHy2 - sumHyC*sumHxHy)/det;
    rsPair.tx.Y = (sumHyC*sumHx2 - sumHxC*sumHxHy)/det;


    /* Iterarte throu all records specified by eqFn and pair */
    if( (stat = findFirstHash(hash, &rsPair, eqFn, &it)) )
      return stat;

    while(1){

      pCur = (WifiPair*)it.ptr;

      pCur->tx.X = rsPair.tx.X;
      pCur->tx.Y = rsPair.tx.Y;

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


      if( (stat = findNextHash(hash, &rsPair, eqFn, &it)) )
	if( stat == E_END || stat == E_NOTFOUND )
	  break;
	else
	  return stat;		
    }
  }
  else
    return E_CANT_CALCULATE;

  return 0;
}



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


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

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

	/* Initialize calibrated hash of unique Tx */
	clbrWifiTx.hFn = getTxHashFn;
	clbrWifiTx.eqFn = isEqTx;
	clbrWifiTx.newFn = newWifiPair;
	clbrWifiTx.cpFn = ctorWifiPair;

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

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

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


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

	while(1){

	  pCur = (WifiPair*)it.ptr;
	  memcpy( &pair, pCur, sizeof(WifiPair) );
	  pair.type = C_SIGNAL | C_RADIUS_CALC | C_TXYZ;

	  /* Try to triangulate location for each unique Rx MAC */
	  stat = triangulateRx( &clbrWifi, isEqRxType, &pair );

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


	/* Iterarte throu unique hash keys (tx MACs) */
	if( (stat = getFirstHash(&clbrWifiTx, &it)) )
	  return stat;

	while(1){

	  pCur = (WifiPair*)it.ptr;
	  memcpy( &pair, pCur, sizeof(WifiPair) );
	  pair.type = C_SIGNAL | C_RADIUS_CALC | C_RXYZ;

	  /* Try to triangulate location for each unique Tx MAC */
	  stat = triangulateTx( &clbrWifi, isEqTxType, &pair );

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


	outputResult(0);

return 0;
}

