#include "HardwareInfo.c"
#include <GetCompoI.h>
#include <SetMotor.h>
#include "turnAngle.c"
#include <GetUltrasound.h>
#include "goline.c"
#include "returnOut.c"
#include "comeBack.c"

int main(void)
{
    unsigned char fyMax = 0;   //當前復眼所有通道的最大光值。
    unsigned char maxIndex = 0;   //當前復眼最大光值的通道號。
    unsigned int distance_middle = 0;   //裝在機器人后面的超聲波測距模塊的返回變量值。
    unsigned int distance_left = 0;   //裝在機器人左面的超聲波測距模塊的返回變量值。
    unsigned char yfy = 70;   //遠距離的光值，一般是指無球情況，用戶要實測。
    unsigned char jfy = 140;   //近距離的光值，是指機器人與球距離多遠的光值，用戶要實測。
    unsigned int dis_middle = 30;   //機器人從場邊走到場中央的距離，用戶要實測。
    unsigned int dis_left = 29;   //機器人離球場邊的距離，用戶要實測。
    unsigned int dis_right = 40;   //機器人離球場邊的距離，用戶要實測。
    unsigned char sr = 0;   //右馬達控制命令字，若為0，向前轉，為1則停止。
    unsigned char sl = 0;   //右馬達控制命令字，若為0，向前轉，為1則停止。
    unsigned char i = 0;   //returnOut()的返回值。
    unsigned char fyMin = 0;   //當前復眼所有通道的最小光值。
    unsigned int distance = 31;   //轉某角度直走，并走一段距離。
    unsigned char value = 50;   //復眼通道的最大值-復眼通道的最小值，其作用：判斷是否發現球。
    while (1)
    {
        fyMax = GetCompoI(_COMPOUNDEYE_fy_, 9);
        fyMin = GetCompoI(_COMPOUNDEYE_fy_, 11);
        if ( (fyMax>yfy)||((fyMax-fyMin)>value) )
        {
            if ( fyMax>jfy )
            {
                maxIndex = GetCompoI(_COMPOUNDEYE_fy_, 8);
                if ( maxIndex==7 )
                {
                    SetMotor(_MOTOR_left_, 0, 80);
                    SetMotor(_MOTOR_right_, 2, 50);
                }
                else
                {
                    if ( maxIndex==6 )
                    {
                        SetMotor(_MOTOR_left_, 0, 80);
                        SetMotor(_MOTOR_right_, 2, 20);
                    }
                    else
                    {
                        if ( maxIndex==5 )
                        {
                            SetMotor(_MOTOR_left_, 0, 50);
                            SetMotor(_MOTOR_right_, 1, 100);
                        }
                        else
                        {
                            if ( maxIndex==1 )
                            {
                                SetMotor(_MOTOR_left_, 2, 50);
                                SetMotor(_MOTOR_right_, 0, 100);
                            }
                            else
                            {
                                if ( maxIndex==2 )
                                {
                                    SetMotor(_MOTOR_left_, 2, 20);
                                    SetMotor(_MOTOR_right_, 0, 100);
                                }
                                else
                                {
                                    if ( maxIndex==3 )
                                    {
                                        SetMotor(_MOTOR_left_, 1, 100);
                                        SetMotor(_MOTOR_right_, 0, 50);
                                    }
                                    else
                                    {
                                        SetMotor(_MOTOR_left_, 0, 100);
                                        SetMotor(_MOTOR_right_, 0, 70);
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            {
                maxIndex = GetCompoI(_COMPOUNDEYE_fy_, 8);
                if ( maxIndex<4 )
                {
                    sl=1;
                    sr=0;
                    SetMotor(_MOTOR_left_, sl, 100);
                    SetMotor(_MOTOR_right_, sr, 100);
                }
                else
                {
                    if ( maxIndex>4 )
                    {
                        sl=0;
                        sr=1;
                        SetMotor(_MOTOR_left_, sl, 100);
                        SetMotor(_MOTOR_right_, sr, 100);
                    }
                    else
                    {
                        sl=0;
                        sr=0;
                        SetMotor(_MOTOR_left_, sl, 100);
                        SetMotor(_MOTOR_right_, sr, 100);
                    }
                }
            }
        }
        else
        {
            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(7, yfy, 90, distance);
                    distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                    while ( distance_middle==888 )
                    {
                        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                        i = returnOut(yfy);
                        if ( i==1 )
                        {
                            break;
                        }
                    }
                    i = returnOut(yfy);
                    if ( i==1 )
                    {
                        break;
                    }
                }
                turnAngle(0, yfy);
            }
            else
            {
                if ( distance_left>dis_right )
                {
                    while ( (distance_middle<dis_middle) )
                    {
                        goline(7, yfy, 270, distance);
                        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                        while ( distance_middle==888 )
                        {
                            distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                            i = returnOut(yfy);
                            if ( i==1 )
                            {
                                break;
                            }
                        }
                        i = returnOut(yfy);
                        if ( i==1 )
                        {
                            break;
                        }
                    }
                    turnAngle(0, yfy);
                }
            }
            comeBack(yfy, 7, 17);
            turnAngle(0, yfy);
        }
    }
    return 1;
}