/*

                                   _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

*/

/* servo.c :: Midnight Code Servo example program			*/


#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <math.h>
#include <arpa/inet.h>
#include <errno.h>

#include <libmidnightcode.h>


int
main(int argc, char **argv) {
  int cfg_controller;
  int cfg_servo;
  int cfg_movement;
  int cfg_help;
  char device[32];
  int device_match;
  struct midnightcode_servo_head_t * servo_list_head_p;
  struct midnightcode_servo_element_t * servo;
  void * tmp_void_p;
  int ret_i;


  struct midnightcode_switch_t switches[] = {
    { "c", "ctrl", "Controller # (0)",
      MIDNIGHTCODE_PARSE_TYPE_INT, 0, 0, &cfg_controller },
    { "s", "servo", "Servo # (0)",
      MIDNIGHTCODE_PARSE_TYPE_INT, 0, 0, &cfg_servo },
    { "m", "move", "Movement in degrees",
      MIDNIGHTCODE_PARSE_TYPE_INT, 0, 0, &cfg_movement },
    { "h", "help", "Display this help/usage screen",
      MIDNIGHTCODE_PARSE_TYPE_NONE, 0, 0, &cfg_help },
    { 0 }
  };


  cfg_controller = 0;
  cfg_servo = 0;
  cfg_movement = -1;
  cfg_help = 0;


  // Process switches
  if((ret_i = midnightcode_switch_process(argc, argv, switches)) != 0) {
    fprintf(stdout, "\n");
    midnightcode_logo(0, MIDNIGHTCODE_COLOUR_STYLE_BLOOD_MOON);
    midnightcode_license(0, MIDNIGHTCODE_COLOUR_STYLE_BLOOD_MOON,
      MIDNIGHTCODE_LICENSE_TEXT_COPYRIGHT);
    fprintf(stdout, "    Error in switches, switch \"%s\" is unkown"
      " at parameter number %d.\n\n", argv[ret_i], ret_i);
    midnightcode_switch_usage(MIDNIGHTCODE_SWITCH_STYLE_SOFT, argv[0],
      switches);
    exit(1);
  }
  if(cfg_help) {
    midnightcode_logo(0, MIDNIGHTCODE_COLOUR_STYLE_BLOOD_MOON);
    midnightcode_license(0, MIDNIGHTCODE_COLOUR_STYLE_BLOOD_MOON,
      MIDNIGHTCODE_LICENSE_TEXT_COPYRIGHT);
    midnightcode_switch_usage(MIDNIGHTCODE_SWITCH_STYLE_SOFT, argv[0],
      switches);
    exit(1);
  }
  fflush(stdout);



  // Process license and logo
  midnightcode_logo(0, MIDNIGHTCODE_COLOUR_STYLE_BLOOD_MOON);
  midnightcode_license(0, MIDNIGHTCODE_COLOUR_STYLE_BLOOD_MOON,
    MIDNIGHTCODE_LICENSE_TEXT_ALL);



  // Program title
  midnightcode_status_text(MIDNIGHTCODE_STATUS_TYPE_H1,
    MIDNIGHTCODE_STATUS_STYLE_NORMAL, "Servo example: %s", argv[0]);

  fprintf(stdout, "\n");
  fflush(stdout);




  // Validate user params
  midnightcode_status_text(MIDNIGHTCODE_STATUS_TYPE_TEXT,
                           MIDNIGHTCODE_STATUS_STYLE_NORMAL,
                           "Validating user parameters");
  if(cfg_movement < 0 || cfg_movement > 180) {
    midnightcode_status_flag(MIDNIGHTCODE_STATUS_FLAG_FAIL);
    fprintf(stdout, "\n");
    exit(1);
  } else {
    midnightcode_status_flag(MIDNIGHTCODE_STATUS_FLAG_OKAY);
  }




  // Initialise Servo library
  midnightcode_list_init(servo_struct_head, midnightcode_servo_element_t);
  tmp_void_p = (void *)&servo_struct_head;
  servo_list_head_p = tmp_void_p;

  midnightcode_status_text(MIDNIGHTCODE_STATUS_TYPE_TEXT,
                           MIDNIGHTCODE_STATUS_STYLE_NORMAL,
                           "Probing servo devices");
  if(midnightcode_servo_list(servo_list_head_p) < 0) {
    midnightcode_status_flag(MIDNIGHTCODE_STATUS_FLAG_FAIL);
    fprintf(stdout, "\n");
    exit(1);
  } else {
    midnightcode_status_flag(MIDNIGHTCODE_STATUS_FLAG_OKAY);
  }




  // Match Servo device
  memset(device, 0, sizeof(device));
  snprintf(device, sizeof(device) - 1, "/dev/h-macs%d:%d",
    cfg_controller, cfg_servo);

  device_match = 0;
  midnightcode_status_text(MIDNIGHTCODE_STATUS_TYPE_TEXT,
                           MIDNIGHTCODE_STATUS_STYLE_NORMAL,
                           "Matching servo device [%s]", device);
  midnightcode_list_for_each(servo, servo_list_head_p) {
    if(strncmp(device, servo->device, strlen(device)) != 0)
      continue;
    device_match = 1;
    break;
  }
  if(!device_match) {
    midnightcode_status_flag(MIDNIGHTCODE_STATUS_FLAG_FAIL);
    fprintf(stdout, "\n");
    exit(1);
  } else {
    midnightcode_status_flag(MIDNIGHTCODE_STATUS_FLAG_OKAY);
  }




  // Moving Servo device
  midnightcode_status_text(MIDNIGHTCODE_STATUS_TYPE_TEXT,
                           MIDNIGHTCODE_STATUS_STYLE_NORMAL,
                           "Moving servo device");
  if(midnightcode_servo_move(servo, cfg_movement) < 0) {
    midnightcode_status_flag(MIDNIGHTCODE_STATUS_FLAG_FAIL);
    fprintf(stdout, "\n");
    exit(1);
  } else {
    midnightcode_status_flag(MIDNIGHTCODE_STATUS_FLAG_OKAY);
  }




  // Success!
  midnightcode_status_text(MIDNIGHTCODE_STATUS_TYPE_BLURB,
    MIDNIGHTCODE_STATUS_STYLE_UNKNOWN,
    "Servo [%d:%d] has moved to %d",
    cfg_controller, cfg_servo, cfg_movement);


  fprintf(stdout, "\n");
  fflush(stdout);


  exit(0);
}

