#include <mega128.h>                            .
#include <delay.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

//////// GS 파싱 변수 ////////////

volatile char a_gs[100];
unsigned char c_gs;
int ch;
char lat_gs[11];
char lon_gs[12];
float lat_gs_=0;
float lon_gs_=0;
float upper_ratio,lower_ratio;      // 퍼지제어 시에 사용될 변수
float FL_duty, FR_duty; 

////////////////////////////
/////theta distance 구하기 변수 /////

volatile float theta=0;
volatile float x_now=0;
volatile float y_now=0;
volatile float x_before=0;
volatile float y_before=0;
volatile float x_destination=3735.9945;
volatile float y_destination=12651.8490;
volatile float distance;

////////////////////////
////// GPS 파싱 변수 ///////////
unsigned char c[100];
char data;
int save_on, ii=0;
char time[10];
char lat[11];
char lon[12];
float time_1,time_2,time_3=0;
float lat_=0;
float lon_=0;
///////////////////////////
void init_timer(void)
{
    ASSR  = 0x00;
    TCCR0=0x61;
    TCCR2=0x61;
    TCNT0=0x00;
    TCNT2=0x00;
    TIMSK=0x82;
}
void init_serial(void)
{
  UCSR0A = 0x00;
  UCSR0B = 0x98;
  UCSR0C = 0x06;
  UBRR0L = 0x67;
  UBRR0H = 0x00;
  UCSR1A = 0x00;
  UCSR1B = 0x98;
  UCSR1C = 0x06;
  UBRR1L = 0x67;
  UBRR1H = 0x00;
}
/////////////////////////////
float dutyratio(float data)
{
    float a;
    a=255*data/100;
    return a;
}

////////////////////////////////////////////////////////////////////////
////////////   duty 비가 정해졌을 때 OCR에 적용하기 위한 함수 /////////

void go(void)
{
    float a,b;
    PORTB.1=1;
    PORTB.0=0;
    a=dutyratio(FL_duty);                                  
    b=dutyratio(FR_duty);                            
    OCR0=a;                                                      // OCR0 = 왼쪽 바퀴
    OCR2=b;                          
    printf("OCR0 = %f, OCR2 = %f\r\n",a,b);
}

////////////////////////////////////////////////////////////////////////
////////////////   theta가 주어졌을 때 duty비 설정 /////////////////////

void duty_select(float data)
{
    float low_L_duty,low_R_duty, high_L_duty, high_R_duty;        //       영역별 높은 수준 낮은 수준별 듀티비
    if((data<-135)&&(data>=-180))         // 퍼지 제어 첫번째
    {
        FL_duty=0;
        FR_duty=100;    
        return;                            
    }
    if((data<-90)&&(data>=-135))          // 퍼지 제어 두번째
    {
        low_L_duty=0;
        low_R_duty=100;
        high_L_duty=30;
        high_R_duty=100;    
    }
    if((data<-45)&&(data>=-90))          // 퍼지 제어 세번째
    {
        low_L_duty=30;
        low_R_duty=100;
        high_L_duty=60;
        high_R_duty=100;    
    }
    if((data<0)&&(data>=-45))          // 퍼지 제어 네번째
    {
        low_L_duty=60;
        low_R_duty=100;
        high_L_duty=100;
        high_R_duty=100;   
    }
    if((data<45)&&(data>=0))          // 퍼지 제어 다섯번째
    {
        low_L_duty=100;
        low_R_duty=100;
        high_L_duty=100;
        high_R_duty=60;   
    }
    if((data<90)&&(data>=45))          // 퍼지 제어 여섯번째
    {
        low_L_duty=100;
        low_R_duty=60;
        high_L_duty=100;
        high_R_duty=30; 
    }
    if((data<135)&&(data>=90))          // 퍼지 제어 일곱번째
    {
        low_L_duty=100;
        low_R_duty=30;
        high_L_duty=100;
        high_R_duty=0;    
    }
    if((data<180)&&(data>=135))          // 퍼지 제어 여덟번째
    {
        FL_duty=100;
        FR_duty=0;   
        return; 
    }
    FL_duty=((low_L_duty*lower_ratio)+(high_L_duty*upper_ratio))*0.99;
    FR_duty=(low_R_duty*lower_ratio)+(high_R_duty*upper_ratio);
}

//////////////////////////////////////////////////////////////////////
//////////////////// 각도 받으면 duty_ratio 결정 /////////////////////

float round(float data)     
{
    float a,c,e;
    int d;
    a=data*100;
    c=a+0.5;    
    d=(float)floor(c);
    e=(float)d/100;
    return e;
}
float mod(float data)                  // 45로 나눈 나머지(항상 양수)
{
    int a;
    float b,c,d;
    d=data/45;
    a=(int)d;
    b=d-a;
    if(b<0){b=1+b;}
    c=b*45;
    return c;    
}
float upper(float data)                  // 퍼지제어에서 상승형 삼각파 부분
{
    float y;
    y=data/45;
    printf("upper=%f\r\n",y);
    return y;
}
float lower(float data)                  // 퍼지제어에서 하강형 삼각파 부분
{
    float y;
    y=1-(data/45);
    printf("lower=%f\r\n",y);
    return y;
}
void cal(float data)                     // data에 theta를 넣었을 때 각 삼각파의 해당 비율 계산
{
    float a;
    a=round(mod(data));
    upper_ratio=round(upper(a));
    lower_ratio=round(lower(a));
    duty_select(data);
    go();
    printf("mod : %f, UR : %f, LR : %f\r\n",a,upper_ratio,lower_ratio);    
}
/////////// 거리 결정 /////////////////
void distance_select(void)
{
 float x_distance, y_distance;
 x_distance=round((x_destination-x_now)*1849.8);
 y_distance=round((y_destination-y_now)*1471.8);
 distance=round((float)sqrt(round(x_distance*x_distance)-round(y_distance*y_distance)));
 printf("distance=%f\r\n",distance);
 if((distance<=2)&&(distance>0))
 {
  printf("mission complete\r\n");
 }
 else
 {
  printf("move!! move!!\r\n");
 }
}
////////// theta 결정 ////////////////////
void theta_select(void)
{
 float sine_theta, x1, y1, x2, y2;
 x1=x_destination-x_now;
 y1=y_destination-y_now;
 x2=x_now-x_before;
 y2=y_now-y_before;
 sine_theta=(x1*y2-y1*x2)/sqrt(x1*x1+y1*y1)/sqrt(x2*x2+y2*y2);
 theta=asin(sine_theta)*180/PI;
 printf("theta=%f\r\n",theta);
}
////////////// gps parsing //////////////////
void parsing(void)
{
    int i;
    char t1[2],t2[2],t3[2];
    x_before=x_now;
    y_before=y_now;
    if(!strncmpf(c, "$GPRMC", 6))
    {
        if(c[6]==',')
        {
          if(c[18]==',')
          {
            if(c[31]==',')
            {
              for(i=0;i<10;i++)
              {
                time[i]=c[7+i];
              }                
              time[9]=NULL;
              for(i=0;i<11;i++)        
              {
                lat[i]=c[19+i];
              }                 
              lat[10]=NULL;
              for(i=0;i<12;i++)        
              {
                lon[i]=c[32+i];
              }    
              lon[11]=NULL;                   
              t1[0]=time[0];   
              t1[1]=time[1];
              t2[0]=time[2];
              t2[1]=time[3];
              t3[0]=time[4];
              t3[1]=time[5];
              time_1=atoi(t1);
              time_2=atoi(t2);
              time_3=atoi(t3);
              lat_=(float)atof(lat);
              lon_=(float)atof(lon);
              x_now=lat_;
              y_now=lon_; 
              printf("xnow : %f\r\n",x_now);
              printf("ynow : %f\r\n",y_now);                   
              printf("%d:%d:%d\r\n",time_1,time_2,time_3);
              printf("La=%f\r\n",lat_);    
              printf("Lo=%f\r\n",lon_); 
              theta_select();
              distance_select();
              cal(theta);
            }
            else
            {
              printf("~Loading~\r\n");
            }
          }
        }
     }  
}
///////// GS parsing //////////////
void parsing_gs(void)
{
    int i;
    printf("%s\r\n",a_gs);
    for(i=0;i<9;i++)        
    {
        lat_gs[i]=a_gs[1+i];
    }                 
    lat_gs[10]=NULL;
    for(i=0;i<10;i++)        
    {
        lon_gs[i]=a_gs[11+i];
    }    
    lon_gs[11]=NULL;                  
    printf("latitude : %s\r\n",lat_gs);
    printf("longitude : %s\r\n",lon_gs);   
    lat_gs_=(float)atof(lat_gs);
    lon_gs_=(float)atof(lon_gs);
    x_destination=lat_gs_;
    y_destination=lon_gs_;
    printf("La=%f\r\n",lat_gs_);    
    printf("Lo=%f\r\n",lon_gs_); 
}  
////////////////////////////////////////

void main(void)
{

 init_serial();
 init_timer();
 SREG = 0x80;
 PORTB=0x90;
 DDRB=0x9f;

}

interrupt [USART1_RXC] void uart(void)
{
    
    while(!(UCSR1A & 0x80));    
    data = UDR1;
        
    if ( data == '$')
    {
        save_on = 1;
    }    
    else if ( data == 0x0a )   
    {
        c[ii] = NULL;        
        save_on = 0;
        parsing();
                ii = 0;
        return ;
    }    
    if ( save_on == 1 )
    {
        c[ii++] = data;
    }       
    UCSR1A |=0x80;
}

 interrupt [USART0_RXC] void uat(void)
{  
  c_gs=UDR0;
  if(c_gs==')')
  { 
  a_gs[ch]=NULL;
  parsing_gs();
  theta_select();
   distance_select();
   cal(theta);
  ch=0;
  }
  else
  {
  a_gs[ch]=c_gs;
  ch++;
  }  
}


interrupt [TIM0_COMP] void time0_comp(void)
{
    #asm("nop");
}

interrupt [TIM2_COMP] void time2_comp(void)
{
    #asm("nop");
}