/*
    Robot server - 2005 Jason Hunt
    
    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; either version 2
    of the License, or (at your option) any later version.

    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
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

    Contact: huntjas2@msu.edu
*/

#include <stdio.h>
#include <memory.h>
#include <termios.h> 
#include <fcntl.h> 
#include <unistd.h>

#include "controller.h"


unsigned char c_data[4]; // controller data
              
int serial_fd;


/* open serial port and reset controller */
void reset_serial( void )
{
    printf("Reset serial\n"); 

    serial_fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);

    if (serial_fd == -1)
    {
        printf("[network]: Unable to open serial port\n");
    }
    else
    if (fcntl(serial_fd, F_SETFL, 0) )
    {
        printf("fcntl trouble\n");
    }

    struct termios options;

    tcgetattr(serial_fd, &options);

    cfsetispeed(&options, B9600);
    cfsetospeed(&options, B9600);
    options.c_cflag |= (CLOCAL | CREAD);

   // options.c_cflag &= ~CSIZE; 
//    options.c_cflag |= CS8;    

    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;

    options.c_oflag &= ~OPOST;

    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    tcsetattr(serial_fd, TCSANOW, &options);


//    for (int i = 0; i < 5; i++)
//        if (write(serial_fd, "r", 1) != 1)
//            printf("[Serial]: Write failed\n");
}
/* end of reset serial */


/* convert a nibble into ascii hex */
unsigned char hex_nib(unsigned char c)
{
    if (c < 10)
        return(c + '0');
    else
        return(c-10+'A');
}
/* end of hex_nib */


/* send motor and servo data to the serial port */
void send_control_packet(int serial_fd)
{
/*    char buff[10];

    buff[0] = 'm' ; // motor control
    buff[1] = hex_nib(c_data[0] >> 4);
    buff[2] = hex_nib(c_data[0] & 0x0f);
    buff[3] = hex_nib(c_data[1] >> 4);
    buff[4] = hex_nib(c_data[1] & 0x0f);
    write(serial_fd, buff, 5);

    buff[0] = 's' ; // motor control
    buff[1] = hex_nib(c_data[2] >> 4);
    buff[2] = hex_nib(c_data[2] & 0x0f);
    buff[3] = hex_nib(c_data[3] >> 4);
    buff[4] = hex_nib(c_data[3] & 0x0f);
    write(serial_fd, buff, 5);*/
}
/* end of send_control_packet */






void send_serial( char s)
{
    char b[1];
    b[0] = s;


//    printf("send: (%x)", s);
    write(serial_fd, b, 1);
  //  printf("done\n");
}





/* send serial data to the motor controller */
void update_controller( void )
{
//  printf("Controller data: %X %X %X %X\n", c_data[0], c_data[1], c_data[2], c_data[3]);

    // send the new data
    send_control_packet(serial_fd); 

//  printf("Controller updated\n");
}
/* end of update_controller */


/* reset the motor controller to the default positions */
void reset_controller(void)
{
    reset_serial();

    printf("Reset controller\n");

    send_serial(0x50);

/*    c_data[0] = 0;
    c_data[1] = 0;
    c_data[2] = 0x80;
    c_data[3] = 0x80;

    fire = 0;
*/

 //   update_controller();
}
/* end of reset_controller */


/* close the serial port */
void shutdown_controller(void)
{
    close(serial_fd);
}
/*end of shutdown_controller */








