#include <dsensor.h>        // dsensorライブラリHeaderのInclude
#include <unistd.h>         // shutdown_requested()のため
#include <dmotor.h>         // dmotorライブラリHeaderのInclude
#include <conio.h>
#include <string.h>

#include "in_course.h"

#if defined(DEBUG_LOG)
#include <lnp.h>
#endif /*defined(DEBUG_LOG)*/
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void init_drive_data(DRIVE_DATA *drive_data);
void set_driving(DRIVE_DATA *drive_data, DRIVE_STAGE stage, LINE_COLOR color);
void get_drive_stage(LINE_COLOR color, DRIVE_STAGE *stage);
//void get_color(DRIVE_DATA *drive_data, volatile unsigned sensor, LINE_COLOR *color );
void get_color(DRIVE_DATA *drive_data, unsigned short sensor, LINE_COLOR *color );
//------------------------------------------------------------------------------

#if defined(DEBUG_LOG)

#define DEBUG_CALIB_ARR 10
unsigned char debug_calib[DEBUG_CALIB_ARR];
unsigned char debug_calib_count = 0;

#define DEBUG_DRIVE_COLOR_ARR 0xfff
unsigned char debug_drive_color[DEBUG_DRIVE_COLOR_ARR];
unsigned long debug_drive_color_count = 0;

unsigned char debug_fixed_gray_sensor[DEBUG_DRIVE_COLOR_ARR];

void debug_send_log(void);
#endif /*defined(DEBUG_LOG)*/


DRIVE_DATA drive_data;
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// 必ず黒線の上からスタートすること
//------------------------------------------------------------------------------

int main(int argc, char **argv)
{
	static DRIVE_STAGE stage = DRIVE_STAGE1;
	static LINE_COLOR color = COLOR_BLACK;

	/*データ初期化*/
	init_drive_data(&drive_data);

#if defined(DEBUG_LOG)
	memset(debug_calib, 0xff, sizeof(DEBUG_CALIB_ARR));
	memset(debug_drive_color, 0xff, sizeof(DEBUG_DRIVE_COLOR_ARR));
#endif /*defined(DEBUG_LOG)*/
	

	/*光センサーをアクティブにする*/
	ds_active(&USE_SENSOR);

	/*キャリブレーションスタート*/
	start_calib(&drive_data);

	/*KEY_PRGMキーで走行開始*/
	sleep(1);
	cputs("S");
	wait_event(dkey_pressed ,KEY_PRGM);


#if defined(DEBUG_LOG)
	lnp_addressing_write("result calib",12,0x80,0);
	debug_calib[0] = (drive_data.calib_black & 0xff00) >> 8;
	debug_calib[1] = (drive_data.calib_black & 0x00ff);

	debug_calib[2] = (drive_data.calib_gray & 0xff00) >> 8;
	debug_calib[3] = (drive_data.calib_gray & 0x00ff);

	debug_calib[4] = (drive_data.calib_white & 0xff00) >> 8;
	debug_calib[5] = (drive_data.calib_white & 0x00ff);

	debug_calib[6] = (drive_data.border_gb & 0xff00) >> 8;
	debug_calib[7] = (drive_data.border_gb & 0x00ff);

	debug_calib[8] = (drive_data.border_wg & 0xff00) >> 8;
	debug_calib[9] = (drive_data.border_wg & 0x00ff);
	debug_calib_count = 10;

	cls(); // clear lcd
#endif /*defined(DEBUG_LOG)*/

	/*走行開始*/
	while (!dkey_pressed(KEY_VIEW)) {       //Viewボタンを押されるまでループ

		/*センサ値から色取得*/
		get_color(&drive_data, USE_SENSOR, &color);
		
		/*走行場所取得*/
		get_drive_stage(color, &stage);

		/*走行設定*/
		set_driving(&drive_data, stage, color);

		DRIVE_MOTOR_SPEED(drive_data.driving_speed);
		DRIVE_MOTOR_DIR(drive_data.driving_vector);

		STEERING_MOTOR_SPEED(drive_data.steering_speed);
		STEERING_MOTOR_DIR(drive_data.steering_vector);

	}
	/*モーターストップ*/
	DRIVE_MOTOR_DIR(CTRL_DIRVE_P);
	STEERING_MOTOR_DIR(CTRL_DIRVE_P);

	/*センサをパッシブにする*/
	ds_passive(&USE_SENSOR);
	
	sleep(1);
	/*走行停止したときにLCDを確認できるようにここでPgrmキーを待つ*/
	wait_event(dkey_pressed ,KEY_PRGM);
	sleep(1);

#if defined(DEBUG_LOG)
	sleep(1);
	cputs("LOG");
	wait_event(dkey_pressed ,KEY_PRGM);
	debug_send_log();
	cls(); // clear lcd
#endif /*defined(DEBUG_LOG)*/
	
	cputs("END");

	return 0;
}

void init_drive_data(DRIVE_DATA *drive_data)
{
	memset(drive_data, 0, sizeof(DRIVE_DATA));

	/*走行速度*/
	drive_data->driving_speed = DRIVE_SPEED_NOMAL;
	/*走行方向*/
	drive_data->driving_vector = CTRL_DIRVE_N;
	/*ステアリング速度*/
	drive_data->steering_speed = STEERING_SPEED_NOMAL;
	/*ステアリング方向*/
	drive_data->steering_vector = CTRL_STEERING_FREE;

	/*黒色確定回数*/
	drive_data->fix_color_count_b = FIXED_COLOR_COUNT_NOMAL;
	/*灰色確定回数*/
	drive_data->fix_color_count_g = FIXED_COLOR_COUNT_TRIGER;
	/*白色確定回数*/
	drive_data->fix_color_count_w = FIXED_COLOR_COUNT_NOMAL;
}
		
	
void set_driving(DRIVE_DATA *drive_data, DRIVE_STAGE stage, LINE_COLOR color)
{
	switch(stage) {
	case DRIVE_STAGE2:		/*難所入口(灰色区間)*/
	case DRIVE_STAGE6:		/*ループA出口(灰色区間)*/
	case DRIVE_STAGE8:		/*ループB入口(灰色区間)*/
	case DRIVE_STAGE12:		/*難所出口(灰色区間)*/
	case DRIVE_STAGE18:		/*難所入口(灰色区間) 2週目 */
	case DRIVE_STAGE22:		/*ループA出口(灰色区間) 2週目 */
	case DRIVE_STAGE28:		/*難所出口(灰色区間) 2週目 */
	/*ライン左側を走行*/
		/*走行速度*/
		drive_data->driving_speed = DRIVE_SPEED_NOMAL;
		/*走行方向*/
		drive_data->driving_vector = CTRL_DIRVE_D;
		/*ステアリング速度*/
		drive_data->steering_speed = STEERING_SPEED_NOMAL;
		switch(color) {
		case COLOR_WHITE:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_RIGHT;
			break;
		case COLOR_GRAY:
		case COLOR_BLACK:
		default:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_LEFT;
			break;
		}
		/*色の確定回数をセット*/
		drive_data->fix_color_count_b = FIXED_COLOR_COUNT_TRIGER;
		drive_data->fix_color_count_g = FIXED_COLOR_COUNT_NOMAL;
		break;
	case DRIVE_STAGE14:		/*坂路入口(灰色区間)*/
	case DRIVE_STAGE16:		/*坂路出口(灰色区間)*/
	case DRIVE_STAGE30:		/*坂路入口(灰色区間) 2週目 */
	case DRIVE_STAGE32:		/*坂路出口(灰色区間) 2週目 */
	/*ライン左側を走行*/
		/*走行速度*/
		drive_data->driving_speed = DRIVE_SPEED_NOMAL;
		/*走行方向*/
		drive_data->driving_vector = CTRL_DIRVE_D;
		/*ステアリング速度*/
		drive_data->steering_speed = STEERING_SPEED_NOMAL;
		switch(color) {
		case COLOR_WHITE:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_RIGHT;
			break;
		case COLOR_GRAY:
		case COLOR_BLACK:
		default:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_LEFT;
			break;
		}
		/*色の確定回数をセット*/
		drive_data->fix_color_count_b = FIXED_COLOR_COUNT_TRIGER;
		drive_data->fix_color_count_g = FIXED_COLOR_COUNT_NOMAL;
		break;
	case DRIVE_STAGE3:		/*難所入口〜ループA入口*/
	case DRIVE_STAGE7:		/*ループA出口〜ループB入口*/
	case DRIVE_STAGE9:		/*ループB入口〜ループB出口*/
	case DRIVE_STAGE19:		/*難所入口〜ループA入口 2週目 */
	case DRIVE_STAGE23:		/*ループA出口〜ループB入口 2週目 */
	case DRIVE_STAGE24:		/*ループB入口(灰色区間) 2週目 */
	case DRIVE_STAGE25:		/*ループB入口〜ループB出口 2週目 */
	/*ライン左側を走行*/
		/*走行速度*/
		drive_data->driving_speed = DRIVE_SPEED_NOMAL;
		/*走行方向*/
		drive_data->driving_vector = CTRL_DIRVE_D;
		/*ステアリング速度*/
		drive_data->steering_speed = STEERING_SPEED_NOMAL;
		switch(color) {
		case COLOR_WHITE:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_RIGHT;
			break;
		case COLOR_GRAY:
		case COLOR_BLACK:
		default:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_LEFT;
			break;
		}
		/*色の確定回数をセット*/
		drive_data->fix_color_count_b = FIXED_COLOR_COUNT_NOMAL;
		drive_data->fix_color_count_g = FIXED_COLOR_COUNT_TRIGER;
		break;
	case DRIVE_STAGE1:		/*スタート〜難所難所入口*/
	case DRIVE_STAGE13:		/*難所出口〜坂道入口*/
	case DRIVE_STAGE15:		/*坂路入口〜坂路出口*/
	case DRIVE_STAGE17:		/*坂路出口〜難所入口 2週目 */
	case DRIVE_STAGE29:		/*難所出口〜坂道入口 2週目 */
	case DRIVE_STAGE31:		/*坂路入口〜坂路出口 2週目 */
	/*ライン左側を走行*/
		/*走行速度*/
		drive_data->driving_speed = DRIVE_SPEED_NOMAL;
		/*走行方向*/
		drive_data->driving_vector = CTRL_DIRVE_D;
		/*ステアリング速度*/
		drive_data->steering_speed = STEERING_SPEED_NOMAL;
		switch(color) {
		case COLOR_WHITE:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_RIGHT;
			break;
		case COLOR_GRAY:
		case COLOR_BLACK:
		default:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_LEFT;
			break;
		}
		/*色の確定回数をセット*/
		drive_data->fix_color_count_b = FIXED_COLOR_COUNT_NOMAL;
		drive_data->fix_color_count_g = FIXED_COLOR_COUNT_TRIGER;
		break;
	case DRIVE_STAGE4:		/*ループA入口(灰色区間)*/
	case DRIVE_STAGE10:		/*ループB出口(灰色区間)*/
	case DRIVE_STAGE20:		/*ループA入口(灰色区間) 2週目 */
	case DRIVE_STAGE26:		/*ループB出口(灰色区間) 2週目 */
	/*ライン右側を走行*/
		/*走行速度*/
		drive_data->driving_speed = DRIVE_SPEED_NOMAL;
		/*走行方向*/
		drive_data->driving_vector = CTRL_DIRVE_D;
		/*ステアリング速度*/
		drive_data->steering_speed = STEERING_SPEED_NOMAL;
		
		switch(color) {
		case COLOR_WHITE:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_LEFT;
			break;
		case COLOR_GRAY:
		case COLOR_BLACK:
		default:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_RIGHT;
			break;
		}
		/*色の確定回数をセット*/
		drive_data->fix_color_count_b = FIXED_COLOR_COUNT_TRIGER;
		drive_data->fix_color_count_g = FIXED_COLOR_COUNT_NOMAL;
		break;
	case DRIVE_STAGE5:		/*ループA入口〜ループA出口*/
	case DRIVE_STAGE11:		/*ループB出口〜難所出口*/
	case DRIVE_STAGE21:		/*ループA入口〜ループA出口 2週目 */
	case DRIVE_STAGE27:		/*ループB出口〜難所出口 2週目 */
	/*ライン右側を走行*/
		/*走行速度*/
		drive_data->driving_speed = DRIVE_SPEED_NOMAL;
		/*走行方向*/
		drive_data->driving_vector = CTRL_DIRVE_D;
		/*ステアリング速度*/
		drive_data->steering_speed = STEERING_SPEED_NOMAL;
		
		switch(color) {
		case COLOR_WHITE:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_LEFT;
			break;
		case COLOR_GRAY:
		case COLOR_BLACK:
		default:
			/*ステアリング方向*/
			drive_data->steering_vector = CTRL_STEERING_RIGHT;
			break;
		}
		/*色の確定回数をセット*/
		drive_data->fix_color_count_b = FIXED_COLOR_COUNT_NOMAL;
		drive_data->fix_color_count_g = FIXED_COLOR_COUNT_TRIGER;
		break;
	}
}

void get_drive_stage(LINE_COLOR color, DRIVE_STAGE *stage)
{
	switch(*stage) {
		case DRIVE_STAGE1:		/*スタート〜難所難所入口*/
		case DRIVE_STAGE3:		/*難所入口〜ループA入口*/
		case DRIVE_STAGE5:		/*ループA入口〜ループA出口*/
		case DRIVE_STAGE7:		/*ループA出口〜ループB入口*/
		case DRIVE_STAGE9:		/*ループB入口〜ループB出口*/
		case DRIVE_STAGE11:		/*ループB出口〜難所出口*/
		case DRIVE_STAGE13:		/*難所出口〜坂道入口*/
		case DRIVE_STAGE15:		/*坂路入口〜坂路出口*/
		case DRIVE_STAGE17:		/*坂路出口〜難所難所入口 2週目 */
		case DRIVE_STAGE19:		/*難所入口〜ループA入口 2週目 */
		case DRIVE_STAGE21:		/*ループA入口〜ループA出口 2週目 */
		case DRIVE_STAGE23:		/*ループA出口〜ループB入口 2週目 */
		case DRIVE_STAGE25:		/*ループB入口〜ループB出口 2週目 */
		case DRIVE_STAGE27:		/*ループB出口〜難所出口 2週目 */
		case DRIVE_STAGE29:		/*難所出口〜坂道入口 2週目 */
		case DRIVE_STAGE31:		/*坂路入口〜坂路出口 2週目 */
			if(color == COLOR_GRAY) {
				/*灰色で次のステージへ遷移*/
				*stage += 1;
			}
		break;
		case DRIVE_STAGE2:		/*難所入口(灰色区間)*/
		case DRIVE_STAGE4:		/*ループA入口(灰色区間)*/
		case DRIVE_STAGE6:		/*ループA出口(灰色区間)*/
		case DRIVE_STAGE8:		/*ループB入口(灰色区間)*/
		case DRIVE_STAGE10:		/*ループB出口(灰色区間)*/
		case DRIVE_STAGE12:		/*難所出口(灰色区間)*/
		case DRIVE_STAGE14:		/*坂路入口(灰色区間)*/
		case DRIVE_STAGE16:		/*坂路出口(灰色区間)*/
		case DRIVE_STAGE18:		/*難所入口(灰色区間) 2週目 */
		case DRIVE_STAGE20:		/*ループA入口(灰色区間) 2週目 */
		case DRIVE_STAGE22:		/*ループA出口(灰色区間) 2週目 */
		case DRIVE_STAGE24:		/*ループB入口(灰色区間) 2週目 */
		case DRIVE_STAGE26:		/*ループB出口(灰色区間) 2週目 */
		case DRIVE_STAGE28:		/*難所出口(灰色区間) 2週目 */
		case DRIVE_STAGE30:		/*坂路入口(灰色区間) 2週目 */
		case DRIVE_STAGE32:		/*坂路出口(灰色区間) 2週目 */
			if(color == COLOR_BLACK) {
				/*黒色で次のステージへ遷移*/
				*stage += 1;
			}
		break;
	}

#if defined(DEBUG_LOG)
	cputw(*stage+1);
#endif /*defined(DEBUG_LOG)*/
}

//void get_color(DRIVE_DATA *drive_data, volatile unsigned sensor, LINE_COLOR *color )
void get_color(DRIVE_DATA *drive_data, unsigned short sensor, LINE_COLOR *color )
{
	static unsigned char b_counter = 0;
	static unsigned char g_counter = 0;
	static unsigned char w_counter = 0;
	static unsigned long count = 0;
	
	if (sensor < drive_data->border_wg) {
	/*白色*/
		b_counter = 0;
		g_counter = 0;
		w_counter++;
		if (w_counter > drive_data->fix_color_count_w) {
			w_counter = 0;
			*color = COLOR_WHITE;
		}
	} else if ( (sensor >= drive_data->border_wg) && (sensor <= drive_data->border_gb) ) {
	/*灰色*/
		debug_fixed_gray_sensor[count] = (sensor & 0xff00) >> 8;
		debug_fixed_gray_sensor[count+1] = (sensor & 0x00ff);
		count+=2;
		b_counter = 0;
		w_counter = 0;
		g_counter++;
		if (g_counter > drive_data->fix_color_count_g) {
			g_counter = 0;
			*color = COLOR_GRAY;
		}
	} else {
	/*黒色*/
		g_counter = 0;
		w_counter = 0;
		b_counter++;
		if (b_counter > drive_data->fix_color_count_b) {
			b_counter = 0;
			*color = COLOR_BLACK;
		}
	}

#if defined(DEBUG_LOG)
	if (debug_drive_color_count < DEBUG_DRIVE_COLOR_ARR) {
		debug_drive_color[debug_drive_color_count++] = *color;
	}
#endif /*defined(DEBUG_LOG)*/
	
}

#if defined(DEBUG_LOG)
void debug_send_log(void)
{
	unsigned long count;
	
	lnp_addressing_write("DEBUG_CALIB_ARR",15,0x80,0);
	for (count = 0; count<debug_calib_count; count+=2) {
		lnp_addressing_write(&(debug_calib[count]),2,0x80,0);
	}

	lnp_addressing_write("DEBUG_DRIVE_COLOR_ARR",21,0x80,0);
	for (count = 0; count<debug_drive_color_count; count+=5) {
		lnp_addressing_write(&(debug_drive_color[count]),5,0x80,0);
	}
	lnp_addressing_write("DEBUG_GRAY_COLOR",16,0x80,0);
	for (count = 0; count<DEBUG_DRIVE_COLOR_ARR; count+=2) {
		lnp_addressing_write(&(debug_fixed_gray_sensor[count]),2,0x80,0);
	}
	sleep(5);
	
}
#endif /*defined(DEBUG_LOG)*/