#ifndef _FINDBALL_
#define _FINDBALL_
#include "HardwareInfo.c"
#include <GetCompoI.h>
#include <SetMotor.h>
#include <GetCompassB.h>
#include <GetUltrasound.h>

void findBall(unsigned char jGuan)
{
    // extern global var
    extern unsigned int g_distanceLeft;

    unsigned char fyMax = 0;   //當前最大光值的通道號
    unsigned char sl = 0;
    unsigned char sr = 0;
    unsigned char maxIndex = 0;
    unsigned int degree = 0;
    fyMax = GetCompoI(_COMPOUNDEYE_fy_, 9);
    if ( fyMax<jGuan )
    {
        maxIndex = GetCompoI(_COMPOUNDEYE_fy_, 8);
        if ( maxIndex==7 )
        {
            SetMotor(_MOTOR_left_, 0, 80);
            SetMotor(_MOTOR_right_, 2, 60);
        }
        else
        {
            if ( maxIndex==6 )
            {
                SetMotor(_MOTOR_left_, 0, 50);
                SetMotor(_MOTOR_right_, 2, 20);
            }
            else
            {
                if ( maxIndex==5 )
                {
                    SetMotor(_MOTOR_left_, 0, 25);
                    SetMotor(_MOTOR_right_, 1, 0);
                }
                else
                {
                    if ( maxIndex==1 )
                    {
                        SetMotor(_MOTOR_left_, 2, 50);
                        SetMotor(_MOTOR_right_, 0, 80);
                    }
                    else
                    {
                        if ( maxIndex==2 )
                        {
                            SetMotor(_MOTOR_left_, 2, 20);
                            SetMotor(_MOTOR_right_, 0, 45);
                        }
                        else
                        {
                            if ( maxIndex==3 )
                            {
                                SetMotor(_MOTOR_left_, 1, 0);
                                SetMotor(_MOTOR_right_, 0, 25);
                            }
                            else
                            {
                                SetMotor(_MOTOR_left_, 0, 100);
                                SetMotor(_MOTOR_right_, 0, 70);
                            }
                        }
                    }
                }
            }
        }
    }
    else
    {
        maxIndex = GetCompoI(_COMPOUNDEYE_fy_, 8);
        if ( maxIndex<3 )
        {
            sl=1;
            sr=0;
            SetMotor(_MOTOR_left_, sl, 100);
            SetMotor(_MOTOR_right_, sr, 70);
        }
        else
        {
            if ( maxIndex==3 )
            {
                sl=1;
                sr=0;
                SetMotor(_MOTOR_left_, sl, 50);
                SetMotor(_MOTOR_right_, sr, 30);
            }
            else
            {
                if ( maxIndex>5 )
                {
                    sl=0;
                    sr=1;
                    SetMotor(_MOTOR_left_, sl, 100);
                    SetMotor(_MOTOR_right_, sr, 70);
                }
                else
                {
                    if ( maxIndex==5 )
                    {
                        sl=0;
                        sr=1;
                        SetMotor(_MOTOR_left_, sl, 30);
                        SetMotor(_MOTOR_right_, sr, 60);
                    }
                    else
                    {
                        sl=0;
                        sr=0;
                        SetMotor(_MOTOR_left_, sl, 100);
                        SetMotor(_MOTOR_right_, sr, 70);
                    }
                }
            }
        }
        degree = GetCompassB(_COMPASS_jd_);
        if ( (degree>80)&&(degree<100)&&(degree!=999) )
        {
            g_distanceLeft = GetUltrasound(_ULTRASOUND_left_);
        }
    }
}
#endif