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

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

#include "UserInterface.h"
#include "TouchSensor.h"

#include "LineTracer.h"
#include "LineMonitor.h"
#include "LightSensor.h"

#include "DriveUnit.h"
#include "GyroSensor.h"
#include "Motor.h"


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

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

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);
	TouchSensor_init(&touchSensor, port_touch_sensor);
	GyroSensor_init(&gyroSensor, port_gyro_sensor);
	Motor_init(&leftMotor, port_motor_l);
	Motor_init(&rightMotor, port_motor_r);

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

	lineTracer.lineMonitor = &lineMonitor;
	lineMonitor.lightSensor = &lightSensor;

	lineTracer.driveUnit = &driveUnit;
	driveUnit.gyroSensor = &gyroSensor;
	driveUnit.leftMotor = &leftMotor;
	driveUnit.rightMotor = &rightMotor;

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