/*********************************************************************
 *
 * AUTHORIZATION TO USE AND DISTRIBUTE
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that: 
 *
 * (1) source code distributions retain this paragraph in its entirety, 
 *  
 * (2) distributions including binary code include this paragraph in
 *     its entirety in the documentation or other materials provided 
 *     with the distribution, and 
 *
 * (3) all advertising materials mentioning features or use of this 
 *     software display the following acknowledgment:
 * 
 *      "This product includes software written and developed 
 *       by Brian Adamson and Joe Macker of the Naval Research 
 *       Laboratory (NRL)." 
 *         
 *  The name of NRL, the name(s) of NRL  employee(s), or any entity
 *  of the United States Government may not be used to endorse or
 *  promote  products derived from this software, nor does the 
 *  inclusion of the NRL written and developed software  directly or
 *  indirectly suggest NRL or United States  Government endorsement
 *  of this product.
 * 
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 ********************************************************************/
 

#include "protocolTimer.h"
#include "debug.h"

#include <math.h>     // for fabs() & isnan()
#include <stdio.h>

ProtocolTimer::ProtocolTimer()
    : timeout(-1.0), repeat_count(0), 
      interval(0), repeats(0), 
      list(NULL), prev(NULL), next(NULL) 
{
}  // end ProtocolTimer::ProtocolTimer()

ProtocolTimer::~ProtocolTimer()
{ 
    if (IsActive()) Deactivate(); 
}

void ProtocolTimer::Reset()
{
    if (IsActive())
    {
        ProtocolTimerMgr* mgr = list->Mgr();
        mgr->RemoveTimer(this);
        mgr->InstallTimer(this);
    }
}  // end ProtocolTimer::Reset()

double ProtocolTimer::TimeRemaining() const
{
    if (list)
    {
        double deltaTime;
        if (interval > PRECISION_TIME_THRESHOLD)
        {
            double currentTime = ((LongProtocolTimerList*)list)->GetCurrentTime();
            deltaTime = timeout - currentTime;
        }
        else
        {
            double currentTime = ((PrecisionProtocolTimerList*)list)->GetCurrentTime();
            deltaTime = PrecisionProtocolTimerList::DeltaTime(timeout, currentTime);
        }
        return (deltaTime > 0.0) ? deltaTime : 0.0;   
    }
    else
    {
        return -1.0;
    }   
}  // end ProtocolTimer::TimeRemaining()


void ProtocolTimer::Deactivate()
{
    ASSERT(list);
    list->Mgr()->RemoveTimer(this);
}  // end ProtocolTimer::Deactivate()


ProtocolTimerList::ProtocolTimerList()
    : head(NULL), tail(NULL) 
{

}  // end ProtocolTimerList()



// Remove timer from linked list and mark as inactive (list == NULL)
void ProtocolTimerList::RemoveTimer(ProtocolTimer* theTimer)
{
    if (theTimer->prev)
        theTimer->prev->next = theTimer->next;
    else
        head = theTimer->next;
    if (theTimer->next)
        theTimer->next->prev = theTimer->prev;
    else
        tail = theTimer->prev;
    theTimer->list = NULL;
}  // end RemoveTimer()


LongProtocolTimerList::LongProtocolTimerList()
{
}

void LongProtocolTimerList::InsertTimer(ProtocolTimer *theTimer)
{
    ASSERT(!theTimer->list);
    ProtocolTimer* next = head;    
    theTimer->list = this;
    double timeout = theTimer->timeout;
    while(next)
    {
        if (timeout < next->timeout)
        {
            theTimer->next = next;
            if((theTimer->prev = next->prev))
                theTimer->prev->next = theTimer;
            else
                head = theTimer;
            next->prev = theTimer;
            return;
        }
        else
        {
            next = next->next;
        }
    }   
    if ((theTimer->prev = tail))
        tail->next = theTimer;
    else
        head = theTimer; 
    tail = theTimer;   
    theTimer->next = NULL;
}  // end LongProtocolTimerList::InsertTimer()

void LongProtocolTimerList::DoTimers()
{
    ProtocolTimer* nextTimer = head;
    double currentTime = GetCurrentTime();
    DMSG(10, "LongProtocolTimerList::DoTimers() nextTimer:%p\n", nextTimer);
    while (nextTimer)
    {
        if (nextTimer->timeout <= currentTime)
        {
            if (nextTimer->DoTimeout() && nextTimer->IsActive())
            {
                mgr->RemoveTimer(nextTimer);
                int repeatCount = nextTimer->repeat_count;
                if (repeatCount) 
                {
                    mgr->InstallTimer(nextTimer);
                    if (repeatCount > 0) repeatCount--;
                    nextTimer->repeat_count = repeatCount;
                }
            }
            nextTimer = head;
        }
        else
        {
            nextTimer = NULL;
        }
    } 
}  // end LongProtocolTimerList::DoTimers() 


double LongProtocolTimerList::TimeRemaining()
{
    if (head)
    {
        double deltaTime = head->timeout - GetCurrentTime();
        return (deltaTime < 0.0) ? 0.0 : deltaTime;
    }
    else
    {
        return -1.0;
    }
}  // end LongProtocolTimerList::TimeRemaining()

PrecisionProtocolTimerList::PrecisionProtocolTimerList()
{
}

void PrecisionProtocolTimerList::InsertTimer(ProtocolTimer* theTimer)
{
    ASSERT(!theTimer->list);
    ProtocolTimer* next = head;
    theTimer->list = this;
    double timeout = theTimer->timeout;
    while(next)
    {
        if (DeltaTime(timeout, next->timeout) <= 0.0)
        {
            theTimer->next = next;
            if((theTimer->prev = next->prev))
                theTimer->prev->next = theTimer;
            else
                head = theTimer;
            next->prev = theTimer;
            return;
        }
        else
        {
            next = next->next;
        }
    }
    if ((theTimer->prev = tail))
        tail->next = theTimer;
    else
        head = theTimer;
    tail = theTimer;
    theTimer->next = NULL;
}  // end PrecisionProtocolTimerList::InsertTimer()


void PrecisionProtocolTimerList::DoTimers()
{
    DMSG(10, "PrecisionProtocolTimerList::DoTimers() ...\n");
    ProtocolTimer* next = head;
    double currentTime = GetCurrentTime();
    while (next)
    {
        //if (DeltaTime(nextTimer->timeout, current_time) <= (double)0.0)
        // (The code should read as the above comment, but floating point
        //  comparisons don't always seem to work quite as expected)
        // This works within a microsecond of accuracy
        DMSG(10, "PrecisionProtocolTimerList::DoTimers() next:%p interval:%f timeout:%f currentTime:%f\n",
                 next, next->interval, next->timeout, currentTime);
        if (DeltaTime(next->timeout, currentTime) < (double)1.0e-06)
        {
            if(next->DoTimeout() && next->IsActive())
            {
                int repeatCount = next->repeat_count;
                if (repeatCount) 
                {
                    DMSG(10, "PrecisionProtocolTimerList::DoTimers() reinstalling next:%p\n", next);
                    mgr->ReinstallTimer(next, currentTime);
                    if (repeatCount > 0) repeatCount--;
                    next->repeat_count = repeatCount;
                }
                else
                {
                    mgr->RemoveTimer(next);
                }
            }
            next = head;
        } 
        else
        {
            next = NULL;
        }
    }
}  // end PrecisionProtocolTimerList::DoTimers() 

double PrecisionProtocolTimerList::TimeRemaining()
{
    if (head)
    {
        double curtime = GetCurrentTime();
        double deltaTime = DeltaTime(head->timeout, curtime);
        return (deltaTime < 0.0) ? 0.0 : deltaTime;
    }
    else
    {
        return -1.0;
    }
}  // end PrecisionProtocolTimerList::TimeRemaining()



ProtocolTimerMgr::ProtocolTimerMgr()
    : installer_func(NULL), installer_data(NULL),
      user_data(NULL), timer_timeout(-1.0), update_pending(false)
{
    precision_list.Init(this);
    long_list.Init(this);
    long_base_timer.Init(1.0, -1, (ProtocolTimerOwner*)this, 
                         (ProtocolTimeoutFunc)&ProtocolTimerMgr::DoLongTimers); 
}



ProtocolTimerMgr::~ProtocolTimerMgr()
{
    Halt();
}  // end ProtocolTimerMgr::~ProtocolTimerMgr()

void ProtocolTimerMgr::Update()
{
    double nextTimeout = NextTimeout();
    DMSG(10, "ProtocolTimerMgr::Update() nextTimeout:%lf timer_timeout:%lf\n",
             nextTimeout, timer_timeout);
    if (nextTimeout != timer_timeout)
    {
        if (nextTimeout < 0.0)
        {
            // Remove pending timeout   
            installer_func(PROTOCOL_TIMER_REMOVE, 0.0, this, installer_data);
            timer_timeout = -1.0;
        }
        else if (timer_timeout < 0.0)
        {
            // Install new timeout (none currently installed)
            if (installer_func(PROTOCOL_TIMER_INSTALL, TimeRemaining(), 
                               this, installer_data))
            {
                timer_timeout = nextTimeout;
            }
            else
            {
                timer_timeout = -1.0;
                DMSG(0, "ProtocolTimerMgr::Update() INSTALL error!\n");
            }
        }
        else
        {
            // Modify existing timeout (replace installed timer) 
            if (installer_func(PROTOCOL_TIMER_MODIFY, TimeRemaining(), 
                               this, installer_data))
            {
                timer_timeout = nextTimeout;
            }
            else
            {
                timer_timeout = -1.0;
                DMSG(0, "ProtocolTimerMgr::Update() MODIFY error!\n");
            }  
        }
    }
}  // end ProtocolTimerMgr::Update() 

void ProtocolTimerMgr::InstallTimer(ProtocolTimer* theTimer)
{
    if (theTimer->interval > PRECISION_TIME_THRESHOLD)
    {
        theTimer->timeout = long_list.GetCurrentTime() + theTimer->interval;
        long_list.InsertTimer(theTimer);
        if (!long_base_timer.IsActive())
        {
            long_base_timer.timeout = 
                precision_list.TotalTime(precision_list.GetCurrentTime(), 
                                         long_base_timer.interval);
            precision_list.InsertTimer(&long_base_timer);
        }
    }
    else
    {
        double currentTime = precision_list.GetCurrentTime();
        theTimer->timeout = 
            precision_list.TotalTime(theTimer->interval, currentTime);
        precision_list.InsertTimer(theTimer);
    }
    theTimer->repeat_count = theTimer->repeats;
    if (!update_pending) Update();
}  // end ProtocolTimerMgr::InstallTimer()
    
void ProtocolTimerMgr::ReinstallTimer(ProtocolTimer* theTimer,
                                      double         now)
{
    ASSERT(theTimer->list);
    theTimer->list->RemoveTimer(theTimer);
    if (theTimer->interval <= PRECISION_TIME_THRESHOLD)
    { 
        theTimer->timeout = 
            precision_list.TotalTime(theTimer->timeout,
                                     theTimer->interval);
        // *** If a "precision timer" falls more than 1 second behind
        // don't keep forcing it to try and catch up
        if (precision_list.DeltaTime(theTimer->timeout, now) < -1.0)
        {
            DMSG(1, "ProtocolTimerMgr: Warning! real time failure\n");
            theTimer->timeout = now;
        }

        precision_list.InsertTimer(theTimer);
        theTimer->repeat_count = theTimer->repeats;
        if (!update_pending) Update();
    }
    else
    {
        InstallTimer(theTimer);
    }
}  // end ProtocolTimerMgr::ReinstallTimer()

void ProtocolTimerMgr::RemoveTimer(ProtocolTimer* theTimer)
{
    if (theTimer->list)
    {
        if (&long_list == theTimer->list)
        {
            long_list.RemoveTimer(theTimer);
            if (!long_list.head) 
                precision_list.RemoveTimer(&long_base_timer);
        }
        else
        {
            precision_list.RemoveTimer(theTimer);
        }
        if (!update_pending) Update();
    }
}  // end ProtocolTimerMgr::RemoveTimer()

void ProtocolTimerMgr::Halt()
{
    DMSG(10, "ProtocolTimerMgr::Halt() ....\n");
    if (timer_timeout >= 0.0)
    {
        installer_func(PROTOCOL_TIMER_REMOVE, 0.0, this, installer_data);
        timer_timeout = -1.0;
    }
}  // end ProtocolTimerMgr::HaltTimers()


