void PuzzleField::runPuzzleFieldVectorChange()

in str/apps/src/PuzzleField.cpp [46:148]
82 lines of code
15 McCabe index (conditional complexity)

void PuzzleField::runPuzzleFieldVectorChange ( int8_t currentPosition, int8_t beforePosition, int8_t nextPosition, int32_t vectorStatus )
{
	char msg[32] ;
	// 現在位置からの接続番号の付与
	connectNumber = connectNumberManager[currentPosition][nextPosition] ;
	// 行先への角度を取得する
	if ( beforePosition == X ) {
		currentDegree = 60 ;
	} else {
		beforeconnectNumber = connectNumberManager[beforePosition][currentPosition] ;
		currentDegree = allconnectNumber[beforePosition][beforeconnectNumber][DEGREE] - vectorStatus ;
		if ( vectorStatus == 180 && currentDegree < -180 ) {
			currentDegree += 360 ;
		}
	}
	nextDegree = allconnectNumber[currentPosition][connectNumber][DEGREE] ;
	spinDegree = nextDegree - currentDegree ;
	if ( spinDegree < 0 ) {
		spinVector = basicWalker.SPIN_RIGHT ;
		spinDegree *= -1 ;
		sprintf ( msg, "Spin Vector:SPIN_RIGHT" ) ;
		msg_f ( msg, 6 ) ;
	} else if ( 180 < spinDegree ) {
		spinVector = basicWalker.SPIN_RIGHT ;
		spinDegree -= 360 ;
		spinDegree *= -1 ;
		sprintf ( msg, "Spin Vector:SPIN_RIGHT" ) ;
		msg_f ( msg, 6 ) ;
	} else {
		spinVector = basicWalker.SPIN_LEFT ;
		sprintf ( msg, "Spin Vector:SPIN_LEFT " ) ;
		msg_f ( msg, 6 ) ;
	}
	if ( spinDegree < 90 ) {
		spinDegree -= spinDegree / 5 ;
	} else {
		spinDegree += spinDegree / 10 ;
	}
	// 行先までの距離を取得する
	nextDistance = allconnectNumber[currentPosition][connectNumber][DISTANCE] ;
	// 値が取れているか確認用のログ
	sprintf ( msg, "Degree>Current:%ld,Next:%ld", currentDegree, nextDegree ) ;
	msg_f ( msg, 4 ) ;
	sprintf ( msg, "          Spin:%ld", spinDegree ) ;
	msg_f ( msg, 5 ) ;
    sprintf ( msg, "Position>Current:%d->Next:%d", currentPosition, nextPosition ) ; 
    msg_f ( msg, 7 ) ;
	sprintf ( msg, "Distance>Next:%ld", nextDistance ) ;
	msg_f ( msg, 8 ) ;
	// 車体を動かして角度を合わせる
	ev3_speaker_play_tone ( NOTE_FS5, 100 ) ;
	basicWalker.spin ( spinVector, spinDegree ) ;
	tslp_tsk ( 100 ) ;
	// ラインをトレースする
	ev3_speaker_play_tone ( NOTE_FS6, 100 ) ;
	msg_f ( "OK", 9 ) ;
	if ( connectNumber < 4 && nextPosition != 0 ) {
		/*
		basicWalker.goStraight ( 30, nextDistance / 4 ) ;
		modifiedSpinDegree = 0 ;
		modifiedSpinVector = basicWalker.SPIN_LEFT ;
		while ( colorSensor.getBrightness () > 80 ) {
			tslp_tsk ( 50 ) ;
			basicWalker.backStraight ( 36, nextDistance / 4 ) ;
			tslp_tsk ( 50 ) ;
			modifiedSpinVector *= -1 ;
			modifiedSpinDegree += 5 ;
			basicWalker.spin ( 30, modifiedSpinVector, modifiedSpinDegree ) ;
			basicWalker.goStraight ( 30, nextDistance / 4 ) ;
		}
		basicWalker.goStraight ( 30, ( nextDistance / 4 ) * 3 ) ;
		*/
		tslp_tsk ( 100 ) ;
		ev3_speaker_play_tone ( NOTE_FS6, 100 ) ;
		distance.resetDistance ( walker.get_count_L (), walker.get_count_R () ) ;
		traceDistance = 0 ;
		lineTracer.speedControl.setPid ( 2.0, 2.0, 0.1, 40.0 ) ;
		lineTracer.turnControl.setPid ( 5.8, 0.6, 0.1, 36.0 ) ;
		while ( traceDistance < nextDistance ) {
			traceDistance = distance.getDistanceTotal ( walker.get_count_L (), walker.get_count_R () ) ;
			color = colorSensor.getBrightness () ;
			lineTracer.runLine ( walker.get_count_L (), walker.get_count_R (), color ) ;
			walker.run ( lineTracer.getForward (), lineTracer.getTurn () ) ;
			if ( nextDistance - 135 < traceDistance ) {
				if ( colorSensor.getColorNumber () == COLOR_RED || colorSensor.getColorNumber () == COLOR_BLUE || colorSensor.getColorNumber () == COLOR_GREEN || colorSensor.getColorNumber () == COLOR_YELLOW ) {
					basicWalker.reset () ;
					basicWalker.goStraight ( 80, 130 ) ;
					break ;
				}
			}
			tslp_tsk ( 4 ) ;
		}
		walker.run ( 0, 0 ) ;
		walker.reset () ;
		tslp_tsk ( 100 ) ;
	} else {
		basicWalker.goStraight ( 60, nextDistance ) ;
		walker.run ( 0, 0 ) ;
	}
	tslp_tsk ( 100 ) ;
	ev3_speaker_play_tone ( NOTE_FS5, 100 ) ;
	tslp_tsk ( 1000 ) ;
}