#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 "ultravsound.c"
//#include <util/IncludeAll.h>

// define global var
unsigned int beginDeathHeading = 0;   //全局變量，其存儲上一循環時的角度，也就是剛卡死時的角度；
unsigned long beginDeathTime = 0;   //全局變量，其存儲上一循環時的時間，也就是剛卡死時的時間
unsigned int g_distanceLeft = 0;   //存儲左超聲測距模塊的返回值。

int main(void)
{
    unsigned char fyMax = 0;   //當前復眼通道的最大值。
    unsigned char minValue = 0;   //當前復眼通道的最小值。
    unsigned char i = 0;   //checkDeath()的返回值
    unsigned long t = 0;   //系統時間
    unsigned char yfy = 20;   //近距離復眼的光值，一般是指無球的情況
    unsigned int backDistance = 55;   //過半場的距離，一般是用在來回半場
    unsigned int distance = 55;   //判斷是否過半場。
    unsigned long time = 45;   //系統時間，要過半場所需要的時間。
    unsigned int b = 0;   //指南針的返回值
    unsigned char jGuan = 110;   //近距離的光值。
    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 var0 = 10;
    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(60, 30, 45, yfy, 190);
            }
            else
            {
                findBall(jGuan);
            }
        }
        else
        {
            ultravsound(29, yfy, 60, 30, 7, 17, 31);
        }
    }
    return 1;
}