/*
    Orpp motioncontroller interface
    (C) 2005 Jason Hunt
    nulluser@gmail.com

    File: orpp.cpp

    Windows version
*/

#include <windows.h>
#include <stdio.h>
#include <time.h>

#include "orpp.h"

#define TIME_OUT 1.00  // Serial timeout in seconds

//#define DEBUG

/* Base constructor */
orpp_class::orpp_class( void )
{
    error_function = NULL;
}    
/* End of :: */

/* Contructior for the board, open serial port */
orpp_class::orpp_class(char *serial_port)
{
    error_function = NULL;
    set_connection(serial_port);
}
/* End of constructor */


/* Network Constructor for orpp board */
orpp_class::orpp_class(char *host, unsigned int port)
{
    error_function = NULL;
    set_connection(host, port);
}
/* End of constructor */


/* Shutdown and close the serial port */
orpp_class::~orpp_class(void)
{
    if (connection == orpp_connection_serial)  CloseHandle(comm);
    if (connection == orpp_connection_network) WSACleanup();
}
/* End of destructor */


/****************************
** Low level communication **
****************************/

/* Configures for serial port connection */
void orpp_class::set_connection(char *serial_port)
{
    connection = orpp_connection_serial;
    strncpy(serial_port_name, serial_port, 64);
}
/* End of Get connection */


/* Configures for network connection */
void orpp_class::set_connection(char *host, unsigned int port)
{
    connection = orpp_connection_network;

    strncpy(host_name, host, 128);
    network_port = port; // Reset to known state    
}
/* End of set_connection */


/* Connect to the board */
// This is called during startup and whenever
// communication is lost
void orpp_class::connect( void )
{
    message("Connecting to ORPP");

    if (connection == orpp_connection_serial)  connect_serial();
    if (connection == orpp_connection_network) connect_network();   
        
    clear();
}
/* End of connect */


/* Connect to locally attached board */
void orpp_class::connect_serial( void )
{
    comm_ok = false;

    CloseHandle(comm);
 
    comm = CreateFile(serial_port_name, GENERIC_READ | GENERIC_WRITE, 
                       0, 0, OPEN_EXISTING, 0,0);
            
    if (comm == INVALID_HANDLE_VALUE)
    {
        error("Unable to open comm port");
        return;
    }  

    if ( SetupComm(comm, 512, 512) == 0)
    {
        error("Unable to setup comm buffers");
        return;
    }
              
    if (PurgeComm(comm, PURGE_RXCLEAR | PURGE_TXCLEAR) == 0)
    {
        error("Unable to purge comm port");
        return;
    }    
              
    DCB dcb = {0};
    dcb.DCBlength = sizeof(DCB);

    if (!GetCommState (comm, &dcb))     // get origional state
    {
        error("Unable to get comm state");        
        return;
    }

    dcb.BaudRate = CBR_19200;
    dcb.ByteSize = 8;
    dcb.Parity   = NOPARITY;
    dcb.StopBits = ONESTOPBIT;

    if (!SetCommState (comm, &dcb))     // Set baud rate, etc
    {
        error("Unable to set comm state");       
        return;
    }   
    
    // Setup timeouts
    COMMTIMEOUTS timeouts;

    timeouts.ReadIntervalTimeout = 0; 
    timeouts.ReadTotalTimeoutMultiplier = 0;
    timeouts.ReadTotalTimeoutConstant = 100;
    timeouts.WriteTotalTimeoutMultiplier = 0;
    timeouts.WriteTotalTimeoutConstant = 100;

    if (!SetCommTimeouts(comm, &timeouts))
    {
        error("Unable to set timeouts");
        return;
    }  

    message("Conncted to ORPP via serial port");
    
    comm_ok = true;
}
/* End of connect serial */


/* Connect to the board over tcp/ip */
void orpp_class::connect_network( void )
{
    comm_ok = false;

    WSADATA wsda;
    
    if(WSAStartup(MAKEWORD(1, 1), &wsda)) return;
    
    // create the socket
    server_socket = socket(AF_INET, SOCK_STREAM,0);

    if (server_socket == INVALID_SOCKET) return;
    
    BOOL bOptVal = TRUE;
    int bOptLen = sizeof(BOOL);

    // Defeat Nagel
    if (setsockopt(server_socket, IPPROTO_TCP, TCP_NODELAY, (char*)&bOptVal, bOptLen) == SOCKET_ERROR) return;

    LPHOSTENT hostent;

    hostent = gethostbyname(host_name);

    if (!hostent) return;

    // Setup address information 
    SOCKADDR_IN server;

    server.sin_addr=*((LPIN_ADDR)*hostent->h_addr_list);
    server.sin_port=htons(network_port);
    server.sin_family=AF_INET;

    if (::connect(server_socket,(LPSOCKADDR)&server, sizeof(struct sockaddr)) != 0) 
    {
        error("Unable to connect to server");
        return;
    }    
                
    message("Connected to ORPP via network");
        
    comm_ok = true;
}
/* end of conenct network */


/***********************
** Utility Functions  **
***********************/

/* Send the clear command five times */
// This will ensure that the serial system is ready for a byte
// Normally called on startup
void orpp_class::clear(void)
{
    byte packet[16], i = 0;
    
    // if network connect, form a proper packet
    if (connection == orpp_connection_network)
    {
        packet[i++] = 0x10; // Command for orpp redirection
        packet[i++] = 0x0;  // Number of bytes for parameters
        packet[i++] = 0x0;  // Number of bytes for parameters
    }

    packet[i++] = 'c'; // Clear command
    
    send_data((byte*)packet, i);
        
    // Init to board starting values
    for (int i = 0; i < NUM_CONTROL; i++)
        for (int j = 0; j < MAX_DEVICE; j++)
            states[i][j] = commands[i].inital_value;
}
/* End of clear */


/* Convert a hex nibble to a number value  */
byte inline to_num(byte c)
{
	if (c > '9')                      // Hex?
	{  
		c &= 0xdf;                    // Force upper case
		return(c - 'A' + 10);         // Return numeric			
	}
	else
		return(c - '0');              //Return numeric
}
/* End of get_hex_nib */


/* Converts number to hex char */
byte inline to_hex(byte c)
{
    if (c > 9)
        return(c + 'a' - 10);
    else
        return(c + '0');     
}
/* End of to_char */


/******************************
** End of Utility Functions  **
******************************/


/*****************************
** Communications functions **
*****************************/

/* Send data to the orpp board */
void orpp_class::send_data(byte c)
{
#ifdef DEBUG
    FILE *f = fopen("log.txt", "at");
    fprintf(f, "(%c)\n", c);
    fclose(f);
#endif
    
    if (!comm_ok) return;

    DWORD sent;             // Number of bytes sent by function

    // Send byte to serial port
    if (connection == orpp_connection_serial)
        if (!WriteFile (comm, &c, 1, &sent, NULL)) comm_ok = 0;

    if (connection == orpp_connection_network)    
        sent = send(server_socket, (char *)&c, 1, 0);

    if (sent != 1) 
    {
        comm_ok = 0;   // No bytes sent
        error("Unable to send data");
    }                
}
/* End of orpp_class::send */


/* Send data to the orpp board */
void orpp_class::send_data(byte *packet, unsigned int size)
{
#ifdef DEBUG
    FILE *f = fopen("log.txt", "at");
    fprintf(f, "packet: ");

    for (int i = 0; i < size; i++)
        fprintf(f, " %x ", packet[i]);
    fprintf(f, "\n");

    fclose(f);
#endif

    if (!comm_ok) return;
        
    DWORD sent;             // Number of bytes sent by function
    
    // Send byte to serial port
    if (connection == orpp_connection_serial)
        if (!WriteFile (comm, packet, size, &sent, NULL)) comm_ok = 0;

    if (connection == orpp_connection_network)    
        sent = send(server_socket, (char *)packet, size, 0);
  
    if (sent != size) 
    {
        comm_ok = 0;   // No bytes sent
        error("Unable to send data");
    }                
}
/* End of orpp_class::send */


/* Send data string to the orpp board */
void orpp_class::send_data_hex(unsigned char c)
{   
    send_data(to_hex(c >> 4));          // Upper nibble
    send_data(to_hex(c & 0x0f));        // Lower nibble
}
/* End of orpp_class::send */


/* Read data from orpp */
char orpp_class::read_data( void )
{
    if (!comm_ok) return(0);

     DWORD read = 0;
         
     byte ch;
    
     int time_out = 0;
    
     time_t s = clock(); // Starting time
 
     // Wait for byte
     while (read != 1 && !time_out)
     {
         if (connection==orpp_connection_serial)
             ReadFile (comm, &ch, 1, &read, NULL);  

         if (connection==orpp_connection_network)
             read = recv(server_socket, (char *)&ch, 1, 0);

        // Update timmeout
        if ((clock() - s) / (double)CLOCKS_PER_SEC > TIME_OUT) time_out = 1;
    }    
 
    // Bail if a timeout occurred
    if (time_out)
    {
        comm_ok = 0;
        message("Read data timeout");
        return(0);
    }                    

#ifdef DEBUG
    FILE *f = fopen("log.txt", "at");
    fprintf(f, "(%x)", ch);
    fclose(f);
#endif

    return(ch);
}
/* End of read */


/* Read a two char hex value from the board */
byte orpp_class::read_data_hex(void)
{
    byte h = to_num(read_data());        // High byte
    byte l = to_num(read_data());        // Low byte
    
    return(h << 4 | l);
}
/* End of read hex */


/* Read a 4 byte hex value form the board */
word orpp_class::read_data_word(void)
{
    byte h = read_data_hex();            // High bytre
    byte l = read_data_hex();            // Low byte
    
    return(h << 8 | l);
}
/* End of read word */


/************************************
** End of communications functions **
************************************/



/* Set an output */
void orpp_class::set(control_type c, byte num, byte param)
{
#ifdef DEBUG
    FILE *f = fopen("log.txt", "at");
    fprintf(f, "set command\n");
    fclose(f);
#endif 

    if (c > NUM_CONTROL) return;

    if (num >= commands[c].num_device) return;  // Range check device number

    byte packet[16], i = 0;

    if (connection == orpp_connection_network)
    {
        packet[i++] = 0x10; // Command for orpp redirection

        if (commands[c].param == param_digit) packet[i++] = 3; else
        if (commands[c].param == param_hex) packet[i++] = 4;

        // Wait for ping response
        packet[i++] = 0x1; // number of bytes for rec data
    }

    packet[i++] = commands[c].command_char;
    packet[i++] = to_hex(num);
    
    // decode parameters
    if (commands[c].param == param_digit)       // Is the param a digit ?
         packet[i++] =  to_hex(param) ;                   // Send as single hex char

    if (commands[c].param == param_hex)         // Is the param two hex nibbles?
    {
      packet[i++] = to_hex(param >> 4);
      packet[i++] = to_hex(param & 0x0f);
    }

    packet[i++] = '.'; // append a ping command
    
    send_data((byte*)packet, i);
    
    
    if (read_data() != '.') comm_ok = 0;        
                
                        
    states[c][num] = param;                     // Save the current state
    
#ifdef DEBUG
    f = fopen("log.txt", "at");
    fprintf(f, "end set command\n");
    fclose(f);
#endif
    
}
/* End of set */


/* Setup a control for set_all */
void orpp_class::set_next(control_type c, byte num, byte param)
{
    if (c > NUM_CONTROL) return;

    if (num >= commands[c].num_device) return;  // Range check device number

    states[c][num] = param;
}
/* Snd of set next */


/* Set all of the outputs at once */
void orpp_class::set_all( void )
{
    byte packet[16], i = 0;
    
    if (connection == orpp_connection_network)
    {
        packet[i++] = 0x10; // Command for orpp redirection
        packet[i++] = 10;   // Number of parameters
        packet[i++] = 0;    // Number of bytes for rec data
    }

    packet[i++] = 't';

    // Add servo data
    for (int j = 0; j < 6; j++) packet[i++] = states[orpp_servo][j];

    // Add pwn outputs
    for (int j = 0; j < 2; j++) packet[i++] = states[orpp_pwm][j];

    byte digital = 0;    
    byte mask = 1;
    
    for (int j = 0; j < 6; j++)
    {
        if (states[orpp_digital_out][j])
        digital |= mask;
        
        mask <<= 1;       
    }    
        
    packet[i++] = digital; // Add digital outputs

    digital = 0; // Clear
      
    // set led outs
    if (states[orpp_led][0]) digital |= 0x01;
    if (states[orpp_led][1]) digital |= 0x02;

    // set open collectors outs
    if (states[orpp_opencollector][0]) digital |= 0x04;
    if (states[orpp_opencollector][1]) digital |= 0x08;

    packet[i++] = digital; // Set other outputs

    packet[i++] = '.'; // Append a ping command
    
    send_data((byte*)packet, i);
        
    if (read_data() != '.') comm_ok = 0;        
}
/* End of set all */



/* Add to a saved value, and set the new value. Used mostly for PWM */
void orpp_class::add(control_type c, byte num, int delta)
{
    if (c > NUM_CONTROL) return;
    if (num > commands[c].num_device) return; // Range check
   
    int new_state = states[c][num] + delta;   // Create new value
   
    if (new_state > 255) new_state = 255;     // Range clip
    if (new_state < 0) new_state = 0;
   
    states[c][num] = new_state;               // Save new state
   
    set(c, num, new_state);                   // Send to board
}

/* End of add */


/* Toggle an output */
void orpp_class::toggle(control_type c, byte num)
{
    if (c > NUM_CONTROL) return;
    if (num > commands[c].num_device) return; // Range check
   
    states[c][num] = !states[c][num];         // Preforn toggle
   
    set(c, num, states[c][num]);              // Send to board
}
/* End of toggle */


/* Get a value from the board */
// If the control is an output, it will return the stored value
//
unsigned int orpp_class::get(control_type c, byte num)
{
#ifdef DEBUG
    FILE *f = fopen("log.txt", "at");
    fprintf(f, "get command\n");
    fclose(f);
#endif

    if (c > NUM_CONTROL) return(0);   
    if (num >= commands[c].num_device) return(0); // Range check    
      
    if (commands[c].ret == param_none) return(states[c][num]); // It is an output ?

    byte packet[16], i = 0;

    // if network connect, form a proper packet    
    if (connection == orpp_connection_network)
    {
        packet[i++] = 0x10; // Command for orpp redirection
        packet[i++] = 0x1;  // Number of bytes for parameters

        // decode return length
        if (commands[c].ret == param_digit) packet[i++] = 1; else
        if (commands[c].ret == param_hex)  packet[i++] = 2; else
        if (commands[c].ret == param_word) packet[i++] = 4; 
    }

    packet[i++] = commands[c].command_char;
    packet[i++] = to_hex(num); 
    
    send_data((byte*)packet, i);  // Send control number

    // The expected value is a digit
    if (commands[c].ret == param_digit) states[c][num] = to_num(read_data());

    // The expect value is 4 hex digits
    if (commands[c].ret == param_hex) states[c][num] = read_data_hex();       
     
    // The expect value is 4 hex digits
    if (commands[c].ret == param_word) states[c][num] = read_data_word();       

#ifdef DEBUG
    f = fopen("log.txt", "at");
    fprintf(f, "end get command\n");
    fclose(f);
#endif

    return(states[c][num]); 
}
/* End of get */


/* Return the last known value of the control */
unsigned int orpp_class::get_last(control_type c, byte num)
{
    if (c > NUM_CONTROL) return(0);               // Range check
    if (num >= commands[c].num_device) return(0); 
        
    return(states[c][num]);                       // Return the last known state
}
/* End of get */


/* Get values from the orpp board all at once */
void orpp_class::get_all(void)
{
    char packet[16];

    unsigned int i = 0;

    // if network connect, form a proper packet
    if (connection == orpp_connection_network)
    {
        packet[i++] = 0x10; // command for orpp redirection
        packet[i++] = 0x0; // number of bytes for parameters
        packet[i++] = 0x0a; // number of bytes for parameters
    }

    packet[i++] = 'g';
    
    send_data((byte*)packet, i);

    unsigned char low, high;
    
    for (int i = 0; i < 5; i++)     // Read each packed value
    {
        high = read_data();
        low  = read_data();
        
        // Extract the digital in
        states[orpp_digital_in][i] = (high & 0x80) == 0x80; 
        
        high &= 0x03; // Mask it off
        
        // Combine bytes and normalize to [0..1]
        states[orpp_analog][i] = (high << 8 | low);
    }    
}
/* End of turbo transfer */

/* Configure the error function */
void orpp_class::error(char * error)
{
    // Redirect to the error function, if defined
    if (error_function != NULL)
        error_function(error);
}    
/* End of error */

/* Configure the message function */
void orpp_class::message(char * message)
{
    // rRdirect to the error function, if defined
    if (message_function != NULL)
        message_function(message);
}    
/* End of message */