#include <kernel.h>
#include <t_syslog.h>
#include <t_stdlib.h>

#include "kernel_cfg.h"
#include "dev_sw.h"
#include "dev_mtr.h"
#include "dev_ad.h"
#include "dev_ls.h"
#include "dev_lcd.h"
#include "line_trace.h"

/*
 * ラインセンサの値を取得
 * 下位3ビットでライン上かどうかをあらわす
 * 0ビット目が右、1ビット目が中央、2ビット目が左
 */
unsigned char get_linesensor_val(void){
	volatile unsigned char left_val = 0;
	volatile unsigned char center_val = 0;
	volatile unsigned char right_val = 0;
	
	volatile unsigned char line_val = 0;
	
	/* センサの値取得 */
	right_val = ls_read(LS_RIGHT);
	center_val = ls_read(LS_CENTER);
	left_val = ls_read(LS_LEFT);
	
	if( right_val == LINE_ON ){
		line_val |= 0x01;	
	}
	if( center_val == LINE_ON ){
		line_val |= 0x02;
	}
	if( left_val == LINE_ON ){
		line_val |= 0x04;
	}
	
	return line_val;	
}

/* 
 * ライントレース
 * 引数：ラインセンサの値、速度
 */
void line_trace(unsigned char line_val,int left_sp,int right_sp)
{
	switch(line_val){
	case 0:	/* LINE_OFF LINE_OFF LINE_OFF */
		/* 後退 */
		mtr_set(BACK,left_sp,BACK,right_sp);
		break;
	case 1:	 /* LINE_OFF LINE_OFF LINE_ON */
		/* 右に旋回 */
		mtr_set(GO,left_sp,STOP,right_sp);
		break;
	case 2:	/* LINE_OFF LINE_ON LINE_OFF */
		/* 前進 */
		mtr_set(GO,left_sp,GO,right_sp);
		break;
	case 3:	/* LINE_OFF LINE_ON LINE_ON */
		/* 右に旋回 */
		mtr_set(GO,left_sp,STOP,right_sp);
		break;
	case 4:	/* LINE_ON LINE_OFF LINE_OFF */
		/* 左に旋回 */
		mtr_set(STOP,left_sp,GO,right_sp);
		break;
	case 5:	/* LINE_ON LINE_OFF LINE_ON */
		/* 停止 */
		mtr_set(STOP,left_sp,STOP,right_sp);
		break;
	case 6:	/* LINE_ON LINE_ON LINE_OFF */
		/* 左に旋回 */
		mtr_set(STOP,left_sp,GO,right_sp);
		break;
	case 7:	/* LINE_ON LINE_ON LINE_ON */
		/* 前進 */
		mtr_set(GO,left_sp,GO,right_sp);
		break;
	default:
		/* ここに至ることはない */
		break;
	}
}

/*
 * 初期化タスク
 */
void init_task(intptr_t exinf)
{
	/* スイッチ初期化 */
	sw_initialize();

	/* モーター初期化 */
	mtr_initialize();

	/* ラインセンサ初期化 */
	ls_initialize();

	/* A/D変換初期化 */
	ad_initialize();

	/* vd3の初期化 */
	vd3_initialize();
	
	/* タスクの終了 */
	ext_tsk();	
}

/*
 * 照度センサタスク（明るい)
 */
void bright_high_task(intptr_t exinf)
{	
	while(1){
		/* このタスクを休止状態にする */
		slp_tsk();
		
		/* 明るいときは赤で状態表示 */
		show_color(RED,0x00);
		
		/* 停止 */
		mtr_set(STOP,0,STOP,0);
	}
}

/*
 * 照度センサタスク（暗い)
 */
void bright_low_task(intptr_t exinf)
{
	while(1){
		/* このタスクを休止状態にする */
		slp_tsk();
		
		/* 暗いときは黒で状態表示 */
		show_color(BLACK,get_linesensor_val());
		
		/* 低速でライントレース */
		line_trace(get_linesensor_val(),50,50);

	}
}

/* 起動待ちタスク */
void sw_wait_task(intptr_t exinf)
{
	unsigned int i = 0;
	
	while(1){
		/* スイッチは青で状態表示 */
		show_color(BLUE,0x00);
		
		/* 停止 */
		mtr_set(STOP,0,STOP,0);
		
		if(sw_read() == SW_ON){
			/* 少し待ち */
			dly_tsk(200);

			/* メインタスクを起床 */
			wup_tsk(MAIN_TASK);
			
			/* このタスクを休止状態にする */
			slp_tsk();
		}
	}
}

/*
 *  メインタスク
 */
void main_task(intptr_t exinf)
{
	unsigned int ad_val;
	 
	/* 初期化タスクの起動 */
	act_tsk(INI_TASK); 
	
	/* 照度センサタスクの起動 */
	act_tsk(BRIGHT_HIGH_TASK);
	act_tsk(BRIGHT_LOW_TASK);

	/* 起動待ちタスクの起動 */
	act_tsk(SW_WAIT_TASK);
	
	/* このタスクを休止状態にする */
	slp_tsk();
	
	/* メインループ */
	do {
		/* A/D変換の値を取得 */
		ad_val = ad_bright();
		
		if(ad_val <= 150){
			/* 明るい時のタスク起動 */
			wup_tsk(BRIGHT_HIGH_TASK);
		}else if(ad_val >= 550){
			/* 暗い時のタスク起動 */
			wup_tsk(BRIGHT_LOW_TASK);
		}else{
			/* ライントレースは緑で状態表示 */
			show_color(GREEN,get_linesensor_val());
		
			/* ライントレース */
			line_trace(get_linesensor_val(),75,75);
		}

		if(sw_read() == SW_ON){
			/* 少し待ち */
			dly_tsk(200);
			
			/* 起動待ちタスクを起床 */
			wup_tsk(SW_WAIT_TASK);
			
			/* このタスクを休止状態にする */
			slp_tsk();
		}
		
		dly_tsk(40);
	} while (1);

	assert(0);
}
