/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "AxisCameraParams.h"

#include "AxisCamera.h"
#include <inetLib.h>
#include "pcre.h"
#include <sockLib.h> 
#include <string.h>
#include "Synchronized.h"
#include "Timer.h"

static const char *const kRotationChoices[] = {"0", "180"};
static const char *const kResolutionChoices[] = {"640x480", "640x360", "320x240", "160x120"};
static const char *const kExposureControlChoices[] = { "automatic", "hold", "flickerfree50", "flickerfree60" };
static const char *const kWhiteBalanceChoices[] = { "auto", "holdwb", "fixed_outdoor1",
		"fixed_outdoor2", "fixed_indoor", "fixed_fluor1", "fixed_fluor2" };

/**
 * AxisCamera constructor
 */
AxisCameraParams::AxisCameraParams(const char* ipAddress)
	: m_paramTask("paramTask", (FUNCPTR) s_ParamTaskFunction)
	, m_ipAddress (inet_addr((char*)ipAddress))
	, m_paramChangedSem (NULL)
{
	m_brightnessParam = new IntCameraParameter("ImageSource.I0.Sensor.Brightness=%i",
			"root.ImageSource.I0.Sensor.Brightness=(.*)", false);
	m_parameters.push_back(m_brightnessParam);
	m_colorLevelParam = new IntCameraParameter("ImageSource.I0.Sensor.ColorLevel=%i",
			"root.ImageSource.I0.Sensor.ColorLevel=(.*)", false);
	m_parameters.push_back(m_colorLevelParam);
	m_exposurePriorityParam = new IntCameraParameter("ImageSource.I0.Sensor.exposurePriority=%i",
			"root.ImageSource.I0.Sensor.ExposurePriority=(.*)", false);
	m_parameters.push_back(m_exposurePriorityParam);
	m_compressionParam = new IntCameraParameter("Image.I0.Appearance.Compression=%i",
			"root.Image.I0.Appearance.Compression=(.*)", true);
	m_parameters.push_back(m_compressionParam);
	m_maxFPSParam = new IntCameraParameter("Image.I0.Stream.FPS=%i",
			"root.Image.I0.Stream.FPS=(.*)", false);
	m_parameters.push_back(m_maxFPSParam);
	m_rotationParam = new EnumCameraParameter("Image.I0.Appearance.Rotation=%s",
			"root.Image.I0.Appearance.Rotation=(.*)", true, kRotationChoices, sizeof(kRotationChoices)/sizeof(kRotationChoices[0]));
	m_parameters.push_back(m_rotationParam);
	m_resolutionParam = new EnumCameraParameter("Image.I0.Appearance.Resolution=%s",
			"root.Image.I0.Appearance.Resolution=(.*)", true, kResolutionChoices, sizeof(kResolutionChoices)/sizeof(kResolutionChoices[0]));
	m_parameters.push_back(m_resolutionParam);
	m_exposureControlParam = new EnumCameraParameter("ImageSource.I0.Sensor.Exposure=%s",
			"root.ImageSource.I0.Sensor.Exposure=(.*)", false, kExposureControlChoices, sizeof(kExposureControlChoices)/sizeof(kExposureControlChoices[0]));
	m_parameters.push_back(m_exposureControlParam);
	m_whiteBalanceParam = new EnumCameraParameter("ImageSource.IO.Sensor.WhiteBalance=%s",
			"root.ImageSource.I0.Sensor.WhiteBalance=(.*)", false, kWhiteBalanceChoices, sizeof(kWhiteBalanceChoices)/sizeof(kWhiteBalanceChoices[0]));
	m_parameters.push_back(m_whiteBalanceParam);

	m_paramChangedSem = semBCreate (SEM_Q_PRIORITY, SEM_EMPTY);

	m_paramTask.Start((int)this);
}

/**
 * Destructor
 */
AxisCameraParams::~AxisCameraParams()
{
	m_paramTask.Stop();
}

/**
 * Static function to start the parameter updating task
 */
int AxisCameraParams::s_ParamTaskFunction(AxisCameraParams* thisPtr)
{
	return thisPtr->ParamTaskFunction();
}

/**
 * Main loop of the parameter task.
 * This loop runs continuously checking parameters from the camera for
 * posted changes and updating them if necessary.
 */
// TODO: need to synchronize the actual setting of parameters (the assignment statement)
int AxisCameraParams::ParamTaskFunction()
{
	while (ReadCamParams() == 0) ;
	while (true)
	{
		bool restartRequired = false;
		// wait for a parameter to be changed
		semTake(m_paramChangedSem, WAIT_FOREVER);

		ParameterVector_t::iterator it = m_parameters.begin();
		ParameterVector_t::iterator end = m_parameters.end();
		for(; it != end; it++)
		{
			bool changed = false;
			char param[150];
			restartRequired |= (*it)->CheckChanged(changed, param);
			if (changed)
			{
				UpdateCamParam(param);
			}
		}

		if (restartRequired)
		{
			RestartCameraTask();
		}
	}
	return 0;
}

/**
 * Write the brightness value to the camera.
 * @param brightness valid values 0 .. 100
 */
void AxisCameraParams::WriteBrightness(int brightness)
{
	m_brightnessParam->SetValue(brightness);
	semGive(m_paramChangedSem);
}

/**
 * Get the brightness value.
 * @return Brightness value from the camera.
 */
int AxisCameraParams::GetBrightness()
{
	return m_brightnessParam->GetValue();
}

/**
 * Set the white balance value.
 * @param whiteBalance Valid values from the WhiteBalance_t enum.
 */
void AxisCameraParams::WriteWhiteBalance(WhiteBalance_t whiteBalance)
{
	m_whiteBalanceParam->SetValue(whiteBalance);
	semGive(m_paramChangedSem);
}

/**
 * Retrieve the current white balance parameter.
 * @return The white balance value.
 */
AxisCameraParams::WhiteBalance_t AxisCameraParams::GetWhiteBalance()
{
	return (WhiteBalance_t) m_whiteBalanceParam->GetValue();
}

/**
 * Write the color level to the camera.
 * @param colorLevel valid values are 0 .. 100
 */
void AxisCameraParams::WriteColorLevel(int colorLevel)
{
	m_colorLevelParam->SetValue(colorLevel);
	semGive(m_paramChangedSem);
}

/**
 * Retrieve the color level from the camera.
 * @Returns the camera color level.
 */
int AxisCameraParams::GetColorLevel()
{
	return m_colorLevelParam->GetValue();
}

/**
 * Write the exposure control value to the camera.
 * @param exposureControl A mode to write in the Exposure_t enum.
 */
void AxisCameraParams::WriteExposureControl(Exposure_t exposureControl)
{
	m_exposureControlParam->SetValue(exposureControl);
	semGive(m_paramChangedSem);
}

/**
 * Get the exposure value from the camera.
 * @returns the exposure value from the camera.
 */
AxisCameraParams::Exposure_t AxisCameraParams::GetExposureControl()
{
	return (Exposure_t) m_exposureControlParam->GetValue();
}

/**
 * Write resolution value to camera.
 * @param resolution The camera resolution value to write to the camera.  Use the Resolution_t enum.
 */
void AxisCameraParams::WriteResolution(Resolution_t resolution)
{
	m_resolutionParam->SetValue(resolution);
	semGive(m_paramChangedSem);
}

/**
 * Get the resolution value from the camera.
 * @returns resultion value for the camera.
 */
AxisCameraParams::Resolution_t AxisCameraParams::GetResolution()
{
	return (Resolution_t) m_resolutionParam->GetValue();
}

/**
 * Write the exposre priority value to the camera.
 * @param exposurePriority Valid values are 0, 50, 100.
 * 0 = Prioritize image quality
 * 50 = None
 * 100 = Prioritize frame rate
 */
void AxisCameraParams::WriteExposurePriority(int exposurePriority)
{
	m_exposurePriorityParam->SetValue(exposurePriority);
	semGive(m_paramChangedSem);
}

int AxisCameraParams::GetExposurePriority()
{
	return m_exposurePriorityParam->GetValue();
}

/**
 * Write the rotation value to the camera.
 * If you mount your camera upside down, use this to adjust the image for you.
 * @param rotation The image from the Rotation_t enum in AxisCameraParams (kRotation_0 or kRotation_180)
 */
void AxisCameraParams::WriteRotation(Rotation_t rotation)
{
	m_rotationParam->SetValue(rotation);
	semGive(m_paramChangedSem);
}

/**
 * Get the rotation value from the camera.
 * @return The rotation value from the camera (Rotation_t).
 */
AxisCameraParams::Rotation_t AxisCameraParams::GetRotation()
{
	return (Rotation_t) m_rotationParam->GetValue();
}

/**
 * Write the compression value to the camera.
 * @param compression Values between 0 and 100.
 */

void AxisCameraParams::WriteCompression(int compression)
{
	m_compressionParam->SetValue(compression);
	semGive(m_paramChangedSem);
}

/**
 * Get the compression value from the camera.
 * @return The cached compression value from the camera.
 */
int AxisCameraParams::GetCompression()
{
	return m_compressionParam->GetValue();
}

/**
 * Write the maximum frames per second that the camera should send
 * Write 0 to send as many as possible.
 * @param maxFPS The number of frames the camera should send in a second, exposure permitting. 
 */
void AxisCameraParams::WriteMaxFPS(int maxFPS)
{
	m_maxFPSParam->SetValue(maxFPS);
	semGive(m_paramChangedSem);
}

/**
 * Get the max number of frames per second that the camera will send
 * @return Maximum frames per second.
 */
int AxisCameraParams::GetMaxFPS()
{
	return m_maxFPSParam->GetValue();
}

/**
 * Update a camera parameter.
 * Write a camera parameter to the camera when it has bene changed.
 * @param param the string to insert into the http request.
 * @returns 0 if it failed, otherwise nonzero.
 */
int AxisCameraParams::UpdateCamParam(const char* param)
{
	char * requestString =
					"GET /axis-cgi/admin/param.cgi?action=update&%s HTTP/1.1\n\
User-Agent: HTTPStreamClient\n\
Connection: Keep-Alive\n\
Cache-Control: no-cache\n\
Authorization: Basic RlJDOkZSQw==\n\n";
	char completedRequest[1024];
	sprintf(completedRequest, requestString, param);
	// Send request
	int camSocket = CreateCameraSocket(completedRequest);
	if (socket == 0) 
	{
		printf("UpdateCamParam failed: %s\n", param);
		return 0;
	}
	close(camSocket);
	return 1;
}

/**
 * Read the full param list from camera, use regular expressions to find the bits we care about
 * assign values to member variables.
 */
int AxisCameraParams::ReadCamParams()
{
	char * requestString =
					"GET /axis-cgi/admin/param.cgi?action=list HTTP/1.1\n\
User-Agent: HTTPStreamClient\n\
Connection: Keep-Alive\n\
Cache-Control: no-cache\n\
Authorization: Basic RlJDOkZSQw==\n\n";

	int camSocket = CreateCameraSocket(requestString);
	if (camSocket == 0)
	{
		return 0;
	}
	char readBuffer[7000];
	int totalRead = 0;
	int bytesRead = recv(camSocket, &readBuffer[totalRead], 1000, 0);
	while (bytesRead > 0)
	{
		totalRead += bytesRead;
		if (bytesRead == ERROR)
		{
			perror("AxisCameraParams: Failed to read image header");
			close(camSocket);
			return 0;
		}
		bytesRead = recv(camSocket, &readBuffer[totalRead], 1000, 0);
	}
	readBuffer[totalRead] = '\0';

	ParameterVector_t::iterator it = m_parameters.begin();
	ParameterVector_t::iterator end = m_parameters.end();
	for(; it != end; it++)
	{
		(*it)->GetParamFromString(readBuffer, totalRead);
	}
	close(camSocket);
	return 1;
}

/*
 * Create a socket connected to camera
 * Used to create a connection to the camera by both AxisCameraParams and AxisCamera.
 * @param requestString The initial request string to send upon successful connection.
 * @return 0 if failed, socket handle if successful.
 */
int AxisCameraParams::CreateCameraSocket(const char *requestString)
{
	int sockAddrSize;
	struct sockaddr_in serverAddr;
	int camSocket;
	/* create socket */
	if ((camSocket = socket(AF_INET, SOCK_STREAM, 0)) == ERROR)
	{
		perror("AxisCameraParams: socket");
		return 0;
	}

	sockAddrSize = sizeof(struct sockaddr_in);
	bzero((char *) &serverAddr, sockAddrSize);
	serverAddr.sin_family = AF_INET;
	serverAddr.sin_len = (u_char) sockAddrSize;
	serverAddr.sin_port = htons(80);

	if ((serverAddr.sin_addr.s_addr = m_ipAddress) == (u_long)ERROR)
	{
		perror("AxisCameraParams: invalid IP");
		close(camSocket);
		return 0;
	}

	/* connect to server */
	if (connect(camSocket, (struct sockaddr *) &serverAddr, sockAddrSize) == ERROR)
	{
		perror("AxisCameraParams: connect");
		close(camSocket);
		return 0;
	}
	int sent = send(camSocket, requestString, strlen(requestString), 0);
	if (sent == ERROR)
	{
		perror("AxisCameraParams: send");
		close(camSocket);
		return 0;
	}
	return camSocket;
}