#include <iom16v.h>
#include <macros.h>
//#include "lcd.h"
#include "delay.h"

#define uchar unsigned char
#define uint  unsigned int

////////////////////////////////////////////////////////////////////
//部分函數聲明
void time0_start( void );
void time0_stop( void );
///////////////////////////////////////////////////////////////////

///////////////////////////////////////L298驅動/////////////////////////////////////
void L298_port_init( void )
{
    DDRC |= 0b11111100;
}

void go_ahead( void )
{
 	PORTC &=~ 0B11111100;
    PORTC |= 0b11010100;
}

void go_back( void )
{
 	PORTC &=~ 0B11111100;
    PORTC |= 0b11101000;
}

void turn_l( void )
{
 	PORTC &=~ 0B11111100;
    PORTC |= 0b11011000;
}

void turn_r( void )
{
 	PORTC &=~ 0B11111100;
    PORTC |= 0b11100100;
}

void car_stop( void )
{
 	 PORTC &=~ 0B11111100;
}
 	 
///////////////////////////////////////L298驅動部分結束/////////////////////////////////


///////////////////////////////////////小車尋光部分/////////////////////////////////////
#define Vref  2.56                //定義基準電壓值
//#define Vref  1.00

//通道選擇宏定義
#define AD_mux(n)  { ADMUX = ( 0xC0 | n ); } //內部基準 0xC0,        

//輸入通道端口配置
#define ADC0_in  { DDRA &= 0XFE; PORTA |= 0X01; } //輸入，高阻態
#define ADC1_in  { DDRA &= 0XFD; PORTA |= 0X02; }
#define ADC2_in  { DDRA &= 0XFB; PORTA |= 0X04; }
#define ADC3_in  { DDRA &= 0XF7; PORTA |= 0X08; }
#define ADC4_in  { DDRA &= 0XEF; PORTA |= 0X10; }
//#define ADC5_in  { DDRA &= 0XDF; PORTA |= 0X20; }
//#define ADC6_in  { DDRA &= 0XBF; PORTA |= 0X40; }
//#define ADC7_in  { DDRA &= 0X7F; PORTA |= 0X80; }

//全局變量
uint small_flag = 0;   //取最小光敏代號存入該變量

uint adcl = 0;         //定義暫存低字節
uint adch = 0;         //高字節
uint adcvalue = 0;     //低+高
uchar ad_m = 0;        //初始化通道為0通道

float ad0_tmp = 0.0;   //轉換完成后暫存值
float ad1_tmp = 0.0;
float ad2_tmp = 0.0;
float ad3_tmp = 0.0;
float ad4_tmp = 0.0; 

//取三個數中的最小值
char get_5small( float a, float b, float c, float d, float e ) //ad0_tmp, ad1_tmp, ad2_tmp, ad3_tmp, ad4_tmp
{
    float small_tmp = 0.0;
    float k = 0.0;
    
    k  = ( ( a < b ? a : b ) < c ) ? ( a < b ? a : b ) : c;
    small_tmp  = ( ( k < d ? k : d ) < e ) ? ( k < d ? k : d ) : e;
    
    if( small_tmp == a ) return 0;
	else if( small_tmp == b ) return 1;
	else if( small_tmp == c ) return 2;
	else if( small_tmp == d ) return 3;
	else if( small_tmp == e ) return 4;
	else return 1;
}

//完成相應動作
void car_xunguang( uchar s_flag )
{
 	 switch( s_flag )
	 {
	 case 0: turn_l();   break;
	 case 1: go_ahead(); break;
	 case 2: turn_r();   break;
	 case 3: turn_l();   break;
	 case 4: turn_r();   break;	                 
	 }
}	  	

//使用ADC
void ADC_start( void )
{
    ADCSRA |= ( 1 << ADEN ) | ( 1 << ADIE ) | ( 1 << ADSC ) ;  //ADC使能位, 中斷位置位
}

//禁用ADC
void ADC_stop( void )
{
    ADCSRA &=~ ( 1 << ADEN ) | ( 1 << ADIE ) | ( 1 << ADSC ) ; //ADC使能位, 中斷位清零
}

//ADC采樣端口初始化
void ADC_port_init( void )
{
    ADC0_in;     //定義PAD0為輸入，高阻態
	ADC1_in;
	ADC2_in;
	ADC3_in;
	ADC4_in;
}

//ADC寄存器初始化
void ADC_REG_init( void )
{
    ADCSRA = 0X00;         //ADC控制器清零，不啟動ADC轉換
	AD_mux( ad_m );        //選擇內部電壓,初始時使用ADC0通道做為單通道輸入
    SFIOR = 0X00;          //定義為連續自由轉換
    ADCSRA = 0B11101101;   //啟動ADC轉換1110 1101, 開啟ADC中斷允許, 32分頻(比較合適）
}

//ADC采樣, 不利用中斷完成的采樣
float ADC_convert( void )
{
    while( !ADIF );        					 //等待ADC轉換完成 ,ADC轉換完成后，ADIF = 1
    adcl = ( uint )ADCL;   					 //定義的為右對齊， 取ADCL的值
    adch = ( uint )ADCH;   					 //取ADCH的值
    adcvalue = adcl + ( adch << 8 );         //取10位ADC的值
    return adcvalue * Vref / 1024;           //將ADC的值比較Vref轉化為浮點數
}

//ADC值轉換完成初始化
#pragma interrupt_handler adc_isr:iv_ADC
void adc_isr(void)
{
 //conversion complete, read value (int) using...
 // value=ADCL;            //Read 8 low bits first (important)
 // value|=(int)ADCH << 8; //read 2 high bits and shift into top byte
 	 ADCSRA &=~ ( 1 << ADIE ) | ( 1 << ADIF );       //禁止轉換中斷
	 
	 switch( ad_m )
 	 {
  	  	  case 0:
	      {
			   ad0_tmp = ADC_convert();
			   ad_m = 1;		//轉換通道	
		  }break;

		  case 1:
	 	  {
			  ad1_tmp = ADC_convert();
			  ad_m = 2;         //轉換通道
		  }break;
		
		  case 2:
	 	  {
			  ad2_tmp = ADC_convert();
			  ad_m = 3;         //轉換通道			  			
		  }break;
		  
		  case 3:
	      {
			   ad3_tmp = ADC_convert();
			   ad_m = 4;		//轉換通道	
		  }break;

		  case 4:
	 	  {
			  ad4_tmp = ADC_convert();
			  ad_m = 0;         //轉換通道
			  
			  			
	          //顯示三個AD光度采樣的值		
			  //LCD_write_datafloat( 2, 1, ad0_tmp, 100, 2);
			  //LCD_write_datafloat( 2, 6, ad1_tmp, 100, 2);
			  //LCD_write_datafloat( 2, 11, ad2_tmp, 100, 2);
			 small_flag = get_5small( ad0_tmp, ad1_tmp, ad2_tmp, ad3_tmp, ad4_tmp );   //獲取三點中最亮的一點
         	 car_xunguang( small_flag );
			 	
		  }break;
		
     }

	 AD_mux( ad_m );                          //重新啟動ADC， 重新選擇基準電壓和輸入通道.此算法適用于多通道.
	 ADCSRA |= ( 1 << ADSC ) | ( 1 << ADIE ); //開中斷, 重新啟動
	 delay_nms( 1 );                          //一定要這個延時, 否則轉換通道會出錯
	 
	 if( ad_m == 0 ) ADC_stop();
	 delay_nms( 2 );
}
//////////////////////////////尋光部分結束///////////////////////////////


///////////////////////////////避障部分///////////////////////////////
//定義全局變量
uchar model = 0;
uchar model_a = 0;
uchar flag_zhangai;
long int count_1 = 0;
long int count_2 = 0;
long int count_3 = 0;

//time0 allow
void time_off( void )
{
 	 TCCR0 &=~ ( 1 << OCIE0 );
}

//time0 ON
void time_on( void )
{
 	 TCCR0 |= ( 1 << OCIE0 );
}

//TIMER0 initialize - prescale:256
// WGM: CTC
// desired value: 4mSec
// actual value:  4.032mSec (-0.8%)
void timer0_init(void)
{
 TCCR0 = 0x00; //stop
 TCNT0 = 0x83; //set count
 OCR0  = 0x7D;  //set compare
 TCCR0 = 0x0C; //start timer
}

//掃描端口初始化
void hw_port_init( void )
{
    DDRC &=~ ( 1 << PC0 ) | ( 1 << PC1 );     //定義PC0, PC1口為上拉
    PORTC |= ( 1 << PC0 ) | ( 1 << PC1 );
}

//紅外返回值
uchar int_scan( void )   //判斷障礙，并執行相應動作
{
    switch( PINC & 0b00000011 )
    {
        case 0b00000000: model = 1; return 1; break;     //左右同時遇到障礙
        case 0b00000010: model = 2; return 2; break;     //左遇到障礙
        case 0b00000001: model = 3; return 3; break;     //右遇到障礙
        default : model = 0; return 0; break;            //沒有遇到障礙
    }
}


#pragma interrupt_handler timer0_comp_isr:iv_TIM0_COMP
void timer0_comp_isr(void)        // CTC模式， 比較匹配中斷服務程序
{	
	//CLI();
	//time_off();			  
	
	if( model == 0 )              //如果沒有障礙返回值，剛繼續前一動作
	{
	    if( flag_zhangai == 1 ) model = model_a;
	    else ADC_start();
	}
	
	if( model == 1 )         //左方右方有障礙, 即是前方有障礙
	{
	    model_a = model;
	    flag_zhangai = 1;
	    turn_r();
	    if( int_scan() == 0 )
	    {
            if( ++count_1 >= 80 )
            {
                go_ahead();
                if( ++count_1 >= 200 )
                {
                    turn_l();
                    if( ++count_1 >= 400 )
                    {
                        model = 0;
                        count_1 = 0;
                        flag_zhangai = 0;
                    }
                }
            }	
        }
    }

    if( model == 2 )              //左邊有障礙
    {
		model_a = model;
	    flag_zhangai = 1;
	    turn_r();
	    if( int_scan() == 0 )
	    {
            if( ++count_2 >= 80 )
            {
                go_ahead();
                if( ++count_2 >= 200 )
                {
                    turn_l();
                    if( ++count_2 >= 400 )
                    {
                        model = 0;
                        count_2 = 0;
                        flag_zhangai = 0;
                    }
                }
            }	
        }
    }

    if( model == 3 )                 //右邊有障礙
    {
		model_a = model;
	    flag_zhangai = 1;
	    turn_l();
	    if( int_scan() == 0 )
	    {
            if( ++count_3 >= 80 )
            {
                go_ahead();
                if( ++count_3 >= 200 )
                {
                    turn_r();
                    if( ++count_3 >= 400 )
                    {
                        model = 0;
                        count_3 = 0;
                        flag_zhangai = 0;
                    }
                }
            }	
        }
    }
	
	//time_on();
	
	//SEI();		   	        	
}


//***************避障部分結束*******************

//call this routine to initialize all peripherals
void init_devices(void)
{
 //stop errant interrupts until set up
 CLI(); //disable all interrupts
 
 //LCD_init();
 L298_port_init();       //L298端口初始化
 hw_port_init();         //紅外感應端口初始化
 timer0_init();          //定時器0初始化
 
 ADC_REG_init();
 ADC_port_init();
 
 DDRD &=~ ( 1 << PD2 );   //int0中斷初始化 用于充電
 PORTD |= ( 1 << PD2 );

 MCUCR = 0x00;
 GICR  = 0x40;            //INT0開啟
 TIMSK = 0x02; //timer interrupt sources
 //LCD_write_string( 1, 2, "car test...." );
 SEI(); //re-enable interrupts
 //all peripherals are now initialized
}
////////////////////////////////////////////
//取電中斷
#pragma interrupt_handler int0_isr:iv_INT0
void int0_isr(void)
{
 //external interupt on INT0
 CLI();
 car_stop();
 delay_ns( 20 );
 go_back();
 delay_ns( 10 );
 SEI();
}

void main( void )
{
    init_devices();
	
    while( 1 )
    {
        model = int_scan();
    }
}