/*

  Powertracker - track power line phase









*/

#define period 20000

#define DEBUGMODE 0

#define SERIALMODE 1
#define PARALLELMODE 0
#define NICMODE 0
#define NOISESUPPRESS 1


#define halfperiod ((period+1)/2)
#define threequarterperiod ((3*period)/4)


#define _GNU_SOURCE 1

#include <stdlib.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sched.h>
#include <fcntl.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>

#include <syslog.h>
#include <sys/poll.h>
#include <linux/ppdev.h>
#include <linux/parport.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <getopt.h>


struct timeval tv;
struct timezone tz;

struct pl_shm
{
 unsigned phase[256];
 unsigned lastwritten;
 unsigned last_ts;
 double pwrtime;
};

struct pl_shm *sh;

struct pl_shm *attach_shm(int unit)
{
  int shm_id=0;
  shm_id=shmget(0x4b4b5000+unit, sizeof(struct pl_shm),
  IPC_CREAT|0700);
  if(shm_id==-1) {
      fprintf(stderr,"shmget failed.\n");
      perror("shmget");
      exit(1);
  }
 sh=(struct pl_shm *) shmat(shm_id, 0, 0);
 if(sh==(void *) -1 || sh==0) {
     fprintf(stderr,"shmat failed.\n");
     perror("shmat");
     exit(1);
 }
 return sh;
}

/*

 __asm__ __volatile__("": : :"memory");
*/


int lastevent_us,actphase,actphaselow;
double actphasef;
unsigned lastt;




void plevent(void)
{
  int delta,m;
  double e;
  unsigned t;
  long long int mll;
  
  delta=(tv.tv_usec-lastevent_us+1000000)%1000000;
  if (delta<threequarterperiod) {
    if (!NOISESUPPRESS) 
      if (DEBUGMODE)
        printf("noise\n");
    return;
  }  
  lastevent_us=tv.tv_usec;
  
  m=tv.tv_usec%period; //captured phase
  delta=((m-actphaselow+period+halfperiod)%period)-halfperiod;
  //max skew, 1%, is 20.2 ms = 200 usec
  if (delta<-200) delta=-200;
  if (delta> 200) delta= 200;
  
  actphase+=delta;
  actphaselow=(actphaselow+delta+period)%period;
  
//  printf("%8u   %i\n",m,actphase);
  
  e=actphase-actphasef;
  actphasef += e*.1; //200 ms tau filter

  t=tv.tv_sec;
  if (t!=lastt) {
    lastt=t;

    mll=actphasef;
    mll%=period;
    mll=(mll+period)%period;

    t%=256;
    sh->phase[t]=mll;
     __asm__ __volatile__("": : :"memory");
    sh->lastwritten=t;
    t=(t+10)%256; //ahead 10 sec
    sh->phase[t]=65535; //wipe ahead of current time
    sh->pwrtime=actphasef;
    sh->last_ts=tv.tv_sec;
     __asm__ __volatile__("": : :"memory");
    if (DEBUGMODE)
      printf("%f   %i\n",actphasef,mll);
  }

}






extern int errno;

int main(argc, argv)
     int argc;
     char *argv[];
{
  int i, f, f1, f2, f3, fx, whatitis; 
  int sp_fd = 0, pp_fd = 0, n_fd=0, zoo_poll = 0, hitolow = 0, flap = 0;
  int deviceis_serial = 0,  deviceis_parallel = 0, deviceis_nic=0, bogon = 0;

  int unit = 2;

  unsigned int pp_nfds = 1;

  struct pollfd pp_pollfd[2];
  struct ppdev_frob_struct frobit;
  struct sched_param sp;
  struct rlimit rl;

  char *prog;
//  char *pps_dev[256];
//  char *pps_sig[256];		/* name of sig line: CD RTS DSR */
  int sig = TIOCM_CD;
  sig = TIOCM_CTS;

  prog = argv[0];

  if (argc == 1) {
    bogon = 1;
  }

/*
  if (memcmp(*pps_sig, "DCD", 3) == 0) sig = TIOCM_CD;
  else 
  if (memcmp(*pps_sig, "CTS", 3) == 0) sig = TIOCM_CTS;
  else
  if (memcmp(*pps_sig, "DSR", 3) == 0) sig = TIOCM_DSR;
  else {
	  printf("%s -lSIGNAME must be one of DCD CTS DSR: %s\n",
						  prog, *pps_sig);
  	  exit(1);
  }
*/

  if (!DEBUGMODE) {
    if(fork()!=0) return(0);
    setsid();
    setpgrp();
    setsid();
    getrlimit(RLIMIT_NOFILE, &rl);
    for (i = 0; i <= rl.rlim_max; i++)
      (void) close(i);
  }
 
  mlockall(MCL_FUTURE);
  errno = 0;
  setpriority(PRIO_PROCESS, getpid(), -20);
  if (errno) {
      fprintf(stderr,"setpriority failed.\n");
  }
  memset(&sp, 0, sizeof(sp));
  sp.sched_priority=sched_get_priority_max(SCHED_FIFO);
  if(sched_setscheduler(0, SCHED_FIFO, &sp)!=0) {
      fprintf(stderr,"unable to set RR scheduling.\n");
  }


  if (PARALLELMODE) { //parallel
    pp_fd = open ("/dev/parport0",O_RDWR);
    if (pp_fd<0) {
      fprintf(stderr, "r/w open failed.\n");
      perror("open");
      exit(1);
    }
    if (ioctl (pp_fd, PPEXCL) == -1) {
      fprintf(stderr, "open exclusive failed.\n");
      perror ("ioctl_ppexcl");
      close (pp_fd);
      exit(1);
    }

    if (ioctl (pp_fd, PPCLAIM) == -1) {
      fprintf (stderr,"open claim failed.\n");
      perror ("ioctl_ppclaim");
      close (pp_fd);
      exit(1);
    }

    fx = IEEE1284_MODE_BYTE; 
    if (ioctl(pp_fd, PPSETMODE, &fx) == -1) {
      fprintf (stderr,"ppsetmode failed.\n");
      perror ("ioctl_ppsetmode");
      f1 = ioctl(pp_fd, PPRELEASE);
      close (pp_fd);
      exit(1);
    }
  }

  if (SERIALMODE) { //serial
    sp_fd = open("/dev/ttyS0", O_RDWR);
    if (sp_fd<0) {
      fprintf(stderr,"open failed.\n");
      perror("open");
      exit(1);
    }
  }


  if (NICMODE) { //nic
     struct sockaddr_in addr;
     int nbytes,addrlen;
     struct ip_mreq mreq;
     unsigned yes=1;            /*** MODIFICATION TO ORIGINAL */
     /* create what looks like an ordinary UDP socket */
     if ((n_fd=socket(AF_INET,SOCK_DGRAM,0)) < 0) {
	  perror("socket");
	  exit(1);
     }
    /* allow multiple sockets to use the same PORT number */
    if (setsockopt(n_fd,SOL_SOCKET,SO_REUSEADDR,&yes,sizeof(yes)) < 0) {
       perror("Reusing ADDR failed");
       exit(1);
       }
     /* set up destination address */
     memset(&addr,0,sizeof(addr));
     addr.sin_family=AF_INET;
     addr.sin_addr.s_addr=htonl(INADDR_ANY);
     addr.sin_port=htons(70);
     
     /* bind to receive address */
     if (bind(n_fd,(struct sockaddr *) &addr,sizeof(addr)) < 0) {
	  perror("bind");
	  exit(1);
     }
     /* use setsockopt() to request that the kernel join a multicast group */
     mreq.imr_multiaddr.s_addr=inet_addr("239.255.171.50");
     mreq.imr_interface.s_addr=inet_addr("10.5.5.251");//htonl(INADDR_ANY);
//     mreq.imr_interface.s_addr=inet_addr("10.222.222.222");//htonl(INADDR_ANY);
     if (setsockopt(n_fd,IPPROTO_IP,IP_ADD_MEMBERSHIP,&mreq,sizeof(mreq)) < 0) {
	  perror("setsockopt");
	  exit(1);
     }
  }

  attach_shm(unit);

  openlog(prog,0,LOG_LOCAL1);

  for (i=0; i<256; ++i) sh->phase[i]=65535;

  if (SERIALMODE) { //serial
    while (1) {
      while (ioctl(sp_fd, TIOCMIWAIT, sig) != 0) ;
      gettimeofday(&tv, &tz);
      if (ioctl(sp_fd, TIOCMGET, &f) != 0) continue;

      if (/*hitolow*/0) {
        if((f & sig) != 0) plevent();
      } else {
        if((f & sig) == 0) plevent();
      }
    }
  }



  if (PARALLELMODE) { //parallel
   f3 = 0;
   pp_pollfd[0].fd = pp_fd;
   pp_pollfd[0].events = POLLIN;
   while(1)
     {
       zoo_poll = poll(&pp_pollfd[0], pp_nfds, 1010);
       gettimeofday(&tv, &tz); /* grab timestamp */
       /* a zero is a timeout. keep going... */
       if(zoo_poll==0) continue;
       if(zoo_poll<0) {
	   fprintf(stderr,"zoo_poll pport failed.\n");
	   perror("zoo_poll");
	 syslog(LOG_ERR,"zoo_poll pport failed.");
	 exit(1); 
       }
       /* this stays here or you get a 40ms offset. yikes? */
       f2 = ioctl(pp_fd,PPCLRIRQ, &f3); 
       if (f2<0) {
	   fprintf(stderr,"parallel ppclrirq failure.\n");
	   perror("ppclrirq");
	 syslog(LOG_ERR,"parallel ppclrirq failure");
	 /* if we don't stop, we'll lock machine in a hard loop. */
	 exit(1);
       }

       if (0) {  //echo signal
	 frobit.mask = PARPORT_CONTROL_AUTOFD;
	 frobit.val = ~ PARPORT_CONTROL_AUTOFD;
	 if (ioctl (pp_fd, PPFCONTROL, &frobit) == -1) {
	     fprintf(stderr, "echo frob set failed.\n");
	     perror("ioctl_frob_set");
	 }
       }

       if(pp_pollfd[0].revents && POLLIN) {
	 plevent();
       }
       
       if (0) { //echo signal
	 frobit.mask = PARPORT_CONTROL_AUTOFD;
	 frobit.val = PARPORT_CONTROL_AUTOFD;
	 if (ioctl (pp_fd, PPFCONTROL, &frobit) == -1) {
	     fprintf(stderr, "echo frob clear failed.\n");
	     perror("ioctl_frob_clear");
	 }
       }
     }
  }



 if (NICMODE) { //nic
  unsigned msgbuf[256],nbytes,addrlen,stat;
  struct sockaddr_in addr;
  while (1) {
	if ((nbytes=recvfrom(n_fd,msgbuf,256,0, (struct sockaddr *) &addr,&addrlen)) < 0) {
	       perror("recvfrom");
	       exit(1);
	  }
	tv.tv_sec=0;
	tv.tv_usec=0;
	stat=ioctl(n_fd,SIOCGSTAMP,&tv);
  	if (stat) gettimeofday(&tv, &tz);
	plevent();
  }
 }


}