// ECE476 Project Base unit with RF transmitter and ultrasonic receiver
// 4/5/2006
//
// connect RF transmitter to port D.1 (TX)
// connect Ultrasonic transducer to Op-Amp and output of Op-Amp to INT0 (PD2)
// connect LCD to Port C

// Includes
#include <Mega32.h>
#include <stdio.h>
#include <string.h>	// for string functions
#include <math.h>
#include <delay.h>

#define DISPLAY_USEPORTC	// specify which port to use for LCD
#define USEDISPLAY

#ifdef USEDISPLAY
    #ifdef DISPLAY_USEPORTA
    #asm(".equ __lcd_port=0x1B")
    #elif defined DISPLAY_USEPORTB
    #asm(".equ __lcd_port=0x18")
    #elif defined DISPLAY_USEPORTC
    #asm(".equ __lcd_port=0x15")
    #elif defined DISPLAY_USEPORTD
    #asm(".equ __lcd_port=0x12")
    #endif
    #include <lcd.h> // lcd library
#endif

// only compile if using PORTA
#ifdef KEYPAD_USEPORTA
  #define KEYPAD_PORT PORTA
  #define KEYPAD_PIN PINA
  #define KEYPAD_DDR DDRA

// only compile if using PORTB
#elif defined KEYPAD_USEPORTB
  #define KEYPAD_PORT PORTB
  #define KEYPAD_PIN PINB
  #define KEYPAD_DDR DDRB

// only compile if using PORTC
#elif defined KEYPAD_USEPORTC
  #define KEYPAD_PORT PORTC
  #define KEYPAD_PIN PINC
  #define KEYPAD_DDR DDRC

// only compile if using PORTD
#elif defined KEYPAD_USEPORTD
  #define KEYPAD_PORT PORTD
  #define KEYPAD_PIN PIND
  #define KEYPAD_DDR DDRD
#endif

//I like these definitions
#define begin {
#define end   } 

#define sync_char 0b10101010   //170
#define start_char 0b11001010  //202
#define end_char 0b11010100    //212
#define SOUNDSPEED 0.0334       // cm per us
#define PI         3.1415926535

//the encoding
flash char code[16] = {0b10001011,0b10001101,0b10010011,0b10010101,0b10010110,
0b10011001,0b10011010,0b10011100,0b10100011,0b10100101,0b10100110,0b10101001,
0b10101100,0b10110001,0b10110010,0b10110100};

// constants
#define t3		 800    // query every 1 sec
#define t3_ovf		 300     // query refresh when no response
#define CMDBUFFERLEN	 10      // length of command and arg string
#define TXBUFSIZE	 30      // transmit string buffer size
#define RXBUFSIZE	 15	 // receive string buffer size
#define NAVERAGE         2       // number of data points to computer running average
#define TIMEOUT          25      // number of 4 ms elapsed before it is considered timeout
#define TIMEOFFSET       3340    // number of microseconds between RF TX and US RX
#define A     0                  // receiver id
#define B     1
#define C     2
#define REC_COUNT        3
#define TIMEOUT_TH       12
#define ServoP           25      // number of ms between each pulse
#define CALIBRATETIME    100      // number of cycles to do calibration
#define CALIBRATEREADY_TH 3      // number of consecutive stable offset calculated for offset to be good
#define TURN              100

// declare functions
void puts_int(void);				// initialize transmit to PC
void gets_int(void);				// initialize receiver from PC

unsigned char lcd_buffer[16];

typedef struct {
  float x;
  float y;
} Pos;

typedef struct {
  float         data[NAVERAGE];
  unsigned long timestamp[NAVERAGE];
  unsigned char idx;
  float         dist;
  unsigned int  timeout;
  Pos           pos;
  unsigned int  offset;
  unsigned char offsetReady;
} Receiver;

Receiver rec[REC_COUNT];
Pos      target, targetCal;

// misc variables
unsigned char i, j;
unsigned int time3;	// count down time in ms
unsigned long gTime;	// counter for request id
unsigned char state;    // state variable
float sum;              // temporary sum of distances
unsigned char count;    // temporary number of valid distances
unsigned char overflow; // indicate the time has overflowed
unsigned char cal;      // indicate whether calibration is ready
unsigned char m_count;   // counter for motor duty cycles
unsigned char turn;      // number of ms between update to servo angle
unsigned char OCR2t;     // target OCR2

//RXC ISR variables
unsigned char r_index;		//current string index
unsigned char r_buffer[RXBUFSIZE];	//input string
unsigned char r_ready;		//flag for receive done
unsigned char r_char;		//current character
unsigned char r_started;   // packet started

//TX empth ISR variables
unsigned short int t_index;			//current string index
unsigned char t_buffer[TXBUFSIZE];	//output string
unsigned char t_ready;			//flag for transmit done
unsigned char t_char;			//current character

// distance variables
float timeTotal, tempOffset;
float distance;
float sumx, sumy, x, y;
unsigned int elapsed;
float d1sq, d2sq, d3, d, angle;

//****************************************************************
// Interrupt Service Routines
//****************************************************************

//========================================================
// INT0: External Intrrupt 0
// enters when sound wave received
//========================================================

interrupt [EXT_INT0] void int0(void)
begin
  // turn off timer 1 counter
  TCCR1B = 0;
  // turn off external interrupt 0
  GICR = 0b10111111 & GICR;
end

//========================================================
// INT0: External Intrrupt 1
// enters when sound wave received
//========================================================

interrupt [EXT_INT1] void int1(void)
begin
  // turn off timer 1 counter
  TCCR1B = 0;
  // turn off external interrupt 1
  GICR = 0b01111111 & GICR;
end

//========================================================
// INT0: External Intrrupt 2
// enters when sound wave received
//========================================================

interrupt [EXT_INT2] void int2(void)
begin
  // turn off timer 1 counter
  TCCR1B = 0;
  // turn off external interrupt 2
  GICR = 0b11011111 & GICR;
end

//========================================================
// Timer 1: Overflow Intrrupt
//========================================================

interrupt [TIM1_OVF] void timer1_ovf(void)
begin
  elapsed++;
  if (elapsed == TIMEOUT)
  begin
    GICR = GICR & 0b00011111; // overflowed, turn off external interrupt
    overflow = 1;
    time3 = t3_ovf;
  end
end

//========================================================
// TIMER 0: Compare match ISR
//========================================================

interrupt [TIM0_COMP] void timer0_comp(void)
begin
  if (time3>0) --time3;
  if (m_count>0) --m_count;
  if (turn>0) --turn;
end

//========================================================
// TIMER 2: Compare match ISR
//========================================================

interrupt [TIM2_COMP] void timer2_comp(void)
begin
  TCCR2 = 0;
  TCNT2 = 0;
end

//========================================================
// UART character-ready ISR
//========================================================
interrupt [USART_RXC] void uart_rec(void)
begin
    //-------------------------------------------------------
    // check for sync byte
    if(r_index==0)
    begin
      r_buffer[r_index]=UDR;
      if(r_buffer[0]==sync_char) r_started=1;
      else r_started=0;                      
      if(r_buffer[0]==sync_char & r_started)
      begin
        r_index++;
      end				
	  end
	  //-------------------------------------------------------
	  // check for start byte
    else if (r_index==1)
	  begin
     	r_buffer[r_index]=UDR;
	    if(r_buffer[0]==sync_char && r_buffer[1]==start_char)
		  begin
        r_index++;
      end
    end
    //-------------------------------------------------------
    // rest of the packet
    else
    begin
	 r_buffer[r_index]=UDR;
//		  if (r_index==4)
//      begin
//	      rx_length = decode(rx_data[3],rx_data[4]);			
//		  end		
//      else if(r_index>=max_rx_length || rx_data[r_index]==212)
//	    begin
      if(UDR == end_char || r_index> 8)
      begin
        r_ready=1;			//signal cmd processor
        UCSRB.7=0;			//stop rec ISR
      end
	    r_index++;
    end
end

//========================================================
// UART xmit-empty ISR
//========================================================
interrupt [USART_DRE] void uart_send(void)
begin
  t_char = t_buffer[++t_index];
  if (t_char == 0) 	
  begin
    UCSRB.5=0; //kill isr 
    t_ready=1; //transmit done
  end
  else UDR = t_char;	//send the char 
end

//****************************************************************
// -- non-blocking keyboard check initializes ISR-driven
// receive. This routine merely sets up the ISR, which then
// does all the work of getting a command.
//****************************************************************
void gets_int(void)
begin
  r_ready=0;
  r_started=0;
  r_index=0;
  UCSRB.7=1;
end

//****************************************************************
//  -- nonblocking print: initializes ISR-driven
// transmit. This routine merely sets up the ISR, then
//send one character, The ISR does all the work.
//****************************************************************   
void puts_int(void) 
begin
  t_ready=0;
  t_index=0;
  if (t_buffer[0]>0)
  begin
    putchar(t_buffer[0]);
    UCSRB.5=1;
  end
end

//****************************************************************
// MAIN
//****************************************************************
void main(void)
begin
  //========================================================
  // Initialization
  //========================================================

  //serial setop for debugging using printf, etc.     
  UCSRB = 0x18 ;
//  UCSRC = 0xA6;          // set even parity
//  UBRRL = 103 ;		// sets baud rate to 9600
  UBRRL = 249 ;		// sets baud rate to 4000
//  UBRRL = 416 ;		    // sets baud rate to 2400
//  UBRRL = 207 ;		    // sets baud rate to 4800

  // set int0 and int1 to falling edge trigger
  MCUCR = 0b00001010;
//  MCUCR = MCUCR & 0b11111010;
//  MCUCR = MCUCR | 0b00001010;
//  MCUCR.3 = 1
//  MCUCR.2 = 0
//  MCUCR.1 = 1
//  MCUCR.0 = 0
//  MCUCSR = MCUCSR & 0b10111111;     // set int2 to falling edge trigger
  MCUCSR = MCUCSR | 0b01000000;     // set int2 to rising edge trigger
//  MCUCSR.6 = 1

  #ifdef USEDISPLAY
    lcd_init(8); // initialize LCD
    
    // initial LCD message
    lcd_clear();
    lcd_gotoxy(0,0);
    lcd_putsf("All Your");
    lcd_gotoxy(0,1);
    lcd_putsf(" Base");
  #endif

  // set I/O ports
  DDRD = 0x82;      // port D is used as TX, LED, and external interrupts
  PORTD = 0;

  time3=t3;

  timeTotal = 0;
  distance = 0;
  elapsed = 0;
  overflow = 0;
  state = A;

  // define receiver position
  rec[A].pos.x = 0;
  rec[A].pos.y = 6.5;
  rec[B].pos.x = 18.7;
  rec[B].pos.y = 6.5;
  rec[C].pos.x = -20.3;
  rec[C].pos.y = 6.5;
  target.x = 0;
  target.y = 0;
  targetCal.x = 0;
  targetCal.y = 60.3;

  r_ready = 0;
  t_ready = 1;
  
  cal = 0;          // calibration is not ready

  // set timer0
  OCR0 = 249;		// makes a 1 ms interrupt
  TIMSK = 134;
  //  TIMSK.1 = 1;        // turn on timer 0 compare match ISR
  //  TIMSK.2 = 1;        // turn on timer 1 overflow ISR
  //  TIMSK.7 = 1;        // turn on timer 2 compare match ISR
  TCCR0 = 0b00001011;	  // prescalar of 64 and clear on match
  TCCR1A = 0;             // timer 1 normal mode
//  TCCR1B = 0b00000001;    // timer 1 normal mode with prescalar 1

//  GICR = GICR | 0b01000000;
  //  GICR.7 = 0;   // enable int 1
  //  GICR.6 = 0;   // enable int 0
  //  GICR.5 = 0;   // enable int 2
  // OCR2 40 is 180 degree
  // OCR2 145 is 0 degree
  // OCR2 92 is middle
  OCR2 = 92;
  OCR2t = 92;


  #asm
  sei
  #endasm
  
  // MAIN loop
  while(1) 
  begin
    // send pulse to servo motor
    if (m_count == 0)
    begin
      m_count = ServoP;
      TCCR2 = 0b01111110;    // 244 Hz fast PWM (4.096 ms period), set OC2 when compare match
    end
    
    if (turn == 0)
    begin
      turn = TURN;
      if(OCR2 < OCR2t)
        OCR2++;
      if(OCR2 > OCR2t)
        OCR2--;    
    end
  
    // query remote unit
    if (time3 == 0)
    begin
      time3 = t3;
      // turn off timer 0
      //TCCR0 = 0b00001000;

    
      // reset timer 1
      TCNT1H = 0;
      TCNT1L = 0;
      elapsed = 0;

      while(!t_ready);

      // build query message
      t_buffer[0] = sync_char;
      t_buffer[1] = sync_char;
      t_buffer[2] = sync_char;
      t_buffer[3] = sync_char;
      t_buffer[4] = sync_char;  // sync
      t_buffer[5] = start_char;
      t_buffer[6] = code[0];
      t_buffer[7] = code[15];
      t_buffer[8] = end_char;
      puts_int();

      GIFR = 0b11100000; // clear external interrupt flag
      
      while(!t_ready);
      // turn on timer 1 counter
      TCCR1B = 1;
    
      // query one of three receivers
      switch(state)
      begin
          
        // receiver A
        case A:
        begin
          // turn on external interrupt 0
          GICR = 0b01000000;      // GICR.6 = 1;
          while((GICR & 0b01000000) != 0)
          begin
            // sleep and wait for external interrupt to trigger as long as external interrupt is still on
            #asm("sleep");
          end
        end
        break;
                    
        // receiver B
        case B:
        begin
          // turn on external interrupt 1
          GICR = 0b10000000;
          // GICR.7 = 1;
          while((GICR & 0b10000000) != 0)
          begin
            // sleep and wait for external interrupt to trigger as long as external interrupt is still on
             #asm("sleep");
          end
        end
        break;
          
        // receiver C
        case C:
        begin
          // turn on external interrupt 2
          GICR = 0b00100000;
          // GICR.5 = 1;
          while((GICR & 0b00100000) != 0)
          begin
            // sleep and wait for external interrupt to trigger as long as external interrupt is still on
            #asm("sleep");
          end
        end
        break;
      end

      TCCR1B = 0; // turn off timer 1 in case it didn't get turned off

      if(overflow)
      begin
        rec[state].timeout++;
        if(rec[state].timeout == 0) rec[state].timeout--; // make sure it doesn't overflow to 0
        overflow = 0;
      end
      else
      begin
        // calculate total time in us
        timeTotal = ((float)elapsed * 4096) + ((float)TCNT1 * 0.0625);
        if(timeTotal > 0)
        begin
          if(cal < CALIBRATETIME)
          begin
            // if offset is not ready (not yet reached stable enough reading)
            if(rec[state].offsetReady < CALIBRATEREADY_TH)
            begin
              distance = sqrt((targetCal.x - rec[state].pos.x)*(targetCal.x - rec[state].pos.x) + (targetCal.y - rec[state].pos.y)*(targetCal.y - rec[state].pos.y));
              tempOffset = timeTotal - distance/SOUNDSPEED;
              // check if offset calculation is stable (within 5 %)
              if(tempOffset < 0)
              begin
                rec[state].offset = 0;
                rec[state].offsetReady = 0;
              end
              else if(fabs(rec[state].offset - tempOffset)/rec[state].offset > 0.05)
              begin
                rec[state].offset = tempOffset;
                rec[state].offsetReady = 0;
              end
              else
              begin
                // average the 2
                rec[state].offset = (rec[state].offset + tempOffset) / 2;
                rec[state].offsetReady++;
              end
            end
            else
            begin
              // this receiver already done calibration, skip to next one immediately
              time3 = t3_ovf;
            end
            #ifdef USEDISPLAY
              lcd_clear();
              lcd_gotoxy(0,0);
              sprintf(lcd_buffer,"%d:%d",state,rec[state].offset);
              lcd_puts(lcd_buffer);
            #endif
            cal++;
            // if all are calibrated, skip ahead
            if(rec[A].offsetReady == CALIBRATEREADY_TH && rec[B].offsetReady == CALIBRATEREADY_TH && rec[C].offsetReady == CALIBRATEREADY_TH)
            cal = CALIBRATETIME;
            #ifdef USEDISPLAY
              lcd_gotoxy(0,1);
              sprintf(lcd_buffer,"(%d)#:%d",rec[state].offsetReady,cal);
              lcd_puts(lcd_buffer);
            #endif            
          end
          else
          begin
            // measuring distance
            timeTotal -= TIMEOFFSET;
            // calculate distance
            distance = timeTotal * SOUNDSPEED;
            #ifdef USEDISPLAY
              lcd_clear();
              lcd_gotoxy(0,0);
              sprintf(lcd_buffer,"t=%3.1f",timeTotal);
              lcd_puts(lcd_buffer);
            #endif
            // store data into averaging array
            rec[state].data[rec[state].idx] = distance;
            rec[state].timestamp[rec[state].idx] = gTime;
            rec[state].idx++;
            if(rec[state].idx == NAVERAGE-1) rec[state].idx = 0;
            rec[state].timeout = 0;
          end
        end
      end

      TCCR0 = 0b00001011;	  // turn on timer 0

      if(rec[state].timeout == 0)
      begin
        // new data point added
        // average up the data points within some nearby time
        sum = 0;
        count = 0;
        for(i=0;i<NAVERAGE;i++)
        begin
          if(gTime - rec[state].timestamp[i] < TIMEOUT_TH)
          begin
            if(rec[state].data[i] > 0)
            begin
              sum += rec[state].data[i];
              count++;
            end
          end
        end
        if(count > 0)
        begin
          rec[state].dist = sum / count;
        end
        else
        begin
          rec[state].dist = 0;
        end
      end

      count = 0;
      sumx = 0;
      sumy = 0;
      // calculate position of remote unit
      for(i = 0; i < REC_COUNT; i++)
      begin
        for(j = i+1; j < REC_COUNT; j++)
        begin
          if(rec[i].dist != 0 && rec[j].dist != 0)
          begin
            // assume y1 == y2, d = x2 - x1
            // x = (d1^2 - d2^2 + 2*x1*d + d^2)/(2*d)
            // y = y1 + (d1^2 - (x1-x)^2 ) ^ 0.5
            d = rec[j].pos.x-rec[i].pos.x;
            d1sq = (rec[i].dist)*(rec[i].dist);
            d2sq = (rec[j].dist)*(rec[j].dist);
            d3 = 2*rec[i].pos.x*(rec[j].pos.x-rec[i].pos.x);
            x = (rec[i].dist)*(rec[i].dist) - (rec[j].dist)*(rec[j].dist) + 2*rec[i].pos.x*(rec[j].pos.x-rec[i].pos.x) + (rec[j].pos.x-rec[i].pos.x)*(rec[j].pos.x-rec[i].pos.x);
            x = x / (2*(rec[j].pos.x-rec[i].pos.x));
            y = (rec[i].dist)*(rec[i].dist) - (rec[i].pos.x - x)*(rec[i].pos.x - x);
            if(y > 0)
            begin
              y = rec[i].pos.y + sqrt(y);
              sumx += x;
              sumy += y;
              count++;
            end
/*            else
            begin
            #ifdef USEDISPLAY
              lcd_clear();
              lcd_gotoxy(0,0);
              sprintf(lcd_buffer,"d1%2.0f",rec[i].dist);
              lcd_puts(lcd_buffer);
              lcd_gotoxy(0,1);
              sprintf(lcd_buffer,"d2%2.0f",rec[j].dist);
              lcd_puts(lcd_buffer);
              delay_ms(5000);

              lcd_clear();
              lcd_gotoxy(0,0);
              sprintf(lcd_buffer,"d1sq%2.0f",d1sq);
              lcd_puts(lcd_buffer);
              lcd_gotoxy(0,1);
              sprintf(lcd_buffer,"d2sq%2.0f",d2sq);
              lcd_puts(lcd_buffer);
              delay_ms(5000);

              lcd_clear();
              lcd_gotoxy(0,0);
              sprintf(lcd_buffer,"d3%2.0f",d3);
              lcd_puts(lcd_buffer);
              lcd_gotoxy(0,1);
              sprintf(lcd_buffer,"d%2.0f",d);
              lcd_puts(lcd_buffer);
              delay_ms(5000);

              lcd_clear();
              lcd_gotoxy(0,0);
              sprintf(lcd_buffer,"x=%2.0f",x);
              lcd_puts(lcd_buffer);
              lcd_gotoxy(0,1);
              sprintf(lcd_buffer,"p%2.0f",rec[i].pos.x);
              lcd_puts(lcd_buffer);
              delay_ms(5000);
              
            #endif
            end*/
          end
        end
      end
      if(count > 0)
      begin
        target.x = sumx / count;
        target.y = sumy / count;
        
        // calculate servo position
        angle = atan2(target.y,target.x)/PI*180;
        OCR2t = 40 + (unsigned char)((PI-atan2(target.y,target.x))/PI*105);
      end

      if(cal >= CALIBRATETIME)
      begin
        #ifdef USEDISPLAY
          lcd_clear();
          lcd_gotoxy(0,0);
          if(target.x < 100 && target.y < 100)
          begin
            sprintf(lcd_buffer,"x%2.0f y%2.0f",target.x,target.y);
            lcd_puts(lcd_buffer);
          end
          lcd_gotoxy(0,1);
          sprintf(lcd_buffer,"%d=%3.1fcm",state,rec[state].dist);
          lcd_puts(lcd_buffer);
//          delay_ms(1000);
          lcd_clear();
          lcd_gotoxy(0,0);
          if(target.x < 100 && target.y < 100)
          begin
            sprintf(lcd_buffer,"theta %2.0f",angle);
            lcd_puts(lcd_buffer);
          end
//          delay_ms(1000);          
        #endif
      end
      
      // enumerate to next receiver
      if(++state == REC_COUNT) state = 0;
      if(cal < CALIBRATETIME)
      begin
        while(rec[state].offsetReady == CALIBRATEREADY_TH)
        begin
          if(++state == REC_COUNT) state = 0;
        end
      end
      gTime++;  // increment global time

    end // end time3 == 0
  end // end while 
end  //end main