/** \file
 *
 *  Contains the AsyncPiEstimator class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "AsyncPiEstimator.h"
#include "AsyncPiEstimatorIF.h"

#include <limits.h>
#include <stdlib.h>
#include <sched.h>

#include "data/Slate.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

AsyncPiEstimator::AsyncPiEstimator( const Module* module )
    : AsyncComponent( AsyncPiEstimatorIF::NAME, module, Timespan( 1.0 / 3.0 ) )
{
    logger_.syslog( "Construct AsyncPiEstimator." );
    piEstimateWriter_ = Slate::NewOutputWriter( AsyncPiEstimatorIF::PI_ESTIMATE, this, Units::NONE() );
    reset();
}

AsyncPiEstimator::~AsyncPiEstimator()
{}

/// Initialize function
void AsyncPiEstimator::initialize( void )
{
    logger_.syslog( "Initialize AsyncPiEstimator." );
    reset();
}

/// The actual "payload" of the component
void AsyncPiEstimator::run()
{
    // Implements the slowly converging...
    // pi=4-4/3+4/5-4/7+4/9-...-((-1)^k)*4/(2*k-1) (for all k=1,2,3,...)
    printf( "now in AsyncPiEstimator::run()\n" );
    //   for( int i = 1; i <= 100; ++ i )
    while( true )
    {
        piEstimate_ += 4 / denominator_;
        denominator_ = -1 * ( denominator_ + 2 * AuvMath::Sign( denominator_ ) );
        sched_yield();
    }
    //printf("pi = %15.12g, denominator=%g\n", piEstimate_, denominator_ );

    /*
    //TODO get rid of this segfault and this component
    if ( failCount_ < 20 && fabs( piEstimate_ - M_PI ) < -1e-4 )
    {
        ++failCount_;
        reset();
        // Let's throw a segfault!
        printf( "Let's throw a AsyncPiEstimator segfault!\n" );
        fflush( stdout );
        int* foo = NULL;
        int bar = *foo;
        printf( "Bar is %d\n", bar );
    }
    //TODO get rid of this fault and this component
    if ( failCount_ < 1 && fabs( piEstimate_ - M_PI ) < 1e-4 )
    {
        ++failCount_;
        reset();
        // Let's throw a div by zero error!
        printf( "Let's throw a AsyncPiEstimator div by zero error!\n" );
        fflush( stdout );
        double foo = 1;
        double bar = 0;
        double baz = ( sqrt( foo ) * 0 ) / bar;
        printf( "( sqrt(%g) * 0 ) / %g is %g\n", foo, bar, baz );
    }
    */

    piEstimateWriter_->write( Units::NONE, piEstimate_ );

    printf( "AsyncPiEstimator::run() - return\n" );
}

void AsyncPiEstimator::reset()
{
    piEstimate_ = 0;
    denominator_ = 1;
}

/// Uninit function
void AsyncPiEstimator::uninitialize( void )
{
    logger_.syslog( "Uninitialize AsyncPiEstimator." );
}