///////////////////////////////////////////////////////////
//  model_impl.c
///////////////////////////////////////////////////////////

#include "OutputPort.h"
#include "InputPort.h"

#include "LineTracer.h"
#include "LightSensor.h"
#include "GyroSensor.h"
#include "Motor.h"
#include "TouchSensor.h"


#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

// オブジェクトを静的に確保する
LineTracer lineTracer;
LightSensor lightSensor;
GyroSensor gyroSensor;
Motor leftMotor, rightMotor;
TouchSensor touchSensor;

void ecrobot_device_initialize()
{
	ecrobot_init_bt_slave("1234");
}

void ecrobot_device_terminate()
{
	ecrobot_term_bt_connection();
}

void user_1ms_isr_type2(void)
{
}

TASK(TaskMain)
{

	// ポートの指定
	OUTPUT_PORT port_motor_r = B;
	OUTPUT_PORT port_motor_l = C;
	INPUT_PORT port_gyro_sensor = S1;
	INPUT_PORT port_light_sensor = S3;
	INPUT_PORT port_touch_sensor = S4;
	
	// 各オブジェクトを初期化する
	LineTracer_init(&lineTracer);
	LightSensor_init(&lightSensor, port_light_sensor);
	GyroSensor_init(&gyroSensor, port_gyro_sensor);
	Motor_init(&leftMotor, port_motor_l);
	Motor_init(&rightMotor, port_motor_r);
	TouchSensor_init(&touchSensor, port_touch_sensor);

	// オブジェクト間のリンクを構築する
	lineTracer.lightSensor = &lightSensor;
	lineTracer.gyroSensor = &gyroSensor;
	lineTracer.leftMotor = &leftMotor;
	lineTracer.rightMotor = &rightMotor;
	lineTracer.touchSensor = &touchSensor;

	// 4ms 周期で、ライントレーサにトレース処理を依頼する
	while(1)
	{
		LineTracer_traceLine(&lineTracer);
		systick_wait_ms(4);
	}
}
