#include <sys/select.h>
#include <sys/time.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>

/*
 *  Copyright (C) 2012  William Robison
 *  All right Reserved
 *  Prior express permission is required for the
 *  use of this software.
 */

/*  SCHEDULING CONTROL, much like the MOD timing in the
 *    instrument (but absent the ??Hz scheduling).
 *
 *  period (expressed in seconds) is MOD PERIOD
 *      offset (expressed in milliseconds) is the MOD offset
 *
 *  We're not concerning ourselves with what the scheduling
 *   period is within the instrument is, only with
 *   synchronizing on some multiple of 1 seconds...
 *
 *     0        1        2        3        0
 *     |------------------------->|          period
 *            |<--------------------------   now
 *
 *     0(now) = period - (now % period) - 1
 */

 struct TIME {
              int secs;
              int msec;
              };
 char *scsim_align_time(int period, FILE *out, int line)
   {
     struct timeval current_time;
     static char buffer[128];
     gettimeofday(&current_time, NULL);
     sprintf(buffer, "%5d.%03d",
                         (int)current_time.tv_sec%86400,
                         (int)current_time.tv_usec/1000);
     if(period)
         sprintf(&buffer[strlen(buffer)], "(%d)", (int)current_time.tv_sec%period);
     if(out)
       {
         fprintf(out, "    %s/%03d[%d]: ", __FILE__, __LINE__, line);
         fprintf(out, "NOW=%s\n", buffer);
        }
     return buffer;
    }

 
 int scsim_align(int period_seconds,    // Scheduling period (seconds)
                   int offset_milli,    // offset into the period
                      int epoch_mod,    // use (now % epoch_mod) for scheduling
                          FILE *out,    // debug FILE
                           int line)    // __LINE__
   {
     struct timeval t_now;
     struct TIME now, then, base, delta;
     useconds_t usec;
     useconds_t us, um;
 
     /*
      *  sanity check
      */
     if( (period_seconds*1000) <= offset_milli)
         return -1;
 
     /*
      * ask for NOW (this assumes the system is
      * using NTP to synchronize with a good
      * time reference!
      */
     gettimeofday(&t_now, NULL);
     /*
      * rebuild "now" slightly, using the EPOCH_MOD
      *   typically EPOCH_MOD is 86400 or 3600 or 60
      *   so we're synchronous with days, hours, or minutes
      */
     if(epoch_mod)
         now.secs   = (int)t_now.tv_sec % epoch_mod;
     else
         now.secs   = (int)t_now.tv_sec;
     now.msec  = (int)t_now.tv_usec / 1000;

     /*
      * Time-0
      */
     base.secs =  now.secs / period_seconds;
     base.secs = base.secs * period_seconds;
     base.msec = 0;
     /*
      * When it should happen
      */
     then.secs = base.secs + offset_milli / 1000;
     then.msec = offset_milli % 1000;

     /*
      * calculate deltas
      */
     delta.secs = then.secs - now.secs;
     delta.msec = then.msec - now.msec;
     if(delta.msec<0)
       {
         delta.msec += 1000;
         delta.secs -= 1;
        }
     if(delta.secs<0)
         delta.secs += period_seconds;

     /*
      * debug output junk...
      */
     if(out)
       {
         fprintf(out, "    %s/%03d[%d]: ", __FILE__, __LINE__, line);
         fprintf(out, "scsim_align(%d, %d, FILE) ", period_seconds, offset_milli);
         fprintf(out, "base=%d.%03d ", base.secs, base.msec);
         fprintf(out, "now=%d.%03d ", now.secs, now.msec);
         fprintf(out, "then=%d.%03d ", then.secs, then.msec);
         fprintf(out, "delta=%d.%03d ", delta.secs, delta.msec);
         fflush(out);
        }
     /*
      * This is the calculated wait time...
      */
     us = delta.secs;
     um = delta.msec;
     usec = us*1000000 + um*1000;
     if(usleep(usec))
         fprintf(stderr, "%s/%d usleep(%d) failed\n", __FILE__, __LINE__, usec);

     /*
      * Returnj current time...
      */
     gettimeofday(&t_now, NULL);
     if(out)
       {
         if(epoch_mod)
             fprintf(out, "(%d", (int)t_now.tv_sec%epoch_mod);
         else
             fprintf(out, "(%d", (int)t_now.tv_sec);
         fprintf(out, ".%06d)  ", (int)t_now.tv_usec);
         fflush(stdout);
        }
     return (int)t_now.tv_sec;
    }