/*

                                   _JNJ`                        
                               .JNMH`                           
                              JMMF`       `;.                   
                            .NMM)          `MN.                 
                            MMM)            (MML                
                           (MMM`             MMML               
                     M  I  D  N  I  G  H  T    C  o  D  E       
                           (NMMF             MHNH               
                            NMML            .MMM                
                             NMML          .NMH                 
                              4MMNL       .#F                   
                               `4HNNL_   `                      
                                   `"""`                        

                          Copyright (C) 2004-2007
       "Ian (Larry) Latter" <ian dot latter at midnightcode dot org>

    Midnight Code is a registered trademark of Ian Latter.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, and mirrored at the Midnight Code web
    site; as at version 2 of the License only.
  
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
    General Public License for more details.
  
    You should have received a copy of the GNU General Public License
    (version 2) along with this program; if not, write to the Free
    Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
    02111-1307 USA, or see http://midnightcode.org/gplv2.txt

*/

/* midnightcode-servo.c :: Midnight Code servo io library		*/


#include <stdio.h>
#include <string.h>
#include <stdarg.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <linux/fs.h>
#include <ctype.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>

#include "preferences.h"
#include "midnightcode-list.h"
#include "midnightcode-parse.h"
#include "midnightcode-config.h"
#include "midnightcode-servo.h"



int
midnightcode_servo_flush(struct midnightcode_servo_element_t * servo) {
  int fd;


  if(!servo)
    return 1;

  if(access(servo->device, F_OK) != 0)
    return 1;

  if((fd = open(servo->device, O_NONBLOCK | O_RDONLY)) < 0)
    return 1;

  if(ioctl(fd, BLKFLSBUF) < 0) {
    close(fd);
    return 1;
  }

  close(fd);

  return 0;
}



int
midnightcode_servo_move(struct midnightcode_servo_element_t * servo,
                     int position) {
  int fd;
  int buffer_size;
  char * buffer;


  if(!servo)
    return -1;
  if(position < 0 || position > 180)
    return -1;

  if(access(servo->device, F_OK) != 0)
    return -1;

  if((fd = open(servo->device, O_WRONLY)) < 0)
    return -1;

  buffer_size = 16;
  buffer = malloc(buffer_size);
  if(!buffer)
    return -1;
  memset(buffer, 0, buffer_size);

  snprintf(buffer, buffer_size, "%d", position);
  if(strlen(buffer) == (unsigned int)(buffer_size - 1)) {
    free(buffer);
    return -1;
  }

  if(write(fd, buffer, strlen(buffer)) == -1)
    return -1;
  servo->position = position;

  free(buffer);
  close(fd);

  return 0;
}



int
midnightcode_servo_probe(struct midnightcode_servo_element_t * servo) {
  int fd;


  if(!servo)
    return 1;

  if(access(servo->device, F_OK) != 0)
    return 1;

  if((fd = open(servo->device, O_NONBLOCK | O_RDONLY)) < 0)
    return 1;

  close(fd);

  return 0;
}



int
midnightcode_servo_list(struct midnightcode_servo_head_t *elements) {
  int devn, count;
  struct midnightcode_servo_device_t * dev;
  struct midnightcode_servo_element_t * now_element;
  struct midnightcode_servo_device_t servo_device[] = {
    { "/dev/h-macs0:0" }, { "/dev/h-macs0:1" },
    { "/dev/h-macs0:2" }, { "/dev/h-macs0:3" },
    { "/dev/h-macs1:0" }, { "/dev/h-macs1:1" },
    { "/dev/h-macs1:2" }, { "/dev/h-macs1:3" },
    { "/dev/h-macs2:0" }, { "/dev/h-macs2:1" },
    { "/dev/h-macs2:2" }, { "/dev/h-macs2:3" },
    { "/dev/h-macs3:0" }, { "/dev/h-macs3:1" },
    { "/dev/h-macs3:2" }, { "/dev/h-macs3:3" },
    { 0 }
  };
 
 
  count = 0;

  for(dev = servo_device, devn = 0; servo_device[devn].device != NULL; devn++) {
    now_element = malloc(sizeof(struct midnightcode_servo_element_t));
    memset(now_element, 0, sizeof(struct midnightcode_servo_element_t));

    snprintf(now_element->device, 63, "%s", servo_device[devn].device);
    now_element->position = 0;

    if(midnightcode_servo_probe(now_element)) {
      free(now_element);
    } else {
      midnightcode_list_append(elements, now_element);
      count++;
    }
  }

  return count;
}

