#ifndef _KICKFOOTBALL_
#define _KICKFOOTBALL_
#include "HardwareInfo.c"
#include <SetSysTime.h>
#include <GetCompassB.h>
#include <GetSysTime.h>
#include <GetCompoI.h>
#include "checkDeath.c"
#include <SetMotor.h>
#include <SetTenthS.h>
#include <GetUltrasound.h>
#include "ballHandle.c"
#include "findBall.c"
//#include <util/IncludeAll.h>

void kickFootball(unsigned char yfy, unsigned char var0, unsigned char jGuan, unsigned char Guan, unsigned int leftDistance, unsigned int rightDistance, unsigned int distance)
{
    // extern global var
    extern unsigned int beginDeathHeading;   //全局變量，其存儲上一循環時的角度，也就是剛卡死時的角度；
    extern unsigned long beginDeathTime;   //全局變量，其存儲上一循環時的時間，也就是剛卡死時的時間
    extern unsigned int g_distanceLeft;   //存儲左超聲測距模塊的返回值。

    unsigned char fyMax = 0;   //當前復眼通道的最大值。
    unsigned char minValue = 0;   //當前復眼通道的最小值。
    unsigned char i = 0;   //checkDeath()的返回值
    unsigned long t = 0;   //系統時間
    unsigned int b = 0;   //指南針的返回值
    unsigned char fy3 = 0;   //當前復眼3號通道
    unsigned char fy4 = 0;   //當前復眼4號通道
    unsigned char fy5 = 0;   //當前復眼5號通道
    unsigned int distance_left = 0;   //當前左超聲測距模塊的返回值
    unsigned int distance_middle = 0;   //當前后超聲測距模塊的返回值
    unsigned char maxIndex = 0;
    SetSysTime(0);
    beginDeathHeading = GetCompassB(_COMPASS_jd_);
    beginDeathTime = GetSysTime();
    while (1)
    {
        fyMax = GetCompoI(_COMPOUNDEYE_fy_, 9);
        minValue = GetCompoI(_COMPOUNDEYE_fy_, 11);
        if ( (fyMax>yfy)||(fyMax-minValue>var0) )
        {
            i = checkDeath(15);
            if ( i )
            {
                beginDeathHeading = GetCompassB(_COMPASS_jd_);
                if ( beginDeathHeading>180 )
                {
                    SetMotor(_MOTOR_left_, 0, 100);
                    SetMotor(_MOTOR_right_, 2, 100);
                    SetTenthS(3);
                }
                else
                {
                    SetMotor(_MOTOR_left_, 2, 100);
                    SetMotor(_MOTOR_right_, 0, 100);
                    SetTenthS(3);
                }
            }
            while ( (fy3>=240||fy4>=240||fy5>=240) )
            {
                fyMax = GetCompoI(_COMPOUNDEYE_fy_, 9);
                maxIndex = GetCompoI(_COMPOUNDEYE_fy_, 8);
                b = GetCompassB(_COMPASS_jd_);
                if ( (b>80)&&(b<100)&&(b!=999) )
                {
                    distance_left = GetUltrasound(_ULTRASOUND_left_);
                    distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                    if ( ((distance_left-g_distanceLeft)>=8)||(distance_middle>=18) )
                    {
                        SetMotor(_MOTOR_left_, 2, 50);
                        SetMotor(_MOTOR_right_, 0, 100);
                        SetTenthS(3);
                    }
                    else
                    {
                        g_distanceLeft=distance_left;
                        break;
                    }
                }
                else
                {
                    if ( (b>260)&&(b<280)&&(b!=999) )
                    {
                        distance_left = GetUltrasound(_ULTRASOUND_left_);
                        distance_middle = GetUltrasound(_ULTRASOUND_middle_);
                        if ( (distance_left>80)&&(distance_middle>18) )
                        {
                            SetMotor(_MOTOR_left_, 0, 100);
                            SetMotor(_MOTOR_right_, 2, 20);
                            SetTenthS(3);
                        }
                        else
                        {
                            break;
                        }
                    }
                    else
                    {
                        break;
                    }
                }
            }
            b = GetCompassB(_COMPASS_jd_);
            fy3 = GetCompoI(_COMPOUNDEYE_fy_, 3);
            fy4 = GetCompoI(_COMPOUNDEYE_fy_, 4);
            fy5 = GetCompoI(_COMPOUNDEYE_fy_, 5);
            if ( ((fy3>=210)||(fy4>=210)||(fy5>=210))&&(b>160)&&(b<200) )
            {
                ballHandle(rightDistance, leftDistance, distance, yfy, Guan);
            }
            else
            {
                findBall(jGuan);
            }
        }
        else
        {
            SetMotor(_MOTOR_left_, 2, 25);
            SetMotor(_MOTOR_right_, 0, 25);
        }
    }
}
#endif