const byte XAXIS = 0x01;
const byte YAXIS = 0x02;
const byte ZAXIS = 0x03;
const byte RAXIS = 0x00;

const double X_NM_PER_COUNT = 500;
const double Y_NM_PER_COUNT = 500;
const double Z_NM_PER_COUNT = 100;
const double R_NM_PER_COUNT = 0.12;

private void _timer1_Elapsed(Object source, ElapsedEventArgs e)
{
	_timer1.Enabled = false;
	byte[] bytesToSend = new byte[6] { 0x80, 0x04, axis_counter, 0x00, 0x11, 0x01 };  //status update
	usb_write(bytesToSend, 6);
	axis_counter++;
	if (axis_counter == 0x04)
	{
		axis_counter = 0x00;
	}
	_timer1.Enabled = true;
}

private void usb_write(byte[] bytesToSend, int length)
{
	if (IsConnected)
	{
		try
		{
			_serialPort.Write(bytesToSend, 0, length);
		}
		catch (Exception ex)
		{
			//MessageBox.Show(ex.Message);
			usb_disconnect();
		}
	}
	else
	{
		usb_connect();
	}
}

private void _serialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
	header = new byte[6];
	if (_serialPort.BytesToRead > 5)
	{
		_serialPort.Read(header, 0, 6);
	}
	if ((header[4] & 0x80) != 0)
	{
		int length = header[2] | header[3] << 8;
		while (_serialPort.BytesToRead < length) { };
		ext_data = new byte[length];
		_serialPort.Read(ext_data, 0, length);
		parse_apt();
	}
	else
	{
		ext_data = new byte[1] { 0 };
		parse_apt();
	}
}

private void parse_apt()
{
	int command = header[0] | header[1] << 8;
	double temp;
	switch (command)
	{
		case MGMSG_MOT_GET_STATUSUPDATE:
			temp = ext_data[9] | ext_data[8] << 8 | ext_data[7] << 16 | ext_data[6] << 24;
			switch (ext_data[1])
			{
				case XAXIS:
					temp = temp * X_NM_PER_COUNT / 1e3;
					_stage_data.x_quad_pos = temp.ToString("F1") + "µm";
					_stage_data.x_cw_limit = (ext_data[13] & 0x01) > 0;
					_stage_data.x_ccw_limit = (ext_data[13] & 0x02) > 0;
					_stage_data.x_cw_moving = ((ext_data[13] & 0x10) | (ext_data[13] & 0x40)) > 0;
					_stage_data.x_ccw_moving = ((ext_data[13] & 0x20) | (ext_data[13] & 0x80)) > 0;
					if((ext_data[11] & 0x01) > 0)
						_stage_data.x_border = "LawnGreen";
					else
						_stage_data.x_border = "Red";
					break;
				case YAXIS:
					temp = temp * Y_NM_PER_COUNT / 1e3;
					_stage_data.y_quad_pos = temp.ToString("F1") + "µm";
					_stage_data.y_cw_limit = (ext_data[13] & 0x01) > 0;
					_stage_data.y_ccw_limit = (ext_data[13] & 0x02) > 0;
					_stage_data.y_cw_moving = ((ext_data[13] & 0x10) | (ext_data[13] & 0x40)) > 0;
					_stage_data.y_ccw_moving = ((ext_data[13] & 0x20) | (ext_data[13] & 0x80)) > 0;
					if ((ext_data[11] & 0x01) > 0)
						_stage_data.y_border = "LawnGreen";
					else
						_stage_data.y_border = "Red";
					break;
				case ZAXIS:
					temp = temp * Z_NM_PER_COUNT / 1e3;
					_stage_data.z_quad_pos = temp.ToString("F1") + "µm";
					_stage_data.z_cw_limit = (ext_data[13] & 0x01) > 0;
					_stage_data.z_ccw_limit = (ext_data[13] & 0x02) > 0;
					_stage_data.z_cw_moving = ((ext_data[13] & 0x10) | (ext_data[13] & 0x40)) > 0;
					_stage_data.z_ccw_moving = ((ext_data[13] & 0x20) | (ext_data[13] & 0x80)) > 0;
					if ((ext_data[11] & 0x01) > 0)
						_stage_data.z_border = "LawnGreen";
					else
						_stage_data.z_border = "Red";
					break;
				case RAXIS:
					temp = temp * R_NM_PER_COUNT / 1e3;
					_stage_data.r_quad_pos = temp.ToString("F4") + "°";
					_stage_data.r_cw_limit = (ext_data[13] & 0x01) > 0;
					_stage_data.r_ccw_limit = (ext_data[13] & 0x02) > 0;
					_stage_data.r_cw_moving = ((ext_data[13] & 0x10) | (ext_data[13] & 0x40)) > 0;
					_stage_data.r_ccw_moving = ((ext_data[13] & 0x20) | (ext_data[13] & 0x80)) > 0;
					if ((ext_data[11] & 0x01) > 0)
						_stage_data.r_border = "LawnGreen";
					else
						_stage_data.r_border = "Red";
					break;
			}
			break;
		case MGMSG_MOT_GET_PMDSTAGEAXISPARAMS:

			break;
	}
}