/*****************************************************************************
 * FILE: ecrobot_interface.c
 *
 * COPYRIGHT 2007 Takashi Chikamasa <takashic@cybernet.co.jp>
 *
 * DESCRIPTION:
 *   This is a glue code to execute Embedded Coder Robot NXT generated code
 *   by using LEJOS NXJ. 
 *
 *   <About LEJOS NXJ>
 *    LEJOS NXJ is a full firmware replacement of LEGO Mindstorms NXT and 
 *   designed for Java programming environment for the NXT 
 *   ( For more detailed information, please see: http://lejos.sourceforge.net/ )
 *   In the LEJOS NXJ distribution, C source files for NXT platform layer is also
 *   included besides with the Java VM. The platform C source code is well
 *   structured, comprehensive, and achieved higher performance than the LEGO's
 *   one. Therefore, LEJOS NXJ (platform) is also the best GCC based C/C++  
 *   development platform for NXT.
 *
 *   The contents of this file are subject to the Mozilla Public License
 *   Version 1.0 (the "License"); you may not use this file except in
 *   compliance with the License. You may obtain a copy of the License at
 *   http://www.mozilla.org/MPL/
 *
 *   Software distributed under the License is distributed on an "AS IS"
 *   basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
 *   License for the specific language governing rights and limitations
 *   under the License.
 *
 *   The Original Code is TinyVM code, first released March 6, 2000,
 *   later released as leJOS on September 23, 2000.
 *
 *   The Initial Developer of the Original Code is Jose H. Solorzano.
 *
 *   Contributor(s): see LEJOS NXJ ACKNOWLEDGEMENTS .
 *
 ******************************************************************************/

#include <stddef.h>
#include <string.h>

/* ECRobot NXT */
#include "ecrobot_interface.h"


/*
 * gMakeRequest is defined in interpreter.c for Java VM,
 * but not used for platform functions, this is dummy
 */
volatile boolean gMakeRequest;

volatile U8 gDevice_status;

/*==============================================================================
 * ECRobot NXT Servo Motor API
 *=============================================================================*/
/* 
 * Set Servo Motor PWM duty ratio
 */
void ecrobot_set_motor_speed(U8 port_id, S8 speed)
{
	/* 1st arg:port id (0/1/2)        */
	/* 2nd arg:speed (-100 to 100)    */
	/* 3rd arg:mode (0:brake/1:float) */
	nxt_motor_set_speed(port_id, speed, 1);
}

/*
 * Get Servo Motor revolution in degree
 */
S32 ecrobot_get_motor_rev(U8 port_id)
{
	return nxt_motor_get_count(port_id);
}

/*==============================================================================
 * ECRobot NXT A/D Sensors API
 *=============================================================================*/
/* 
 * Turn infra-red light on
 */
void ecrobot_set_light_sensor_active(U8 port_id)
{
	if (gDevice_status == DEVICE_NO_INIT)
	{
		set_digi0(port_id);
	}
}

/* 
 * Turn infra-red light off
 */
void ecrobot_set_light_sensor_inactive(U8 port_id)
{
	unset_digi0(port_id);
}

/*
 * Get Light Sensor raw A/D data
 */
U16 ecrobot_get_light_sensor(U8 port_id)
{
	return (U16)sensor_adc(port_id);
}

/* 
 * Get Touch Sensor on/off status
 */
U8 ecrobot_get_touch_sensor(U8 port_id)
{
	return (sensor_adc(port_id) < 512);
}

/*
 * Get Sound Sensor raw A/D data
 */
U16 ecrobot_get_sound_sensor(U8 port_id)
{
	return (U16)sensor_adc(port_id);
}

/*==============================================================================
 * ECRobot NXT Digital Sensors API
 *=============================================================================*/
static S32 i2c_state; 
/* Ultrasonic Sensor measurement data */
/*
 * Init an I2C for Ultrasonic Sensor
 */
void ecrobot_init_sonar_sensor(U8 port_id)
{
	if (gDevice_status == DEVICE_NO_INIT)
	{
		i2c_state = -1;
		nxt_avr_set_input_power(port_id,2);
		i2c_enable(port_id);
	}
}

/*
 * Get Ultrasonic Sensor measurement data in cm via I2C
 */
S32 ecrobot_get_sonar_sensor(U8 port_id)
{
	static U8 distance;

	if (i2c_busy(port_id) == 0)
	{
	   /* i2c_start_transaction just triggers an I2C transaction,
		* actual data transaction between ARM and AVR in Ultrasonic
		* Sensor is done by an ISR after this, so there is one cycle 
		* delay for consistent data acquistion
		*/
		i2c_state = (S32)distance;
		i2c_start_transaction(port_id,1,0x42,1,&distance,1,0);
	}

	return i2c_state;
}

/*
 * Terminate an I2C for Ultrasonic Sensor
 */
void ecrobot_term_sonar_sensor(U8 port_id)
{
	i2c_disable(port_id);
}

/*==============================================================================
 * ECRobot NXT internal status API
 *=============================================================================*/
/* Buttons and Battery voltage */
static nxt_inputs ecrobot_inputs;

/*
 * Get Battery Voltage in mille volt
 */
U16 ecrobot_get_battery_voltage(void)
{
	return (U16)ecrobot_inputs.battery_state;
}

/*
 * Get System tick in mille second
 */
U32 ecrobot_get_systick_ms(void)
{
	return systick_get_ms();
}

/*
 * Get NXT button state
 */
U8 ecrobot_get_button_state(void)
{
	return ecrobot_inputs.buttons_state;
}

void ecrobot_poll_nxtstate(void)
{
	static U8 buttons_states[N_BTN_STATE];
	static int buttons_i = 0;
	int i;

	ecrobot_inputs.battery_state = battery_voltage();

	/* button debouncer */
	buttons_states[buttons_i++] = buttons_get();
	if (buttons_i == N_BTN_STATE) buttons_i = 0;
	for (i = 1; i < N_BTN_STATE; i++)
	{
		if (buttons_states[i-1] != buttons_states[i])
		{
	  		break;
		}
		else if (i == N_BTN_STATE-1)
    	{
	  		ecrobot_inputs.buttons_state = buttons_states[i];
    	}
  	}
}

/*==============================================================================
 * ECRobot NXT Bluetooth API
 *=============================================================================*/
/* Bluetooth data buffers */
static U8 sendBuf[256];
static U8 receiveBuf[256];
static U8 handle;

/*
 * Send Bluetooth command
 * (dead copy of LEJOS Bluetooth.sendCommand)
 */
static void ecrobot_send_bt_command(U8 *cmd, U32 len)
{
	S32 checkSum = 0;
	S32 i;

	sendBuf[0] = (U8) (len + 2);

	for(i = 0; i < len; i++)
	{
		sendBuf[i+1] = cmd[i];
		checkSum += cmd[i];
	}

    checkSum = -checkSum;
    sendBuf[len+2] = (U8) ((checkSum >> 8) & 0xff);
    sendBuf[len+3] = (U8) (checkSum & 0xff);

	bt_send(sendBuf,len+3);

}

/*
 * Receive Bluetooth reply or command
 * (dead copy of LEJOS Bluetooth.receiveReply)
 */
static S32 ecrobot_receive_bt_reply(U8 *buf, U32 bufLen)
{
	S32 checkSum, negSum, i;
	U32 len;

	memset(&receiveBuf[0],0,sizeof(receiveBuf));
	bt_receive(&receiveBuf[0]);
	len = receiveBuf[0];
	buf[0] = (U8) len;

	if (len == 0)
	{
		return 0;
	}

	checkSum = len;

	if (len-1 <= bufLen)
	{
		for(i = 1; i < len-1; i++) 
		{
			buf[i] = receiveBuf[i];
			checkSum += (buf[i] & 0xff);
		}
		negSum = (receiveBuf[len-1] & 0xff) << 8;
		negSum += (receiveBuf[len] & 0xff);
		if (checkSum + negSum == 65536)
		{
			 return len-1;
		}
		else
		{
			return 0;
		}
	}
	return 0;
}

/*
 * Initialize Bluetooth connection
 */
void ecrobot_init_bt_connection(void)
{
	S32 i;
	U8  reply[32];
	U8  dummy[32];
	U8    msg[32];
	U8  device[7];

	if (gDevice_status == DEVICE_NO_INIT)
	{
		/* low level initialization */
		bt_init();

		/* set Bluetooth command mode */
		bt_clear_arm7_cmd();
	}
	else if (gDevice_status == DEVICE_INITIALIZED)
	{
		/* searching for Bluetooth device
		* (based on LEJOS Bluetooth.waitForConnection)
    	*/
		ecrobot_receive_bt_reply(reply, 32);

		if (reply[0] != 0)
		{
			if (reply[1] == MSG_REQUEST_PIN_CODE)
			{
				for(i = 0; i < 7; i++)
				{
					device[i] = reply[i+2];
				}
				msg[0] = MSG_PIN_CODE;
				for(i = 0; i < 7; i++)
				{
					msg[i+1] = device[i];
				}
				/* PIN CODE is MATLAB */
				msg[8]  = 'M';
				msg[9]  = 'A';
				msg[10] = 'T';
				msg[11] = 'L';
				msg[12] = 'A';
				msg[13] = 'B';
				for(i = 0; i < 10; i++)
				{
					msg[i+14] = 0;
				}

				ecrobot_send_bt_command(msg, 24);
			}	

			if (reply[1] == MSG_REQUEST_CONNECTION)
			{
				for(i = 0; i < 7; i++)
				{
					device[i] = reply[i+2];
				}
				msg[0] = MSG_ACCEPT_CONNECTION;
				msg[1] = 1;

				ecrobot_send_bt_command(msg, 2);
			}

			/* Bluetooth connection was succedded */
			if (reply[1] == MSG_CONNECT_RESULT)
			{
				systick_wait_ms(300);
				ecrobot_receive_bt_reply(dummy,32);

				if (dummy[0] == 0)
				{
					msg[0] = MSG_OPEN_STREAM;
					msg[1] = reply[3];
					handle = reply[3]; /* handle will be used for BT termination */

					ecrobot_send_bt_command(msg, 2);
					systick_wait_ms(300);

					/* set Bluetooth stream mode */
					bt_set_arm7_cmd();
					gDevice_status = BLUETOOTH_CONNECTED;
				} 
			}
		}
	}
}

/*
 * Terminate Bluetooth connection
 */
void ecrobot_term_bt_connection(void)
{
	U8    msg[32];
	U8  reply[32];

	if (gDevice_status == BLUETOOTH_CONNECTED)
	{
		/* set Bluetooth command mode */
		bt_clear_arm7_cmd();
		systick_wait_ms(100);

		msg[0] = MSG_CLOSE_CONNECTION;
		msg[1] = handle;

		ecrobot_send_bt_command(msg, 2);
		systick_wait_ms(100);
	}
}

/*
 * Send Bluetooth data packet
 * (dead copy of LEJOS Bluetooth.sendPacket)
 */
void ecrobot_send_bt_packet(U8 *buf, U32 bufLen)
{
	S32 i;

	if (gDevice_status == BLUETOOTH_CONNECTED && bufLen <= 254)
    {
		sendBuf[0] = (U8) (bufLen & 0xFF);
		sendBuf[1] = (U8) ((bufLen >> 8) & 0xFF);
		for(i = 0; i < bufLen; i++)
		{
			sendBuf[i+2] = buf[i];
		}
		bt_send(&sendBuf[0],bufLen+2);
    }
}

/*
 * Read Bluetooth data packet
 * (dead copy of LEJOS Bluetooth.readPacket)
 */
S32 ecrobot_read_bt_packet(U8 *buf, U32 bufLen)
{
	S32 i;
	U32 len;

	if (gDevice_status == BLUETOOTH_CONNECTED)
	{
		bt_receive(&receiveBuf[0]);
		len = receiveBuf[0];

		if (len > 0 && len <= bufLen)
		{
			for(i = 0; i < len; i++)
			{
				buf[i] = receiveBuf[i+2];
			}
			return len;
		}
		return 0;
	}
	return 0;
}

/*==============================================================================
 * ECRobot NXT Bluetooth data logging API
 *=============================================================================*/
void ecrobot_bt_data_logger(S8 data1, S8 data2)
{
	U8 data_log_buffer[32];

	*((U32 *)(&data_log_buffer[0]))  = (U32)systick_get_ms();
	*(( S8 *)(&data_log_buffer[4]))  =  (S8)data1;
	*(( S8 *)(&data_log_buffer[5]))  =  (S8)data2;
	*((U16 *)(&data_log_buffer[6]))  = (U16)ecrobot_inputs.battery_state;
	*((S32 *)(&data_log_buffer[8]))  = (S32)nxt_motor_get_count(0);
	*((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
	*((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
	*((S16 *)(&data_log_buffer[20])) = (S16)sensor_adc(0);
	*((S16 *)(&data_log_buffer[22])) = (S16)sensor_adc(1);
	*((S16 *)(&data_log_buffer[24])) = (S16)sensor_adc(2);
	*((S16 *)(&data_log_buffer[26])) = (S16)sensor_adc(3);
	*((S32 *)(&data_log_buffer[28])) = (S32)i2c_state;
	
	ecrobot_send_bt_packet(data_log_buffer, 32);
}

/*==============================================================================
 * ECRobot NXT LCD display API
 *=============================================================================*/
void ecrobot_show_int(S32 var)
{
	display_clear(0);

	display_goto_xy(0, 7);
	display_string("VAR: ");
	display_int(var, 0);

	display_update();
}

void ecrobot_debug(unsigned int var)
{
	display_clear(0);

	display_goto_xy(0, 7);
	display_string("DBG: ");
	display_int(var, 0);

	display_update();
}

void ecrobot_debug1(unsigned int var1, unsigned int var2, unsigned int var3)
{
	display_clear(0);

	display_goto_xy(0, 0);
   	display_string((const char *)target_subsystem_name);

	display_goto_xy(0, 1);
	display_string("VAR1: ");
	display_int(var1, 0);

	display_goto_xy(0, 2);
	display_string("VAR2: ");
	display_int(var2, 0);

	display_goto_xy(0, 3);
	display_string("VAR3: ");
	display_int(var3, 0);

	display_update();
}

void ecrobot_debug2(unsigned int var1, unsigned int var2, unsigned int var3)
{
	display_clear(0);

	display_goto_xy(0, 0);
   	display_string((const char *)target_subsystem_name);

	display_goto_xy(0, 4);
	display_string("VAR4: ");
	display_int(var1, 0);

	display_goto_xy(0, 5);
	display_string("VAR5: ");
	display_int(var2, 0);

	display_goto_xy(0, 6);
	display_string("VAR6: ");
	display_int(var3, 0);

	display_update();
}

void ecrobot_status_monitor(void)
{
	display_clear(0);

	display_goto_xy(0, 0);
   	display_string((const char *)target_subsystem_name);

   	display_goto_xy(0, 1);
   	display_string("TIME:");
   	display_unsigned(systick_get_ms()/1000, 0);

   	display_goto_xy(0, 2);
   	display_string("BATT:");
   	display_unsigned(ecrobot_inputs.battery_state/100, 0);
    
   	display_goto_xy(0, 3);
   	display_string("REV: ");
   	display_int(nxt_motor_get_count(0), 0);
   	display_int(nxt_motor_get_count(1), 6);

   	display_goto_xy(0, 4);
   	display_string("     ");
   	display_int(nxt_motor_get_count(2), 0);

   	display_goto_xy(0, 5);
   	display_string("ADC: ");
   	display_unsigned(sensor_adc(0), 0);
   	display_unsigned(sensor_adc(1), 5);

   	display_goto_xy(0, 6);
   	display_string("     ");
   	display_unsigned(sensor_adc(2), 0);
   	display_unsigned(sensor_adc(3), 5);

   	display_goto_xy(0, 7);
   	display_string("BT/I2C: ");
   	if (gDevice_status == BLUETOOTH_CONNECTED)
   	{
		display_int(1, 0);
   	}
   	else
   	{
		display_int(0, 0);
   	}
   	display_int(i2c_state, 5);

	display_update();
}

