text:
package netcode

import (
	"errors"
	"log"
	"net"
	"time"
)

const CLIENT_MAX_RECEIVE_PACKETS = 64
const PACKET_SEND_RATE = 10.0
const NUM_DISCONNECT_PACKETS = 10 // number of disconnect packets the client/server should send when disconnecting

type Context struct {
	WritePacketKey []byte
	ReadPacketKey  []byte
}

type ClientState int8

const (
	StateTokenExpired               ClientState = -6
	StateInvalidConnectToken                    = -5
	StateConnectionTimedOut                     = -4
	StateConnectionResponseTimedOut             = -3
	StateConnectionRequestTimedOut              = -2
	StateConnectionDenied                       = -1
	StateDisconnected                           = 0
	StateSendingConnectionRequest               = 1
	StateSendingConnectionResponse              = 2
	StateConnected                              = 3
)

var clientStateMap = map[ClientState]string{
	StateTokenExpired:               "connect token expired",
	StateInvalidConnectToken:        "invalid connect token",
	StateConnectionTimedOut:         "connection timed out",
	StateConnectionResponseTimedOut: "connection response timed out",
	StateConnectionRequestTimedOut:  "connection request timed out",
	StateConnectionDenied:           "connection denied",
	StateDisconnected:               "disconnected",
	StateSendingConnectionRequest:   "sending connection request",
	StateSendingConnectionResponse:  "sending connection response",
	StateConnected:                  "connected",
}

type Client struct {
	id           uint64
	connectToken *ConnectToken

	time                  float64
	startTime             float64
	lastPacketSendTime    float64
	lastPacketRecvTime    float64
	shouldDisconnect      bool
	state                 ClientState
	shouldDisconnectState ClientState
	sequence              uint64
	challengeSequence     uint64

	clientIndex   uint32
	maxClients    uint32
	serverIndex   int
	address       *net.UDPAddr
	serverAddress *net.UDPAddr

	challengeData    []byte
	context          *Context
	replayProtection *ReplayProtection
	conn             *NetcodeConn
	packetQueue      *PacketQueue
	allowedPackets   []byte
	packetCh         chan *NetcodeData
}

func NewClient(connectToken *ConnectToken) *Client {
	c := &Client{connectToken: connectToken}

	c.lastPacketRecvTime = -1
	c.lastPacketSendTime = -1
	c.packetCh = make(chan *NetcodeData, PACKET_QUEUE_SIZE)
	c.setState(StateDisconnected)
	c.shouldDisconnect = false
	c.challengeData = make([]byte, CHALLENGE_TOKEN_BYTES)

	c.context = &Context{}
	c.packetQueue = NewPacketQueue(PACKET_QUEUE_SIZE)
	c.replayProtection = NewReplayProtection()

	c.allowedPackets = make([]byte, ConnectionNumPackets)
	c.allowedPackets[ConnectionDenied] = 1
	c.allowedPackets[ConnectionChallenge] = 1
	c.allowedPackets[ConnectionKeepAlive] = 1
	c.allowedPackets[ConnectionPayload] = 1
	c.allowedPackets[ConnectionDisconnect] = 1
	return c
}

func (c *Client) GetState() ClientState {
	return c.state
}

func (c *Client) SetId(id uint64) {
	c.id = id
}

func (c *Client) setState(newState ClientState) {
	c.state = newState
}

func (c *Client) Connect() error {
	var err error

	c.startTime = 0
	if c.serverIndex > len(c.connectToken.ServerAddrs) {
		return errors.New("invalid server address, exceeded # of servers")
	}

	c.serverAddress = &c.connectToken.ServerAddrs[c.serverIndex]

	c.conn = NewNetcodeConn()
	c.conn.SetRecvHandler(c.handleNetcodeData)
	if err = c.conn.Dial(c.serverAddress); err != nil {
		return err
	}

	c.context.ReadPacketKey = c.connectToken.ServerKey
	c.context.WritePacketKey = c.connectToken.ClientKey

	c.setState(StateSendingConnectionRequest)
	return nil
}

func (c *Client) connectNextServer() bool {
	if c.serverIndex+1 >= len(c.connectToken.ServerAddrs) {
		return false
	}

	c.serverIndex++
	c.serverAddress = &c.connectToken.ServerAddrs[c.serverIndex]

	c.Reset()

	log.Printf("client[%d] connecting to next server %s (%d/%d)\n", c.id, c.serverAddress.String(), c.serverIndex, len(c.connectToken.ServerAddrs))
	if err := c.Connect(); err != nil {
		log.Printf("error connecting to next server: %s\n", err)
		return false
	}
	c.setState(StateSendingConnectionRequest)
	return true
}

func (c *Client) Close() error {
	return c.conn.Close()
}

func (c *Client) Reset() {
	c.lastPacketSendTime = c.time - 1
	c.lastPacketRecvTime = c.time
	c.shouldDisconnect = false
	c.shouldDisconnectState = StateDisconnected
	c.challengeData = make([]byte, CHALLENGE_TOKEN_BYTES)
	c.challengeSequence = 0
	c.replayProtection.Reset()
}

func (c *Client) resetConnectionData(newState ClientState) {
	c.sequence = 0
	c.clientIndex = 0
	c.maxClients = 0
	c.startTime = 0
	c.serverIndex = 0
	c.serverAddress = nil
	c.connectToken = nil
	c.context = nil
	c.setState(newState)
	c.Reset()
	c.packetQueue.Clear()
	c.conn.Close()
}

func (c *Client) LocalAddr() net.Addr {
	return c.conn.LocalAddr()
}

func (c *Client) RemoteAddr() net.Addr {
	return c.conn.RemoteAddr()
}

func (c *Client) Update(t float64) {
	c.time = t

	c.recv()

	if err := c.send(); err != nil {
		log.Printf("error sending packet: %s\n", err)
	}

	state := c.GetState()
	if state > StateDisconnected && state < StateConnected {
		expire := c.connectToken.ExpireTimestamp - c.connectToken.CreateTimestamp
		if c.startTime+float64(expire) <= c.time {
			log.Printf("client[%d] connect failed. connect token expired\n", c.id)
			c.Disconnect(StateTokenExpired, false)
			return
		}
	}

	if c.shouldDisconnect {
		log.Printf("client[%d] should disconnect -> %s\n", c.id, clientStateMap[c.shouldDisconnectState])
		if c.connectNextServer() {
			return
		}
		c.Disconnect(c.shouldDisconnectState, false)
		return
	}

	switch c.GetState() {
	case StateSendingConnectionRequest:
		timeout := c.lastPacketRecvTime + float64(c.connectToken.TimeoutSeconds*1000)
		if timeout < c.time {
			log.Printf("client[%d] connection request timed out.\n", c.id)
			if c.connectNextServer() {
				return
			}
			c.Disconnect(StateConnectionRequestTimedOut, false)
		}
	case StateSendingConnectionResponse:
		timeout := c.lastPacketRecvTime + float64(c.connectToken.TimeoutSeconds*1000)
		if timeout < c.time {
			log.Printf("client[%d] connect failed. connection response timed out\n", c.id)
			if c.connectNextServer() {
				return
			}
			c.Disconnect(StateConnectionResponseTimedOut, false)
		}
	case StateConnected:
		timeout := c.lastPacketRecvTime + float64(c.connectToken.TimeoutSeconds*1000)
		if timeout < c.time {
			log.Printf("client[%d] connection timed out\n", c.id)
			c.Disconnect(StateConnectionTimedOut, false)
		}
	}
}

func (c *Client) recv() {
	// empty recv'd data from channel
	for {
		select {
		case recv := <-c.packetCh:
			c.OnPacketData(recv.data, recv.from)
		default:
			return
		}
	}
}

func (c *Client) Disconnect(reason ClientState, sendDisconnect bool) error {
	log.Printf("client[%d] disconnected: %s\n", c.id, clientStateMap[reason])
	if c.GetState() <= StateDisconnected {
		log.Printf("state <= StateDisconnected")
		return nil
	}

	if sendDisconnect && c.GetState() > StateDisconnected {
		for i := 0; i < NUM_DISCONNECT_PACKETS; i += 1 {
			packet := &DisconnectPacket{}
			c.sendPacket(packet)
		}
	}
	c.resetConnectionData(reason)
	return nil
}

func (c *Client) SendData(payloadData []byte) error {
	if c.GetState() != StateConnected {
		return errors.New("client not connected, unable to send packet")
	}
	p := NewPayloadPacket(payloadData)
	return c.sendPacket(p)
}

func (c *Client) send() error {
	// check our send rate prior to bother sending
	if c.lastPacketSendTime+float64(1.0/PACKET_SEND_RATE) >= c.time {
		return nil
	}

	switch c.GetState() {
	case StateSendingConnectionRequest:
		p := &RequestPacket{}
		p.VersionInfo = c.connectToken.VersionInfo
		p.ProtocolId = c.connectToken.ProtocolId
		p.ConnectTokenExpireTimestamp = c.connectToken.ExpireTimestamp
		p.ConnectTokenSequence = c.connectToken.Sequence
		p.ConnectTokenData = c.connectToken.PrivateData.Buffer()
		log.Printf("client[%d] sent connection request packet to server\n", c.id)
		return c.sendPacket(p)
	case StateSendingConnectionResponse:
		p := &ResponsePacket{}
		p.ChallengeTokenSequence = c.challengeSequence
		p.ChallengeTokenData = c.challengeData
		log.Printf("client[%d] sent connection response packet to server\n", c.id)
		return c.sendPacket(p)
	case StateConnected:
		p := &KeepAlivePacket{}
		p.ClientIndex = 0
		p.MaxClients = 0
		log.Printf("client[%d] sent connection keep-alive packet to server\n", c.id)
		return c.sendPacket(p)
	}

	return nil
}

func (c *Client) sendPacket(packet Packet) error {
	buffer := make([]byte, MAX_PACKET_BYTES)
	packet_bytes, err := packet.Write(buffer, c.connectToken.ProtocolId, c.sequence, c.context.WritePacketKey)
	if err != nil {
		return err
	}

	_, err = c.conn.Write(buffer[:packet_bytes])
	if err != nil {
		log.Printf("error writing packet %s to server: %s\n", packetTypeMap[packet.GetType()], err)
	}
	c.lastPacketSendTime = c.time
	c.sequence++
	return err
}

func (c *Client) RecvData() ([]byte, uint64) {
	packet := c.packetQueue.Pop()
	p, ok := packet.(*PayloadPacket)
	if !ok {
		return nil, 0
	}
	return p.PayloadData, p.sequence
}

// write the netcodeData to our unbuffered packet channel. The NetcodeConn verifies
// that the recv'd data is > 0 < maxBytes and is of a valid packet type before
// this is even called.
func (c *Client) handleNetcodeData(packetData *NetcodeData) {
	c.packetCh <- packetData
}

func (c *Client) OnPacketData(packetData []byte, from *net.UDPAddr) {
	var err error
	var size int
	var sequence uint64

	if !addressEqual(c.serverAddress, from) {
		log.Printf("client[%d] unknown/old server address sent us data %s != %s\n", c.id, c.serverAddress.String(), from.String())
		return
	}

	size = len(packetData)
	timestamp := uint64(time.Now().Unix())

	packet := NewPacket(packetData)
	if err = packet.Read(packetData, size, c.connectToken.ProtocolId, timestamp, c.context.ReadPacketKey, nil, c.allowedPackets, c.replayProtection); err != nil {
		log.Printf("error reading packet: %s\n", err)
	}

	c.processPacket(packet, sequence)
}

func (c *Client) processPacket(packet Packet, sequence uint64) {

	state := c.GetState()
	switch packet.GetType() {
	case ConnectionDenied:
		if state == StateSendingConnectionRequest || state == StateSendingConnectionResponse {
			c.shouldDisconnect = true
			c.shouldDisconnectState = StateConnectionDenied
		}
	case ConnectionChallenge:
		if state != StateSendingConnectionRequest {
			return
		}

		p, ok := packet.(*ChallengePacket)
		if !ok {
			return
		}
		c.challengeData = p.ChallengeTokenData
		c.challengeSequence = p.ChallengeTokenSequence
		c.setState(StateSendingConnectionResponse)
	case ConnectionKeepAlive:
		p, ok := packet.(*KeepAlivePacket)
		if !ok {
			return
		}

		if state == StateSendingConnectionResponse {
			c.clientIndex = p.ClientIndex
			c.maxClients = p.MaxClients
			c.setState(StateConnected)
		}
	case ConnectionPayload:
		if state != StateConnected {
			return
		}

		c.packetQueue.Push(packet)
	case ConnectionDisconnect:
		if state != StateConnected {
			return
		}
		c.shouldDisconnect = true
		c.shouldDisconnectState = StateDisconnected
	default:
		return
	}
	// always update last packet recv time for valid packets.
	c.lastPacketRecvTime = c.time
}


text:
/******************************************************************************
 *
 *  Title:        visiond.c
 *
 *  Description:  Main program for vision daemon.
 *
 *****************************************************************************/

#include "visiond.h"

/// Global file descriptors. Only global so that visiond_exit() can close them.
int server_fd;
CvCapture *f_cam;
CvCapture *b_cam;
IplImage *bin_img;
IplImage *save_img;
DIR *dirp;
FILE *log_file;

/******************************************************************************
 *
 * Title:       void visiond_sigint( int signal )
 *
 * Description: This function is called when SIGINT (ctrl-c) is invoked.
 *
 * Input:       signal: The SIGINT signal.
 *
 * Output:      None.
 *
 *****************************************************************************/

void visiond_sigint( int signal )
{
    exit( 0 );
} /// end visiond_sigint()

/******************************************************************************
 *
 * Title:       void visiond_exit( )
 *
 * Description: Exit function for main program. Closes all file descriptors.
 *              This function is called when SIGINT (ctrl-c) is invoked.
 *
 * Input:       None.
 *
 * Output:      None.
 *
 *****************************************************************************/

void visiond_exit( )
{
    printf("\nVISIOND_EXIT: Shutting down visiond program ... ");

    /// Sleep to let things shut down properly.
    usleep( 200000 );

    /// Close the open file descriptors.
    if( server_fd > 0 ) {
        close( server_fd );
    }

    /// Close the cameras and windows.
    if( f_cam ) {
        cvReleaseCapture( &f_cam );
    }

    if( b_cam ) {
        cvReleaseCapture( &b_cam );
    }

    cvReleaseImage( &bin_img );
    if ( save_img ) {
    	cvReleaseImage( &save_img );
	}

    if ( dirp ) {
    	closedir( dirp );
	}

	if ( log_file ) {
		fclose( log_file );
	}

    printf("<OK>\n\n");
} /// end visiond_exit()

/******************************************************************************
 *
 * Title:       int main( int argc, char *argv[] )
 *
 * Description: Initialize data. Open ports. Run main program loop.
 *
 * Input:       argc: Number of command line arguments.
 *              argv: Array of command line arguments.
 *
 * Output:      None.
 *
 *****************************************************************************/

int main( int argc, char *argv[] )
{
    /// Setup exit function. It is called when SIGINT (ctrl-c) is invoked.
    void( *exit_ptr )( void );
    exit_ptr = visiond_exit;
    atexit( exit_ptr );

    struct sigaction sigint_action;
    sigint_action.sa_handler = visiond_sigint;
    sigint_action.sa_flags = 0;
    sigaction( SIGINT, &sigint_action, NULL );

    /// Set up state variables and initialize them.
    int recv_bytes = 0;
    char recv_buf[MAX_MSG_SIZE];
    CONF_VARS cf;
    MSG_DATA msg;

    /// Set up image/window variables and initialize them.
    IplImage *img = NULL;
	const char *win = "Image";
	const char *binwin = "Binary";
	save_img = NULL;

	/// Set up video variables and initialize them.
	int saving_fvideo = FALSE;
    int saving_bvideo = FALSE;
    CvVideoWriter *f_writer = 0;
    CvVideoWriter *b_writer = 0;
    int is_color = TRUE;
    struct timeval ctime;
    struct tm ct;

    /// Set up file access variables and initialize them.
	struct dirent *dfile = NULL;
	int diropen = FALSE;
	char filename[STRING_SIZE * 2];
	log_file = NULL;
	char write_time[80] = {0};
    char curr_save_dir[STRING_SIZE];
    int vision_classified = 0;
    int vision_considered = 0;

	/// Set up timer variables and intialize them.
	TIMING timer_fps;
	TIMING timer_save;
	TIMING timer_open;
	int dt = 0;
	int nframes = 0;
    double fps = 0.0;

	/// Actually initialize timers.
	timing_set_timer(&timer_fps);
	timing_set_timer(&timer_save);
	timing_set_timer(&timer_open);

    printf( "MAIN: Starting Vision daemon ...\n" );

    /// Initialize variables.
    server_fd = -1;
    memset( &msg, 0, sizeof(MSG_DATA) );
	messages_init( &msg );

	/// Initialize task to none.
	msg.task.data.task = TASK_NONE;

    /// Parse command line arguments.
    parse_default_config( &cf );
    parse_cla( argc, argv, &cf, STINGRAY, (const char *)VISIOND_FILENAME );

    /// Initialize HSV message data to configuration values.
	visiond_msg_cf_init( &msg, &cf );

    /// Set up server.
    if( cf.enable_server ) {
        server_fd = net_server_setup( cf.server_port );
		if( server_fd > 0 ) {
			printf("MAIN: Server setup OK.\n");
		}
		else {
			printf("MAIN: WARNING!!! Server setup failed.\n");
		}
    }

    /// Make sure open image directory is formatted correctly.
	if ( cf.open_image_dir[strlen( cf.open_image_dir ) - 1] != '/' ) {
		cf.open_image_dir[strlen( cf.open_image_dir )] = '/';
	}

	/// Make sure save image directory is formatted correctly.
	if ( cf.save_image_dir[strlen( cf.save_image_dir ) - 1] != '/' ) {
		cf.save_image_dir[strlen( cf.save_image_dir )] = '/';
	}

	/// Make sure the hardcoded save sub directories exist. Create if not.
	strncpy( curr_save_dir, cf.save_image_dir, STRING_SIZE );
	if ( !opendir ( strncat( curr_save_dir, "post/", 5 ) ) ) {
		if ( mkdir( curr_save_dir, S_IRWXU ) == -1 ) {
			printf( "MAIN: WARNING!!! Cannot setup save post image directory.\n" );

			/// Turn off saving post processed image.
			cf.save_image_post = 0;
		}
	}

	strncpy( curr_save_dir, cf.save_image_dir, STRING_SIZE );
	if ( !opendir ( strncat( curr_save_dir, "front/", 6 ) ) ) {
		if ( mkdir( curr_save_dir, S_IRWXU ) == -1 ) {
			printf( "MAIN: WARNING!!! Cannot setup front image directory.\n" );

			/// Turn off saving post processed image.
			cf.save_image_front = 0;
		}
	}

	strncpy( curr_save_dir, cf.save_image_dir, STRING_SIZE );
	if ( !opendir ( strncat( curr_save_dir, "bottom/", 7 ) ) ) {
		if ( mkdir( curr_save_dir, S_IRWXU ) == -1 ) {
			printf( "MAIN: WARNING!!! Cannot setup bottom image directory.\n" );

			/// Turn off saving post processed image.
			cf.save_image_bottom = 0;
		}
	}

    /// Decide whether to use source images or cameras.
	if ( cf.open_image_rate && strcmp( cf.open_image_dir, "" ) != 0 ) {

		if ( ( diropen = visiond_open_image_init( cf.open_image_dir, filename ) ) ) {
    		img = cvLoadImage( filename );
    		bin_img = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 1 );
    		save_img = cvCreateImage( cvSize( img->width * 2, img->height ), IPL_DEPTH_8U, img->nChannels );
			printf( "MAIN: Load from file as camera OK.\n" );
		}
		else {
			exit( 0 );
		}
	}
	else if( cf.op_mode == 99 ) {
    	/// Special case for bad camera.
    	printf( "MAIN: Skipping camera opening because op mode = 99 in "
		   	"configuration file.\n" );
	}
	else {
		/// Using actual cameras.
    	printf( "MAIN: Using actual cameras for vision...\n" );

		/// Open front camera.
		f_cam = cvCaptureFromCAM( 0 );
		if( !f_cam ) {
			cvReleaseCapture( &f_cam );
			printf( "MAIN: WARNING!!! Could not open f_cam.\n" );
		}
		else {
			img = cvQueryFrame( f_cam );
			bin_img = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 1 );
			printf( "MAIN: Front camera opened OK.\n" );
		}

		/// Open bottom camera.
		b_cam = cvCaptureFromCAM( 1 );
		if( !b_cam ) {
			cvReleaseCapture( &b_cam );
			printf( "MAIN: WARNING!!! Could not open b_cam.\n" );
		}
		else {
			img = cvQueryFrame( b_cam );
			bin_img = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 1 );
			printf( "MAIN: Bottom camera opened OK.\n" );
		}
	}

    /// Create windows to display video if set in configuration file.
	if( cf.vision_window ) {
		cvNamedWindow( win, CV_WINDOW_AUTOSIZE );
		cvNamedWindow( binwin, CV_WINDOW_AUTOSIZE );
	}

	/// Show the image in a window.
	if( cf.vision_window ) {

		/// OpenCV needs a little pause here.
		if( cvWaitKey( 500 ) >= 0 );

		/// Actually show the image.
		if ( img )
		{
			cvShowImage( win, img );
		}
		if( bin_img )
		{
			cvShowImage( binwin, bin_img );
		}

		/// OpenCV needs a little pause here.
		if( cvWaitKey( 500 ) >= 0 );
	}

	/// Need a way to force task from file.
	if ( diropen ) {
		msg.task.data.task = visiond_translate_task( cf.vision_task );
	}

	/// Open the post log file
	if ( cf.save_log_post ) {
		strncpy( filename, cf.save_image_dir, STRING_SIZE );
		strncat( filename, "post/", 5 );
		strncat( filename, "center_points.txt", STRING_SIZE );
		if ( !( log_file = fopen( filename, "w" ) ) ) {
			/// Don't do post log
			printf( "MAIN: WARNING!!! Cannot setup log post processing file.\n" );
			cf.save_log_post = 0;
		}
	}

    printf( "MAIN: Vision server running now.\n" );

    /// Main loop.
    while( 1 ) {

    	/// Get network data.
        if( (cf.enable_server) && (server_fd > 0) ) {
            recv_bytes = net_server( server_fd, recv_buf, &msg, MODE_VISION );
            if( recv_bytes > 0 ) {
                recv_buf[recv_bytes] = '\0';
                messages_decode( server_fd, recv_buf, &msg, recv_bytes );

                /// Force vision to look for the pipe no matter which pipe
			    /// subtask we are currently searching for.
				if( msg.task.data.task == TASK_PIPE1 || msg.task.data.task == TASK_PIPE2 ||
					msg.task.data.task == TASK_PIPE3 || msg.task.data.task == TASK_PIPE4 ) {
					msg.task.data.task = TASK_PIPE;
				}
            }
        }

		/// Reset "THE" image to be null. This way we know if there is a new image or not.
		img = NULL;

    	/// Do vision processing based on task.
    	if( msg.task.data.task == TASK_NONE ) {
    		/// Do nothing and give cleared values.
    		msg.vision.data.front_x = 0;
    		msg.vision.data.front_y = 0;
    		msg.vision.data.bottom_x = 0;
    		msg.vision.data.bottom_y = 0.0;
		}
		else {
			/// Check if time to pull a new image.
			if( diropen && timing_check_period(&timer_open, cf.open_image_rate)) {
				/// Get the next file pointer.
				dfile = readdir( dirp );

				/// Find the next file by ignoring directories.
				while ( dfile != NULL && strstr( dfile->d_name, ".jpg" ) == NULL ) {
					/// Iterate to next file.
					dfile = readdir( dirp );
				}

				if( dfile != NULL ) {
					/// This is a file so use it.

					/// Load image here.
					strncpy( filename, cf.open_image_dir, STRING_SIZE );
					strncat( filename, dfile->d_name, STRING_SIZE );
					img = cvLoadImage( filename );
				}
				else {
					/// Finished the directory.
					printf( "MAIN: Finished loading from source directory.\n\n" );
					printf( "VISION RESULTS:\n" );
					printf( "  %d/%d ---> %f%% positive\n", vision_classified, vision_considered,
								((double)vision_classified/vision_considered) * 100 );
					printf( "  %f ms per frame\n", vision_processing_time() );
					exit( 0 );
				}

				/// Reset the open image timer.
				timing_set_timer(&timer_open);
			}
			else if ( f_cam && ( msg.task.data.task == TASK_GATE ||
								msg.task.data.task == TASK_BUOY ||
								msg.task.data.task == TASK_FENCE ) ) {
				img = cvQueryFrame( f_cam );
				cvFlip( img, img );
			}
			else if ( b_cam && ( msg.task.data.task == TASK_PIPE ||
								msg.task.data.task == TASK_BOXES ||
								msg.task.data.task == TASK_SUITCASE ) ) {
				img = cvQueryFrame( b_cam );
			}
		}

		/// Only handle the image if there is a valid one.
		if ( img != NULL ) {

			if ( cf.save_log_post ) {
				/// Write the filename
				fprintf( log_file, "%s,", dfile->d_name );
			}

			/// Process the image according to the task.
			if ( visiond_process_image( img, bin_img, &msg ) ) {
				vision_classified++;
			}
			vision_considered++;

			if ( cf.save_log_post ) {
				/// Write the filename
				fprintf( log_file, "\n" );
			}

			/// Show the image in a window.
			if( cf.vision_window ) {

				/// OpenCV needs a little pause here.
				if( cvWaitKey( 5 ) >= 0 );

				/// Actually show the image.
				if ( img )
				{
					cvShowImage( win, img );
				}
				if( bin_img )
				{
					cvShowImage( binwin, bin_img );
				}
			}

			/// Calculate frames per second.
			nframes++;
			if( timing_check_period(&timer_fps, 1.) ) {
				timing_get_dt(&timer_fps, &timer_fps);
				dt = timing_s2us(&timer_fps);
				fps = (double)nframes * (1000000 / dt);
				timing_set_timer(&timer_fps);
				nframes = 0;
				msg.vision.data.fps = fps;
			}

			if ( diropen ) {
				/// The processed images need to be saved before the live feeds.
				if ( cf.save_image_post ) {
					vision_concat_images( img, bin_img, save_img );
					strncpy( curr_save_dir, cf.save_image_dir, STRING_SIZE );
					vision_save_frame( save_img, strncat( curr_save_dir, "post/", 5 ), dfile->d_name );
				}
			}

			/// If image was loaded from file, release it. Don't need to release if
		 	/// image is captured from camera.
			if( diropen ) {
				cvReleaseImage( &img );
			}
		}

		/// Save the front and/or bottom images. (CONFIG)
		if ( !diropen && timing_check_period(&timer_save, cf.save_image_rate) ) {
			if ( cf.save_image_front && f_cam ) {
				/// Save an image from the front camera.
				img = cvQueryFrame( f_cam );
				strncpy( curr_save_dir, cf.save_image_dir, STRING_SIZE );
				vision_save_frame( img, strncat( curr_save_dir, "front/", 6 ) );
			}

			if ( cf.save_image_bottom && b_cam ) {
				/// Save an image from the bottom camera.
				img = cvQueryFrame( b_cam );
				strncpy( curr_save_dir, cf.save_image_dir, STRING_SIZE );
				vision_save_frame( img, strncat( curr_save_dir, "bottom/", 7 ) );
			}

			timing_set_timer(&timer_save);
		}

		/// Save the front or bottom image. (GUI)
		if( msg.vsetting.data.save_fframe && f_cam ) {
			/// Save image to disk.
			img = cvQueryFrame( f_cam );
			strncpy( curr_save_dir, cf.save_image_dir, STRING_SIZE );
			vision_save_frame( img, strncat( curr_save_dir, "front/", 6 ) );
			msg.vsetting.data.save_fframe = FALSE;
		}
		if( msg.vsetting.data.save_bframe && b_cam ) {
			/// Save image to disk.
			img = cvQueryFrame( b_cam );
			strncpy( curr_save_dir, cf.save_image_dir, STRING_SIZE );
			vision_save_frame( img, strncat( curr_save_dir, "bottom/", 7 ) );
			msg.vsetting.data.save_bframe = FALSE;
		}

		/// Save the front or bottom video. (GUI)
		if( msg.vsetting.data.save_fvideo && !saving_fvideo && f_cam ) {
			/// Get a timestamp and use for filename.
			gettimeofday( &ctime, NULL );
			img = cvQueryFrame( f_cam );
			ct = *( localtime ((const time_t*) &ctime.tv_sec) );
			strftime( write_time, sizeof(write_time), "stream/f20%y%m%d_%H%M%S", &ct);
			snprintf( write_time + strlen(write_time),
					strlen(write_time), ".%03ld.avi", ctime.tv_usec );
			fps = cvGetCaptureProperty( f_cam, CV_CAP_PROP_FPS );
			f_writer = cvCreateVideoWriter( write_time, CV_FOURCC('M', 'J', 'P', 'G'),
				fps, cvGetSize( img ), is_color );
			saving_fvideo = TRUE;
		}
		else if( !msg.vsetting.data.save_fvideo && saving_fvideo ) {
			cvReleaseVideoWriter( &f_writer );
			saving_fvideo = FALSE;
		}
		else if( msg.vsetting.data.save_fvideo && saving_fvideo ) {
			cvWriteFrame( f_writer, img );
		}
		if( msg.vsetting.data.save_bvideo && !saving_bvideo && b_cam ) {
			/// Get a timestamp and use for filename.
			gettimeofday( &ctime, NULL );
			img = cvQueryFrame( b_cam );
			ct = *( localtime ((const time_t*) &ctime.tv_sec) );
			strftime( write_time, sizeof(write_time), "stream/b20%y%m%d_%H%M%S", &ct);
			snprintf( write_time + strlen(write_time),
					strlen(write_time), ".%03ld.avi", ctime.tv_usec );
			fps = cvGetCaptureProperty( b_cam, CV_CAP_PROP_FPS );
			b_writer = cvCreateVideoWriter( write_time, CV_FOURCC('M', 'J', 'P', 'G'),
				fps, cvGetSize( img ), is_color );
			saving_bvideo = TRUE;
		}
		else if( !msg.vsetting.data.save_bvideo && saving_bvideo ) {
			cvReleaseVideoWriter( &b_writer );
			saving_bvideo = FALSE;
		}
		else if( msg.vsetting.data.save_bvideo && saving_bvideo ) {
			cvWriteFrame( b_writer, img );
		}

    }

    exit( 0 );
} /// end main()

/******************************************************************************
 *
 * Title:       int visiond_visiond_open_image_init( char *dir, char *filename )
 *
 * Description: Tests if we should and can load images from file.
 *
 *****************************************************************************/

int visiond_open_image_init( char *dir, char *filename )
{
	struct dirent *dfile = NULL;

	/// Load an images from disk.
	//printf( "visiond_open_image_init: Using source images from directory for vision...\n" );

	/// Open directory.
	if ( (dirp = opendir( dir ) ) ) {

		dfile = readdir( dirp );
		if ( dfile == NULL ) {
			printf( "visiond_open_image_init: WARNING!!! Image directory not valid.\n" );
			return FALSE;
		}
		else {
			/// Find the next file by ignoring directories.
			while ( dfile != NULL && strstr( dfile->d_name, ".jpg" ) == NULL ) {

				/// Iterate to next file.
				dfile = readdir( dirp );
			}

			/// Setup the images.
			if( dfile != NULL ) {
				/// This is a file so use it.

				/// Load image here.
				strncpy( filename, dir, STRING_SIZE );
				strncat( filename, dfile->d_name, STRING_SIZE );

				/// Close and Open the directory so we start from the beginning.
				closedir( dirp );
				if ( (dirp = opendir( dir ) ) ) {
					/// Reopen OK.
					//printf( "visiond_open_image_init: Image source directory OK.\n" );
					return TRUE;
				}
				else {
					/// Reopen failed.
					printf( "visiond_open_image_init: WARNING!!! Source directory reopen failed.\n" );
					return FALSE;
				}
			}
			else {
				/// No images so exit the module.
				printf( "visiond_open_image_init: WARNING!!! No image files in directory.\n" );
				return FALSE;
			}
		}
	}
	else {
		/// Directory was not opened so exit the module.
		printf( "visiond_open_image_init: WARNING!!! Image directory '%s' not opened.\n", dir );
		return FALSE;
	}

	return TRUE;
} /// end visiond_open_image_init()

/******************************************************************************
 *
 * Title:       int visiond_translate_task( char *task_name )
 *
 * Description: Translates the task name to the task ID.
 *
 * Input:      	task_name: The name of the task.
 *
 * Output:      None.
 *
 *****************************************************************************/
int visiond_translate_task( char *task_name )
{
	if ( strcmp( task_name, "buoy" ) == 0 ) {
		return TASK_BUOY;
	}
	else if ( strcmp( task_name, "gate" ) == 0 ) {
		return TASK_GATE;
	}
	else if ( strcmp( task_name, "fence" ) == 0 ) {
		return TASK_FENCE;
	}
	else if ( strcmp( task_name, "pipe" ) == 0 ) {
		return TASK_PIPE;
	}
	else if ( strcmp( task_name, "boxes" ) == 0 ) {
		return TASK_BOXES;
	}
	else if ( strcmp( task_name, "suitcase" ) == 0 ) {
		return TASK_SUITCASE;
	}

	return TASK_NONE;
} /// end visiond_translate_task()

/******************************************************************************
 *
 * Title:       int visiond_process_image( IplImage *img, IplImage *bin_img,
 * 									MSG_DATA *msg )
 *
 * Description: Processes the given image based on the current task. Fills the
 * 				given binary image file with the result of processing and
 * 				draws a circle in the object spot on the given image.
 *
 * Input:      	img: The given image to be processed.
 * 				bin_img: The place to put the resulting binary image.
 * 				msg: The message structure that holds relevant information
 * 					including hsv values, vision_angle, and task.
 *
 * Output:      Draws cirlce on img if object found and draws bin_img.
 *
 *****************************************************************************/
int visiond_process_image( IplImage *img, IplImage *bin_img, MSG_DATA *msg )
{
	int status = -1;

	/// BUOY VARIABLES.
	int dotx = -1;
    int doty = -1;
	int xrot = -1;
    int yrot = -1;

    double angleFrontCam = VISIOND_FRONT_CAM_ANGLE_OFFSET * M_PI /  180;

    /// PIPE VARIABLES.
    /// Temporary variable to make it easier to switch between using HSV
	/// olding or boxes to try and find pipe. HSV = 1, Boxes = 2.
	int pipe_type = VISION_PIPE_HSV;
	int pipex = -10000;
	int pipey = -10000;
	double bearing = -10000;
	int ii = 0;

	/// BOXES VARIABLES.
	/// Variables to hold box centroid sequence and vertex sequence.
	CvMemStorage *storage1 = 0;
	storage1 = cvCreateMemStorage(0);
	CvSeq *boxes = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPoint), storage1 );
	CvSeqReader reader1;
	CvMemStorage *storage2 = 0;
	storage2 = cvCreateMemStorage(0);
	CvSeq *squares = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPoint), storage2 );
	CvSeqReader reader2;
	CvPoint box_pt;
	CvPoint pt[4];
	CvPoint *rect = pt;
	int count = 4;

	/// FENCE VARIABLES.
    int fence_center = img->width / 2;
	int y_max = -1;
	int lineWidth = 75;

	if ( msg->task.data.task == TASK_GATE ) {
		/// Set to not detected to start and reset if we get a hit.
		msg->vision.data.status = TASK_NOT_DETECTED;

		/// Look for the gate .
		status = vision_find_gate( &dotx, &doty, msg->vsetting.data.vision_angle, img,
			bin_img, &msg->vsetting.data.buoy_hsv );
		if( status == 1 || status == 2 ) {
			/// We have detected the gate.
			//printf("MAIN: Gate Status = %d\n", status);
			msg->vision.data.status = TASK_GATE_DETECTED;

			/// The subtractions are opposite of each other on purpose. This is
			/// so that they match the way the depth sensor and yaw sensor work.
			msg->vision.data.front_x = dotx - (img->width / 2);
			msg->vision.data.front_y = (img->height / 2) - doty;

			xrot =  msg->vision.data.front_x * cos( angleFrontCam ) +
					msg->vision.data.front_y * sin( angleFrontCam );
			yrot = -msg->vision.data.front_x * sin( angleFrontCam ) +
					msg->vision.data.front_y * cos( angleFrontCam );

			msg->vision.data.front_x = xrot;
			msg->vision.data.front_y = yrot;

			/// Draw a circle at the centroid location.
			cvCircle( img, cvPoint(dotx, doty),
				10, cvScalar(0, 255, 0), 5, 8 );

			if( status == 2 ) {
				msg->vision.data.status = TASK_GATE_CLEARED;
			}
		}
	} /// end TASK_GATE

	else if ( msg->task.data.task == TASK_BUOY ) {
		/// Set to not detected to start and reset if we get a hit.
		msg->vision.data.status = TASK_NOT_DETECTED;

		/// Look for the buoy.
		status = vision_find_dot( &dotx, &doty, msg->vsetting.data.vision_angle, img,
			bin_img, &msg->vsetting.data.buoy_hsv );

		if( status == 1 || status == 2 ) {
			/// We have detected the buoy.
			//printf("MAIN: Bouy Status = %d\n", status);
			msg->vision.data.status = TASK_BUOY_DETECTED;

			/// The subtractions are opposite of each other on purpose. This is
			/// so that they match the way the depth sensor and yaw sensor work.
			msg->vision.data.front_x = dotx - (img->width / 2);
			msg->vision.data.front_y = (img->height / 2) - doty;

			xrot =  msg->vision.data.front_x * cos( angleFrontCam ) +
					msg->vision.data.front_y * sin( angleFrontCam );
			yrot = -msg->vision.data.front_x * sin( angleFrontCam ) +
					msg->vision.data.front_y * cos( angleFrontCam );

			msg->vision.data.front_x = xrot;
			msg->vision.data.front_y = yrot;

			/// Draw a circle at the centroid location.
			cvCircle( img, cvPoint(dotx, doty),
				10, cvScalar(0, 255, 0), 5, 8 );

			if ( log_file ) {
				fprintf( log_file, "%d,%d", dotx, doty );
			}

			if( status == 2 ) {
				msg->vision.data.status = TASK_BUOY_TOUCHED;
			}
		}
		else {
			if ( log_file ) {
				fprintf( log_file, "X,Y" );
			}
		}
	} /// end TASK_BUOY

	else if ( msg->task.data.task == TASK_PIPE && pipe_type == VISION_PIPE_HSV ) {
		/// Set to not detected to start and reset if we get a hit.
		msg->vision.data.status = TASK_NOT_DETECTED;

		/// Look for the pipe.
		status = vision_find_pipe( &pipex, &pipey, &bearing, img, bin_img,
			&msg->vsetting.data.pipe_hsv );

		/// If we get a positive status message, render the box and populate
		/// the network message.
		if( status == 1 ) {
			/// Set the detection status of vision.
			//printf("MAIN Pipe Status = %d\n", status);
			msg->vision.data.status = TASK_PIPE_DETECTED;

			msg->vision.data.bearing = bearing;
			msg->vision.data.bottom_x = pipex - img->width / 2;
			msg->vision.data.bottom_y = pipey - img->height / 2;

			/// Draw a circle at the centroid location.
			cvCircle( img, cvPoint(pipex, pipey),
				10, cvScalar(255, 0, 0), 5, 8 );

			/// Draw the bearing if it is there.
			if( bearing != 0 ) {
				for( ii = 0; ii < 25; ii++ ) {
					cvCircle( img,
						cvPoint(img->width / 2 + ((int)(bearing * ii)),
						img->width / 2 + ii), 2, cvScalar(255, 255, 0), 2 );
				}
			}
			else {
				for( ii = 0; ii < 25; ii++ ) {
					cvCircle( img,
						cvPoint(img->width / 2 + ((int)(bearing * ii)),
						img->width / 2 + ii), 2, cvScalar(0, 0, 255), 2 );
				}
			}
		}
	} /// end TASK_PIPE -- hsv

	else if ( msg->task.data.task == TASK_PIPE && pipe_type == VISION_PIPE_BOX ) {
		/// Set to not detected to start and reset if we get a hit.
		msg->vision.data.status = TASK_NOT_DETECTED;

		/// Look for the pipe.
		status = vision_find_boxes( img, boxes, squares, VISION_PIPE,
			&msg->vision.data.bearing );

		/// If we get a positive status message, render the box
		/// and populate the network message.
		if( status > 0 ) {
			/// Initialize the centroid sequence reader.
			cvStartReadSeq( boxes, &reader1, 0 );

			/// Read four sequence elements at a time.
			for( ii = 0; ii < boxes->total; ii += 2 ) {
				/// Read centroid x and y coordinates.
				CV_READ_SEQ_ELEM( box_pt, reader1 );

				/// Draw the centroid as a circle.
				cvCircle( img, box_pt,
					10, cvScalar(0, 0, 255), 5, 8 );
			}

			/// Initialize the vertex sequence reader.
			cvStartReadSeq( squares, &reader2, 0 );
			for( ii = 0; ii < squares->total; ii += 4 ) {
				/// Read vertex x and y coordinates.
				CV_READ_SEQ_ELEM( pt[0], reader2 );
				CV_READ_SEQ_ELEM( pt[1], reader2 );
				CV_READ_SEQ_ELEM( pt[2], reader2 );
				CV_READ_SEQ_ELEM( pt[3], reader2 );

				/// Draw the square as a closed polyline.
				cvPolyLine( img, &rect, &count, 1, 1, CV_RGB(0, 255, 0),
					3, CV_AA, 0 );
			}

			/// Set target offsets in network message.
			if( boxes->total > 0 ) {
				/// Set the detection status of vision.
				msg->vision.data.status = TASK_PIPE_DETECTED;

				msg->vision.data.bearing = 0; //// !!! TODO: Fix this value !!!
				msg->vision.data.bottom_x = box_pt.x - img->width / 2;
				msg->vision.data.bottom_y = box_pt.y - img->height / 2;
			}
			/// Clear out the sequences so that next time we only draw newly
			/// found squares and centroids.
			cvClearSeq( boxes );
			cvClearSeq( squares );
		}
	} /// end TASK_PIPE -- boxes

	else if ( msg->task.data.task == TASK_FENCE ) {
		/// Set to not detected to start and reset if we get a hit.
		msg->vision.data.status = TASK_NOT_DETECTED;

		/// Look for the fence.
		status = vision_find_fence( &fence_center, &y_max, img,	bin_img,
			&msg->vsetting.data.fence_hsv );

		if( status == 1 ) {
			/// Set the detection status of vision.
			msg->vision.data.status = TASK_FENCE_DETECTED;

			/// Draw a circle at the centroid.
			cvCircle( img, cvPoint(fence_center, y_max),
				10, cvScalar(0, 0, 255), 5, 8 );

			/// Draw a horizontal line indicating the lowest point of the fence.
			y_max = vision_get_fence_bottom(bin_img);
			for( ii = 0; ii < lineWidth; ii++ ) {
				cvCircle( img, cvPoint(img->width / 2 + ii, y_max),
					2, cvScalar(100, 255, 20), 2 );
			}

			/// Set target offsets in network message. The subtractions are
			/// opposite of each other on purpose. This is so that they
			/// match the way the depth sensor and yaw sensor work.
			msg->vision.data.front_x = fence_center - img->width / 2;
			msg->vision.data.front_y = y_max - img->height / 4;
		}
	} /// end TASK_FENCE

	else if ( msg->task.data.task == TASK_BOXES ) {
		/// Set to not detected to start and reset if we get a hit.
		msg->vision.data.status = TASK_NOT_DETECTED;

		/// Look for the boxes.
		status = vision_find_boxes( img, boxes, squares, VISION_BOX,
			&msg->vision.data.bearing );

		/// If we get a positive status message, render the box and populate
		/// the network message.
		if( status > 0 ) {
			/// Initialize the centroid sequence reader.
			cvStartReadSeq( boxes, &reader1, 0 );

			/// Read two sequence elements at a time.
			for( ii = 0; ii < boxes->total; ii += 2 ) {
				/// Read centroid x and y coordinates.
				CV_READ_SEQ_ELEM( box_pt, reader1 );

				/// Draw the centroid as a circle.
				cvCircle( img, box_pt,
					10, cvScalar(0, 0, 255), 5, 8 );
			}

			/// Initialize the vertex sequence reader.
			cvStartReadSeq( squares, &reader2, 0 );

			/// Read four sequence elements at a time.
			for( ii = 0; ii < squares->total; ii += 4 ) {
				/// Read vertex x and y coordinates.
				CV_READ_SEQ_ELEM( pt[0], reader2 );
				CV_READ_SEQ_ELEM( pt[1], reader2 );
				CV_READ_SEQ_ELEM( pt[2], reader2 );
				CV_READ_SEQ_ELEM( pt[3], reader2 );

				/// Draw the square as a closed polyline.
				cvPolyLine( img, &rect, &count, 1, 1, CV_RGB(0, 255, 0), 3, CV_AA, 0 );
			}

			/// Set target offsets in network message.
			if( boxes->total > 0 ) {
				/// Set the detection status of vision.
				msg->vision.data.status = TASK_BOXES_DETECTED;

				/// Set the message variables to the center of the detected box.
				msg->vision.data.box1_x = box_pt.x - img->width / 2;
				msg->vision.data.box1_y = box_pt.y - img->height / 2;
			}
			/// Clear out the sequences so that next time we only draw newly
			/// found squares and centroids.
			cvClearSeq( boxes );
			cvClearSeq( squares );
		}
	} /// end TASK_BOXES

	else if ( msg->task.data.task == TASK_SUITCASE ) {
		/// Set to not detected to start and reset if we get a hit.
		msg->vision.data.status = TASK_NOT_DETECTED;

		/// Look for the suitcase.
		status = vision_suitcase( img, boxes, squares );
		if( status > 0 ) {

			/// Initialize the centroid sequence reader.
			cvStartReadSeq( boxes, &reader1, 0 );

			/// Read four sequence elements at a time.
			for( ii = 0; ii < boxes->total; ii += 2 ) {
				/// Read centroid x and y coordinates.
				CV_READ_SEQ_ELEM( box_pt, reader1 );

				/// Draw the centroid as a circle.
				cvCircle( img, box_pt,
					10, cvScalar(0, 0, 255), 5, 8 );
			}

			/// Initialize the vertex sequence reader.
			cvStartReadSeq( squares, &reader2, 0 );
			for( ii = 0; ii < squares->total; ii += 4 ) {
				/// Read vertex x and y coordinates.
				CV_READ_SEQ_ELEM( pt[0], reader2 );
				CV_READ_SEQ_ELEM( pt[1], reader2 );
				CV_READ_SEQ_ELEM( pt[2], reader2 );
				CV_READ_SEQ_ELEM( pt[3], reader2 );

				/// Draw the square as a closed polyline.
				cvPolyLine( img, &rect, &count, 1, 1, CV_RGB(0, 255, 0), 3, CV_AA, 0 );
			}

			/// Set target offsets in network message.
			if( boxes->total > 0 ) {
				/// Set the detection status of vision.
				msg->vision.data.status = TASK_SUITCASE_DETECTED;

				msg->vision.data.suitcase_x = box_pt.x - img->width / 2;
				msg->vision.data.suitcase_y = box_pt.y - img->height / 2;
			}
			/// Clear out the sequences so that next time we only draw newly
			/// found squares and centroids.
			cvClearSeq( boxes );
			cvClearSeq( squares );
		}
	} /// end TASK_SUITCASE

	else {
		/// Clear the detection status.
		msg->vision.data.status = TASK_NOT_DETECTED;

	}

	if ( msg->vision.data.status == TASK_GATE_DETECTED || msg->vision.data.status == TASK_BUOY_DETECTED ||
			msg->vision.data.status == TASK_PIPE_DETECTED || msg->vision.data.status == TASK_FENCE_DETECTED ||
			msg->vision.data.status == TASK_BOXES_DETECTED || msg->vision.data.status == TASK_SUITCASE_DETECTED ) {
		return 1;
	}

	return 0;
} /// end visiond_process_image()

/******************************************************************************
 *
 * Title:       int visiond_msg_cf_init( MSG_DATA *msg, CONF_VARS *cf )
 *
 * Description: Initializes the msg variables using config file variables.
 *
 *****************************************************************************/

int visiond_msg_cf_init( MSG_DATA *msg, CONF_VARS *cf )
{
    msg->vsetting.data.pipe_hsv.hL  = cf->pipe_hL;
    msg->vsetting.data.pipe_hsv.hH  = cf->pipe_hH;
    msg->vsetting.data.pipe_hsv.sL  = cf->pipe_sL;
    msg->vsetting.data.pipe_hsv.sH  = cf->pipe_sH;
    msg->vsetting.data.pipe_hsv.vL  = cf->pipe_vL;
    msg->vsetting.data.pipe_hsv.vH  = cf->pipe_vH;
    msg->vsetting.data.buoy_hsv.hL  = cf->buoy_hL;
    msg->vsetting.data.buoy_hsv.hH  = cf->buoy_hH;
    msg->vsetting.data.buoy_hsv.sL  = cf->buoy_sL;
    msg->vsetting.data.buoy_hsv.sH  = cf->buoy_sH;
    msg->vsetting.data.buoy_hsv.vL  = cf->buoy_vL;
    msg->vsetting.data.buoy_hsv.vH  = cf->buoy_vH;
    msg->vsetting.data.fence_hsv.hL = cf->fence_hL;
    msg->vsetting.data.fence_hsv.hH = cf->fence_hH;
    msg->vsetting.data.fence_hsv.sL = cf->fence_sL;
    msg->vsetting.data.fence_hsv.sH = cf->fence_sH;
    msg->vsetting.data.fence_hsv.vL = cf->fence_vL;
    msg->vsetting.data.fence_hsv.vH = cf->fence_vH;
    msg->vsetting.data.vision_angle = cf->vision_angle;

	return TRUE;
} /// end visiond_msg_cf_init()

redpajama_set_name: RedPajamaGithub


text:
var expect = require("chai").expect;
var command = require('../src/command.js');

describe('Command:_constructor()', function () {

    describe('path marginal cases', function () {
        function testCase(p) {
            expect(function () {
                var sut = new command(p);
            }).to.throw('path cannot be empty');
        }

        it('should throw when called with no path', function () {
            testCase();
        });
        it('should throw when called with empty path', function () {
            testCase('');
        });
        it('should throw when called with non-string path', function () {
            testCase([]);
        });
    });

    describe('when called with single part', function () {
        var sut = new command('app');

        it('should have correct name', function () {
            expect(sut.name).to.equal('app');
        });
        it('should have correct path', function () {
            expect(sut.path).to.equal('app');
        });
        it('should have empty parent path', function () {
            expect(sut.parentPath).to.be.empty;
        });
        it('should have empty command set', function () {
            expect(sut.commands).to.be.empty;
        });
        it('should have empty help', function () {
            expect(sut.$help).to.be.empty;
        });
        it('should not be terminal', function () {
            expect(sut.$terminal).to.be.false;
        });
        it('should have empty controller', function () {
            expect(sut.$controller).to.be.false;
        });
        it('should have empty alias', function () {
            expect(sut.$alias).to.be.empty;
        });
    });

    describe('when called with two parts', function () {
        var sut = new command('app.test');

        it('should have correct name', function () {
            expect(sut.name).to.equal('test');
        });
        it('should have correct path', function () {
            expect(sut.path).to.equal('app.test');
        });
        it('should have correct parent path', function () {
            expect(sut.parentPath).to.equal('app');
        });
    });

    describe('when called with multiple parts', function () {
        var sut = new command('app.test.foo.bar');

        it('should have correct name', function () {
            expect(sut.name).to.equal('bar');
        });
        it('should have correct path', function () {
            expect(sut.path).to.equal('app.test.foo.bar');
        });
        it('should have correct parent path', function () {
            expect(sut.parentPath).to.equal('app.test.foo');
        });
    });
});

describe('Command:configure()', function () {

    it('when pass terminal, should set terminal', function () {
        var sut = new command('app');
        sut.configure({ terminal: true });
        expect(sut.$terminal).to.be.true;
    });

    it('when pass alias, should set alias', function () {
        var sut = new command('app');
        sut.configure({ alias: 'test' });
        expect(sut.$alias).to.equal('test');
    });

    it('when pass controller, should set controller', function () {
        var sut = new command('app');
        var fn = function () {};
        sut.configure({ controller: fn });
        expect(sut.$controller).to.equal(fn);
    });

    it('when pass help, should set help', function () {
        var sut = new command('app');
        var h = { some: 'help' };
        sut.configure({ help: h });
        expect(sut.$help).to.equal(h);
    });

    it('should return command', function () {
        var sut = new command('app');
        var res = sut.configure();
        expect(res).to.equal(sut);
    });

    describe('when pass alias and controller (multiple)', function () {
        var sut = new command('app');
        var fn = function () {};
        sut.configure({ alias: 'test', controller: fn });

        it('should set alias', function () {
            expect(sut.$alias).to.equal('test');
        });
        it('should set controller', function () {
            expect(sut.$controller).to.equal(fn);
        });
    });
});

describe('Command:help()', function () {
    describe('when pass a string', function () {
        var sut = new command('app');
        var res = sut.help('some help');

        it('help should be an object', function () {
            expect(sut.$help).to.be.an('object');
        });

        it('help should have correct description', function () {
            expect(sut.$help.description).to.equal('some help');
        });

        it('should return command', function () {
            expect(res).to.equal(sut);
        });
    });

    it('should set help, when pass an object', function () {
        var sut = new command('app');
        var h = { some: 'help' };
        sut.help(h);

        expect(sut.$help).to.equal(h);
    });
});

describe('Command:controller()', function () {
    describe('throws when non-function is passed', function () {
        var sut = new command('app');
        function testCase(v) {
            expect(function () {
                sut.controller(v);
            }).to.throw('controller must be a function');
        }

        it('test-case: nothing', function () { testCase(); });
        it('test-case: string', function () { testCase('something'); });
        it('test-case: array', function () { testCase([]); });
    });

    it('when pass function, should set controller', function () {
        var sut = new command('app');
        var fn = function () {};
        sut.controller(fn);
        expect(sut.$controller).to.equal(fn);
    });
});

describe('Command:_getChild()', function () {
    var sut = new command('app');
    var c1 = new command('app.test1'),
        c2 = new command('app.test2'),
        c3 = (new command('app.test3')).configure({ alias: 't3'}),
        c4 = new command('app.test4'),
        c5 = new command('app.test5');

    sut.commands = [c1, c2, c3, c4, c5];

    it('should return correct sub-command when name match', function () {
        var res = sut._getChild('test4');
        expect(res).to.equal(c4);
    });

    it('should return correct sub-command when alias match', function () {
        var res = sut._getChild('t3');
        expect(res).to.equal(c3);
    });

    it('should return null when nothing match', function () {
        var res = sut._getChild('none');
        expect(res).to.be.empty;
    });
});