// howto compile 
// gcc -lm servoserialdriver.c mini_ssc.o -o servoserialdriver
// mkfifo /dev/pwm0
// chmod a+w /dev/pwm0

#include <stdio.h>
#include <math.h>
#include "mini_ssc.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <ctype.h>
#define max(a,b) ((b)<=(a)?(a):(b))

// range used to be 73 to 238

fd_set rfds; // globally defined structure for file descriptor select
int fd0;
int fd1;

int openfifo() {
  //int voht_pin_fifo0;
  //int voht_pin_fifo1;
  /* open the fifo for reading */
  //voht_pin_fifo = open("/dev/voht_pin", O_RDONLY | O_SYNC);
  /* RDWR seems to keep it from looping too much */
  fd0 = open("/dev/pwm0", O_RDWR | O_SYNC);
  fd1 = open("/dev/pwm1", O_RDWR | O_SYNC);
  if( fd0 == -1 ) { perror("open");exit(1); }
  if( fd1 == -1 ) { perror("open");exit(1); }
  return 0;
}

int readfromfifo(int *whichservo) { // returns whichservo and value of servo
  int something0 = 0;
  unsigned char buffer; // just one byte is needed now
  FD_ZERO(&rfds);
  FD_SET(fd0, &rfds);
  FD_SET(fd1, &rfds);
  select( max(fd0, fd1)+1, &rfds, NULL, NULL, NULL);

  if (FD_ISSET(fd0, &rfds)) {
    read( fd0, &buffer, 1);
    *whichservo=0;
    printf("servo %d got some data\n", *whichservo);
  }
  if (FD_ISSET(fd1, &rfds)) {
    read( fd1, &buffer, 1);
    *whichservo=1;
    printf("servo %d got some data\n", *whichservo);
  }

  // printf("got %d:%c\n", numread, buffer[j-1]);
  //return atoi(buffer);
  something0 = (int) buffer;
  printf("got %d\n", something0);
  return something0;
}

int main(int argc,char **argv)
{
  int port, baudrate, servo=0, min, max, delay, step;
  // int fd; // file descriptor for serial port
  char *s;
  int i,n,z;
  int x_in, y_in;
  int x_out, y_out;

  // defaults
  port     = 0;
  baudrate = 2400;

  // connect to the ssc
  if(ssc_open(port,baudrate)) perror("failed to open serial port");
  openfifo(); // global variable istead of return
  while (1) {
    // x_in = atoi(argv[1]);
    // change this to read from fifo:
    x_in = readfromfifo(&servo); // servo number and x_in returned from function

    x_in = x_in + 55;
    if (x_in > 254){
      x_in = 254;
      fprintf(stderr,"input greater than 254 is clipped to 254\n");
      fprintf(stderr,"you should enter only numbers from 0 to 199\n");
    }
    if (x_in < 22){
      x_in = 22;
      fprintf(stderr,"input less than 22 damages servo, so clipped to 22\n");
      fprintf(stderr,"you should enter only numbers from 0 to 199\n");
    }
    if (x_in < 55){
      x_in = 55;
      fprintf(stderr,"input less than 55 damages mount, so clipped to 55\n");
      fprintf(stderr,"you should enter only numbers from 0 to 199\n");
    }
  
    // enter infinite loop that translates pairs of screen coordinates
    // to pairs of telepointer coodinates.
    
    ssc_move( servo, x_in  );
  
    fprintf(stderr,"%d\n", x_in);
  }
}