/*****************************
File:       RemoteSync.CPP
Language:   C++ (header)
Project:    H3DNetworkingUtils
The contents of this file are subject to the Mozilla Public License
Version 1.1 (the "License"); you may not use this file except in
compliance with the License. You may obtain a copy of the License at
http://www.mozilla.org/MPL/

Software distributed under the License is distributed on an "AS IS"
basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
License for the specific language governing rights and limitations
under the License.

The Original Code is H3DNetworkingUtils v1.0.

The Initial Developer of the Original Code is CSIRO.
Portions created by the Initial Developer are Copyright (C) 1009 CSIRO. All Rights Reserved.
Contributor(s):
    Chris Gunn  <Chris.Gunn@csiro.au> <ChrisJGunn@gmail.com>
***************************/
#include "H3DNetworkingUtils/RemoteSync.h"
#include <iomanip>
#include "H3DNetworkingUtils/RemoteTimePacket.h"
// Although the next line is not used directly in this file, it is still needed 
// because of template instantiation issues, to do with allowing RemoteSF.h to use conP->  ... etc
// and avoid a circular dependency in the headers. The alternative would be to move the conP-> stuff
// to RemoteSF.cpp.
#include "H3DNetworkingUtils/RemoteConnection.h" 

using namespace H3D;
using namespace H3DNetworkingUtils;

// ----------------------------------------------------------------------------
H3DNodeDatabase RemoteSync::database(   
  "RemoteSync", 
  &(newInstance<RemoteSync>),
  typeid(RemoteSync),
  &Group::database);

namespace RemoteSyncInternals {   
  FIELDDB_ELEMENT( RemoteSync, clockOffset, OUTPUT_ONLY );
  FIELDDB_ELEMENT( RemoteSync, timePacketReceived, INPUT_ONLY );
  FIELDDB_ELEMENT( RemoteSync, remoteTimePacket, INITIALIZE_ONLY );
}

// ----------------------------------------------------------------------------
RemoteSync::RemoteSync( Inst<ClockOffset > _clockOffset,
                        Inst< TimePacketCollector > _timePacketReceived,
                        Inst< SFRemoteTimePacket> _remoteTimePacket) :
clockOffset( _clockOffset ),
timePacketReceived (_timePacketReceived),
remoteTimePacket( _remoteTimePacket ),
time_packer( new TimePacker ),
timer( new TimeSensor ) {

  type_name = "RemoteSync";
  database.initFields( this );

   timePacketReceived->remote_syncP = this;
   children->push_back( timer.get() );
   clockOffset->rs = this;
   timer->enabled->setValue( false );
   timer->cycleInterval->setValue( 0.1 );
   timer->loop->setValue( true );
   timer->cycleTime->route( time_packer );
   clockOffset->setValue(0.0);
   timer->cycleTime->route(clockOffset);
  

///  remoteTimePacket->rs = this;
///  remoteTimePacket->id->setValue( TIME_STAMP_ID  );
}

// ----------------------------------------------------------------------------
void RemoteSync::initialize() {
  Group::initialize();
  if (remoteTimePacket->getValue()) {
    time_packer->route( remoteTimePacket->getValue()->toBeSent );
    timer->enabled->setValue( true );
    // It does not make sense for a remote sync's remote time packet to have timestamps false.
    remoteTimePacket->getValue()->timestamps->setValue(true);
  }
}

// ----------------------------------------------------------------------------
void RemoteSync::TimePacker::update(  ) {
  value.state = false;
  H3DTime now = TimeStamp::now();
  value.send_time = now;
  value.remote_time = 0.0;
  value.receive_time = 0.0;
}


// ----------------------------------------------------------------------------
void RemoteSync::ClockOffset::update() {
  // We start up at a faster rate, then slow down.
  if ( rs->results.size() == 20 ) {
    if ( startup ) {
      rs->timer->cycleInterval->setValue( 0.2 );
      startup = false;
    }
  }
  if ( rs->results.size() > 20 ) {
    // remove old values
    int extra = ( rs->results.size() - 20 );
    rs->results.erase( rs->results.begin(), ( rs->results.begin()
    + extra ) );

    // recalculate clock offset
    // determine mean
    double sum = 0;
    double latency = 0;
    vector< TimePacket >::iterator i;
    for ( i = rs->results.begin();
      i != rs->results.end();
      i++ ) {
        // round-trip latency
        latency = i->receive_time - i->send_time;
        sum = sum + ( ( i->remote_time - i->send_time ) -
        ( 0.5 * latency ) );
    }
    double mean = sum / rs->results.size();

    // determine standard deviation
    sum = 0;
    for ( i = rs->results.begin();
      i != rs->results.end();
      i++ ) {
        // round-trip latency
        latency = i->receive_time - i->send_time;
        sum = sum + pow( ( ( ( i->remote_time - i->send_time ) -
        ( 0.5 * latency ) ) - mean ), 2 );
    }
    double std_dev = sqrt( sum / ( rs->results.size() - 1 ) );

    // recalculate mean and use that as offset
    sum = 0;
    int count = 0;
    for ( i = rs->results.begin();
      i != rs->results.end();
      i++ ) {
        // round-trip latency
        latency = i->receive_time - i->send_time;
        double temp = ( i->remote_time - i->send_time ) -
        ( 0.5 * latency );
        if ( abs( temp - mean ) < std_dev ) {
          sum = sum + temp;
          count++;
        }
    }
    value = sum / count;
  }
}
