#ifndef _ULTRAVSOUND_
#define _ULTRAVSOUND_
#include "HardwareInfo.c"
#include "turnAngle.c"
#include <GetUltrasound.h>
#include "goline.c"
#include "returnOut.c"
#include "comeBack.c"

void ultravsound(unsigned char dis_left, unsigned char yfy, unsigned char dis_right, unsigned char dis_middle, unsigned char middleDistance, unsigned char backDistance, unsigned int distance)
{
    unsigned int distance_middle = 0;   //裝在機器人后面的超聲波測距模塊的返回變量值。
    unsigned int distance_left = 0;   //裝在機器人左面的超聲波測距模塊的返回變量值。
    unsigned char i = 0;   //returnOut()的返回值。
    turnAngle(0, yfy);
    for (int _FOR_0_ = 0; _FOR_0_ < 3; _FOR_0_++ )
    {
        distance_left = GetUltrasound(_ULTRASOUND_left_);
        if ( distance_left<110 )
        {
            break;
        }
    }
    for (int _FOR_1_ = 0; _FOR_1_ < 3; _FOR_1_++ )
    {
        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
        if ( distance_middle<=110 )
        {
            break;
        }
    }
    if ( distance_left<dis_left )
    {
        while ( (distance_middle<dis_middle) )
        {
            goline(8, yfy, 90, distance);
            distance_middle = GetUltrasound(_ULTRASOUND_middle_);
            while ( distance_middle==888 )
            {
                distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                i = returnOut(yfy);
                if ( i==1 )
                {
                    return ;
                }
            }
            i = returnOut(yfy);
            if ( i==1 )
            {
                return ;
            }
        }
        turnAngle(0, yfy);
    }
    else
    {
        if ( distance_left>dis_right )
        {
            while ( (distance_middle<dis_middle) )
            {
                goline(8, yfy, 270, distance);
                distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                while ( distance_middle==888 )
                {
                    distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                    i = returnOut(yfy);
                    if ( i==1 )
                    {
                        return ;
                    }
                }
                i = returnOut(yfy);
                if ( i==1 )
                {
                    return ;
                }
            }
            turnAngle(0, yfy);
        }
    }
    comeBack(yfy, middleDistance, backDistance);
    turnAngle(0, yfy);
}
#endif