/****************************************************************************
    This file contains NXT-specific functions, which should generally
    not be called directly from application programs.
****************************************************************************/

#include <stdio.h>
#include <string.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <usb.h>
#include "legoctl.h"

extern int  Debug;

int     nxt_open(nxt_t *nxt)

{
    return nxt_open_usb(nxt);
}


int     nxt_open_usb(nxt_t *nxt)

{
    char    response[NXT_RESPONSE_MAX+1];
    int     bytes;

    /* No error return code listed in libusb docs.  Is this call
       supposed to always succeed? */
    nxt->usb_hdl = usb_open(nxt->usb_dev);
    //fprintf(stderr, "usb_open returned %p...\n", brick->nxt.usb_hdl);

    /*
     * ret = usb_set_configuration(brick->nxt.usb_hdl, 1); if (ret < 0) {
     * usb_close(brick->nxt.usb_hdl); return LCT_NXT_CONFIGURATION_ERROR; }
     */
    if ( usb_claim_interface(nxt->usb_hdl, NXT_USB_INTERFACE) < 0 )
    {
	fputs("nxt_open_usb(): Error: Could not claim interface.\n",stderr);
	usb_close(nxt->usb_hdl);
	return NXT_IN_USE;
    }

    /* NXT handshake */
    bytes = nxt_send_cmd(nxt,NXT_DIRECT_CMD,
			NXT_DC_BATTERY_LEVEL,response,
			NXT_RESPONSE_MAX);
    
    if (bytes != 5)
    {
	usb_release_interface(nxt->usb_hdl, 0);
	usb_close(nxt->usb_hdl);
	fputs("Handshake failed...\n", stderr);
	return NXT_HANDSHAKE_FAILED;
    }
    return LCT_OK;
}


int     nxt_get_battery_level(nxt_t *nxt)

{
    int     battery_level,
	    bytes;
    char    response[NXT_RESPONSE_MAX + 1];

    /*
     * NXT brick should respond with 5 bytes: 
     *  reply command status level-lsb level-msb
     */
    bytes = nxt_send_cmd(nxt, NXT_DIRECT_CMD,
			NXT_DC_BATTERY_LEVEL, response,
			NXT_RESPONSE_MAX);
    nxt_dump_response(response,bytes,"NXT_SC_BATTERY_LEVEL");
    if ( bytes == 5 )
    {
	battery_level = nxt->battery_level = buf2short(response+3);
	return battery_level;
    }
    else
    {
	fputs("nxt_get_battery_level() failed.\n",stderr);
	return 1;
    }
}


int     nxt_print_battery_level(nxt_t *nxt)

{
    nxt_get_battery_level(nxt);
    printf("Battery level = %dmV.\n", nxt->battery_level);
    printf("(Maximum expected is 9000mV for alkaline batteries, less for rechargeables.)\n");
    return 0;
}


int     nxt_get_device_info(nxt_t *nxt)

{
    int     bytes;
    char    response[NXT_RESPONSE_MAX + 1];

    /*
     * NXT brick should respond with 5 bytes: 
     *  reply command status level-lsb level-msb
     */
    bytes = nxt_send_cmd(nxt, NXT_SYSTEM_CMD,
			NXT_SC_GET_DEVICE_INFO, response,
			NXT_RESPONSE_MAX);
    if ( bytes == 33 )
    {
	strlcpy(nxt->name,response+3,NXT_NAME_LEN);
	memcpy(nxt->bt_address,response+18,6);
	nxt->bt_signal = buf2long(response+24);
	nxt->free_flash = buf2long(response+28);
    }
    else
    {
	fputs("nxt_get_device_info() failed.\n",stderr);
	return 1;
    }
    return 0;
}


int     nxt_print_device_info(nxt_t *nxt)

{
    int     status;
    
    status = nxt_get_device_info(nxt);
    if ( status == 0 )
    {
	printf("Device name: %s\n",nxt->name);
	printf("Bluetooth address: %02x:%02x:%02x:%02x:%02x:%02x\n",
		nxt->bt_address[0], nxt->bt_address[1],
		nxt->bt_address[2], nxt->bt_address[3],
		nxt->bt_address[4], nxt->bt_address[5]);
	printf("Bluetooth signal strength: %lu\n",nxt->bt_signal);
	printf("Free flash RAM: %luk\n",nxt->free_flash/1024);
    }
    return status;
}


int     nxt_get_firmware_version(nxt_t *nxt)

{
    int     bytes;
    char    response[NXT_RESPONSE_MAX + 1];

    /*
     * NXT brick should respond with 5 bytes: 
     *  reply command status level-lsb level-msb
     */
    bytes = nxt_send_cmd(nxt, NXT_SYSTEM_CMD,
			NXT_SC_GET_VERSIONS, response,
			NXT_RESPONSE_MAX);
    if ( bytes == 7 )
    {
	nxt->firmware_major = response[6];
	nxt->firmware_minor = response[5];
	nxt->protocol_major = response[4];
	nxt->protocol_minor = response[3];
    }
    else
    {
	fputs("nxt_get_firmware_version(): command failed.\b",stderr);
	return LCT_FAILURE;
    }
    return 0;
}


int     nxt_print_firmware_version(nxt_t *nxt)

{
    nxt_get_firmware_version(nxt);
    printf("Firmware: %d.%02d\n",nxt->firmware_major,nxt->firmware_minor);
    printf("Protocol: %d.%d\n",nxt->protocol_major,nxt->protocol_minor);
    return 0;
}


/****************************************************************************
Name:

Description: 
 
Author: Jason W. Bacon

Returns: 0 on error, otherwise the length of the response from the NXT.
****************************************************************************/

int     nxt_send_cmd(nxt_t * nxt, int cmd_type, int cmd, 
		    char *response,int response_max)

{
    int     bytes = 0;
    char    cmd_buff[2];
    
    if ( !nxt_is_open(nxt) )
    {
	fputs("nxt_send_cmd(): Error: NXT is not currently open.\n",stderr);
    }
    else
    {
	cmd_buff[0] = cmd_type;
	cmd_buff[1] = cmd;
	if (nxt_send_buf(nxt, cmd_buff, 2) == 2)
	{
	    bytes = nxt_recv_buf(nxt, response, response_max);
	}
	else
	{
	    fprintf(stderr, 
		    "nxt_direct_cmd(): Error: Failed to send command %d.\n",
		    cmd);
	}
    }
    return bytes;
}


int     nxt_send_buf(nxt_t * nxt, char *buf, int len)

{
    int     bytes;

    bytes = usb_bulk_write(nxt->usb_hdl, NXT_USB_OUT_ENDPOINT, buf, len,
			   USB_TIMEOUT);
    return bytes;
}


int     nxt_send_str(nxt_t * nxt, char *str)

{
    return nxt_send_buf(nxt, str, strlen(str));
}


int     nxt_recv_buf(nxt_t * nxt, char *buf, int maxlen)

{
    int     bytes;

    bytes = usb_bulk_read(nxt->usb_hdl, NXT_USB_IN_ENDPOINT, buf, maxlen, USB_TIMEOUT);
    return bytes;
}


int     nxt_close(nxt_t *nxt)

{
    return nxt_close_usb(nxt);
}


int     nxt_close_usb(nxt_t *nxt)

{
    usb_release_interface(nxt->usb_hdl, 0);
    usb_close(nxt->usb_hdl);
    return LCT_OK;
}


int     nxt_validate_filename(char *filename)

{
    char    *base;
    
    /* Strip off any leading path info. The memmove() function
       properly handles overlapping source and dest. */
    if ( (base = strrchr(filename,'/')) != NULL )
	memmove(filename,base+1,strlen(base+1)+1);
    
    if ( strlen(filename) > 20 )
    {
	fprintf(stderr, "nxt_validate_filename(): Error: Filename %s is more than 20 characters.\n",
	    filename);
	return LCT_FAILURE;
    }
    return LCT_OK;
}


int     nxt_upload_program(nxt_t *nxt,char *filename)

{
    int     file_handle,
	    status;
    
    /* Validate file as an NXT program */
    status = nxt_validate_filename(filename);
    if ( status != LCT_OK )
	return status;
    
    /* Upload file */
    fprintf(stderr,"Uploading %s\n",filename);

    file_handle = nxt_open_file(nxt,filename,NXT_WRITE_MODE);
    if ( file_handle != -1 )
    {
	nxt_write_file(nxt,filename,file_handle);
	nxt_close_file(nxt,file_handle);
	return LCT_OK;
    }
    else
    {
	fprintf(stderr,"nxt_upload_file(): Error: Unable to open brick for write.\n");
	nxt_close_file(nxt,file_handle);
	return LCT_FAILURE;
    }
}


int     nxt_delete_file(nxt_t *nxt,char *filename)

{
    int         bytes,
		status;
    char        cmd[23],
		response[NXT_RESPONSE_MAX+1];
    
    status = nxt_validate_filename(filename);
    if ( status != LCT_OK )
	return status;
    
    cmd[0] = NXT_SYSTEM_CMD;
    cmd[1] = NXT_SC_DELETE;
    memset(cmd+2,'\0',20);
    strlcpy(cmd+2,filename,20);
    nxt_dump_cmd(cmd,22,"NXT_SC_DELETE");
    if ( nxt_send_buf(nxt,cmd,22) != 22 )
    {
	fprintf(stderr,"nxt_delete_file(): Error sending delete command.\n");
	return LCT_FAILURE;
    }
    bytes = nxt_recv_buf(nxt,response,NXT_RESPONSE_MAX);
    nxt_dump_response(response,bytes,"NXT_SC_DELETE");
    return 0;
}


int     nxt_start_program(nxt_t *nxt,char *filename)

{
    int         bytes,
		status;
    char        cmd[23],
		response[NXT_RESPONSE_MAX+1];
    
    status = nxt_validate_filename(filename);
    if ( status != LCT_OK )
	return status;
    
    cmd[0] = NXT_DIRECT_CMD;
    cmd[1] = NXT_DC_START_PROGRAM;
    memset(cmd+2,'\0',20);
    strlcpy(cmd+2,filename,20);
    nxt_dump_cmd(cmd,22,"NXT_DC_START_PROGRAM");
    if ( nxt_send_buf(nxt,cmd,22) != 22 )
    {
	fprintf(stderr,"nxt_start_program(): Error sending start command.\n");
	return LCT_FAILURE;
    }
    bytes = nxt_recv_buf(nxt,response,NXT_RESPONSE_MAX);
    nxt_dump_response(response,bytes,"NXT_SC_DELETE");
    return 0;
}


int     nxt_stop_program(nxt_t *nxt)

{
    int         bytes;
    char        response[NXT_RESPONSE_MAX+1];
    
    bytes = nxt_send_cmd(nxt,NXT_DIRECT_CMD,
			NXT_DC_STOP_PROGRAM,response,
			NXT_RESPONSE_MAX);
    
    if (bytes != 3)
    {
	usb_release_interface(nxt->usb_hdl, 0);
	usb_close(nxt->usb_hdl);
	fputs("nxt_stop_program(): Error sending stop command.\n", stderr);
	return LCT_FAILURE;
    }
    return LCT_OK;
}


int     nxt_open_file(nxt_t *nxt,char *filename,nxt_open_mode_t mode)

{
    int         bytes,
		status,
		file_handle;
    char        cmd[27],
		response[NXT_RESPONSE_MAX+1];
    struct stat st;
    
    cmd[0] = NXT_SYSTEM_CMD;
    cmd[1] = NXT_SC_OPEN_WRITE_DATA;
    memset(cmd+2,'\0',20);
    strlcpy(cmd+2,filename,20);
    
    /* Add file-size in guaranteed little-endian format */
    if ( stat(filename,&st) != 0 )
    {
	fprintf(stderr,"nxt_upload_prog(): Cannot stat %s.\n",filename);
	return LCT_FAILURE;
    }
    long2buf(cmd+22,st.st_size);
    
    if ( Debug )
    {
	fprintf(stderr,"File size in st is %u\n",(unsigned)st.st_size);
	fprintf(stderr,"File size in cmd is %ld\n",*(long *)(cmd+22));
    }
    nxt_dump_cmd(cmd,26,"NXT_SC_OPEN_WRITE_DATA");
    
    if ( nxt_send_buf(nxt,cmd,26) != 26 )
    {
	fprintf(stderr,"nxt_upload_file(): Error sending open command.\n");
	return LCT_FAILURE;
    }
    bytes = nxt_recv_buf(nxt,response,NXT_RESPONSE_MAX);
    nxt_dump_response(response,bytes,"NXT_SC_OPEN_WRITE_DATA");
    if ( bytes != 4 )
    {
	fprintf(stderr,"Bad response to open command.  Got %d bytes, expected 4.\n",bytes);
	return LCT_FAILURE;
    }
    status = response[2];
    if ( status == 0 )
    {
	file_handle = (unsigned char)response[3];
	return file_handle;
    }
    else
	return -1;
}


int     nxt_write_file(nxt_t *nxt,char *filename,int file_handle)

{
    int     fd,
	    bytes;
    char    buff[65],
	    response[NXT_RESPONSE_MAX+1];
    
    fd = open(filename,O_RDONLY);
    if ( fd < 0 )
    {
	fprintf(stderr,"Cannot open %s.\n",filename);
	return LCT_FAILURE;
    }
    
    buff[0] = NXT_SYSTEM_CMD;
    buff[1] = NXT_SC_WRITE;
    buff[2] = file_handle;
    while ( (bytes = read(fd,buff+3,61)) > 0 )
    {
	memset(buff+3+bytes,'\0',61-bytes); /* Null pad */
	bytes = nxt_send_buf(nxt,buff,64);
	bytes = nxt_recv_buf(nxt,response,NXT_RESPONSE_MAX);
	nxt_dump_response(response,bytes,"NXT_SC_WRITE");
    }
    
    close(fd);
    return LCT_OK;
}


int     nxt_close_file(nxt_t *nxt,int file_handle)

{
    int     bytes;
    char    buff[3],
	    response[NXT_RESPONSE_MAX+1];
    
    buff[0] = NXT_SYSTEM_CMD;
    buff[1] = NXT_SC_CLOSE;
    buff[2] = file_handle;
    bytes = nxt_send_buf(nxt,buff,3);
    if ( bytes != 3 )
    {
	fprintf(stderr,"Error sending close command.\n");
	return LCT_FAILURE;
    }
    
    bytes = nxt_recv_buf(nxt,response,NXT_RESPONSE_MAX);
    if ( bytes != 4 )
    {
	fprintf(stderr,"Error closing file.  Received %d byte, expected 4.\n",bytes);
	return LCT_FAILURE;
    }
    return 0;
}


int     nxt_is_open(nxt_t *nxt)

{
    return ( nxt->usb_hdl != NULL );
}


void    nxt_init(nxt_t *nxt)

{
    nxt->usb_hdl = NULL;
    nxt->usb_dev = NULL;
    nxt->is_in_reset_mode = 0;
}


short   buf2short(char *buf)

{
    return buf[0] + (buf[1] << 8);
}


long    buf2long(char *buf)

{
    return buf[0] + (buf[1] << 8) + (buf[2] << 16) + (buf[3] << 24);
}

void    long2buf(char *buf,long val)

{
    buf[0] = (val & 0x000000ff);
    buf[1] = (val & 0x0000ff00) >> 8;
    buf[2] = (val & 0x00ff0000) >> 16;
    buf[3] = (val & 0xff000000) >> 24;
}

void    nxt_dump_response(char *response,int bytes,char *cmd)

{
    int     c;
    
    if ( Debug )
    {
	printf("Received %d bytes in response to %s.\n",bytes,cmd);
	for (c = 0; c < bytes; ++c)
	    printf("%02x ", (unsigned char)response[c]);
	putchar('\n');
    }
}


void    nxt_dump_cmd(char *cmd,int bytes,char *cmd_name)

{
    int     c;
    
    if ( Debug )
    {
	printf("Sending %s command: %d bytes.\n",cmd_name,bytes);
	for (c = 0; c < bytes; ++c)
	    printf("%02x ", (unsigned char)cmd[c]);
	putchar('\n');
    }
}