#include <Mega32.h>
#include <stdio.h> //for debugging using printf, etc
#include <stdlib.h>
#include <math.h>

//timeout values for each task
#define prescale0 3
#define t1 1
#define kgC 1770
#define koC 29
#define LCDwidth 16 //characters

// State variables used by sensor for obstacle detection

#define RESET 0
#define INIT 1
#define MEAS 2
#define WAIT 3

// State variables used by navigation AI
#define NOMOVE 0
#define FORWARD 1
#define RIGHT 2
#define LEFT 3
#define BACKWARD 4

// State variables used for obstacle detection
#define NOOBSTACLE 0
#define OBSTACLE 1
#define TURN 2
#define GOFORWARD 3


//the subroutines
void sensor(void); // detect distance to obstacle
void initialize(void); // all the usual mcu stuff

unsigned char reload, //timer 0 reload to set 1 mSec
lcd_buffer[17]; // LCD display buffer

int count, count1, count2, count3, count4, Cdis;
char time1, state, index, Ct[8], C[6];
float Ccm;

unsigned int temp;
unsigned char stepR, // Right motor step
stepL, // Left motor step
obstacle; // Obstacle detection flag
unsigned char dir, // Direction of movement
target, // Direction of target
obsDir; // Obstacle detection

// Declare Timer 0 interrupt
interrupt [TIM0_OVF] void timer0_overflow(void)
{
TCNT0=reload;
if(time1 > 0) --time1;
}

// B R O W
// + +
// + +
// + +
// + +

// Stepping sequence

// Step the motor in the FORWARD direction
char forward(char x){
switch(x){
case (1+2):
x = 2+4;
break;
case (2+4):
x = 4+8;
break;
case (4+8):
x = 8+1;
break;
case (8+1):
x = 1+2;
break;
default:
x = 1+2;
break;
}
return x;
}

// Step the motor in the backward direction
char backward(char x){
switch(x){
case (1+2):
x = 8+1;
break;
case (2+4):
x = 1+2;
break;
case (4+8):
x = 2+4;
break;
case (8+1):
x = 4+8;
break;
default:
x = 1+2;
break;
}
return x;
}

// Determine direction of movemnent
void movement(void){
// Turn RIGHT
if( dir == RIGHT ){
stepR = forward(stepR);
stepL = forward(stepL);

}
// Turn LEFT
else if( dir == LEFT ){
stepR = backward(stepR);
stepL = backward(stepL);

}
// Move FORWARD
else if( dir == FORWARD ){
stepR = forward(stepR);
stepL = backward(stepL);

}
// Move BACKWARD
else if( dir == BACKWARD ){
stepR = backward(stepR);
stepL = forward(stepL);

}
// Output stepper sequence to motor
PORTA = (stepL<<4) | stepR;
}


void initialize(void)
{

// Vin
DDRC.0=1;
PORTC.0=1;

// Vout
DDRC.1=0;
PORTC.1=0;

// PORT A D OUPUT
DDRA = 0xff;
DDRD = 0xff;

// PORT B INPUT
DDRB = 0x00;

// Used for RS232 debugging
UCSRB = 0x10 + 0x08;
UBRRL = 103;
UBRRH = 0;

// Initially scan the vicinity for the target
dir = RIGHT;
// Initially reset to no obstacle
obsDir = NOOBSTACLE;

//set up timer 0
reload=256-26; //value for 0.1 msec
TCNT0=reload; //preload timer 1 so that is interrupts after 1 mSec.
TCCR0=prescale0; //prescalar to 64
TIMSK=1; //turn on timer 0 overflow ISR

// Initiate counters
time1 = t1;
count = 0;
count2 =0;
count3 = 0;
count4 = 0;
count1 = 0;

// Initiate obstacle distance to 0
Cdis = 0;

// Reset the distance counter
for(index=0;index<8;index++){
Ct[index]=0;
}

// Initialize obstacle sensor
index = 0;
state = RESET;

#asm
sei
#endasm

}

void getObsDir(void){
switch(obsDir){
case NOOBSTACLE:
dir = FORWARD;
if(obstacle)
obsDir = OBSTACLE;
break;
case OBSTACLE:
dir = RIGHT;
if(!obstacle)
obsDir = TURN;
break;
case TURN:
if( count2++ > 7500 ){
count2 = 0;
obsDir = NOOBSTACLE;
}
if(obstacle)
obsDir = OBSTACLE;
}
}

void main(void)
{
// Initialize
initialize();
while(1) {

// If there is an obstacle in the vicinity set a flag
if(Ccm > 34) obstacle = 0;
else obstacle = 1;



// Interrupt
if(time1 == 0){
// AI to find direction
switch(obsDir){

// In the event that there is no obstacle
// the bot scans the vicinity for the target every 3 sec
// Once it is facing the target it starts moving forward
// in that direction
// Every 3 seconds the bot beings to scan again
case NOOBSTACLE:
// In case an obstacle is detected
// deal with the obstacle
if(obstacle){
obsDir = OBSTACLE;
break;
}
// If bot is scanning and the forward sensor comes on
// then move in the forward direction
if(~PINB.0 == 1 && ( dir != FORWARD) ){ // FORWARD blue
dir = FORWARD;
}

// If the bot is scanning and the right sensor comes on
// continue scanning in the right direction
if( ~PINB.1 == 1 && ( dir != FORWARD)){ // RIGHT red
dir = RIGHT;
}

// If the bot is scanning and the left sensor comes on
// continue scanning in the left direction
if( ~PINB.3 == 1 && ( dir != FORWARD)){ // LEFT black
dir = LEFT;
}

// If the bot is facing backwards
// start scanning in the left direction
if( ~PINB.2 == 1 ){ // BACKWARD green
dir = LEFT;
}
// Reset the bot every 3 secs so that it starts scanning again
if(count3++ > 30000){
dir = LEFT;
count3 = 0;
}
break;
case OBSTACLE:
// In case of an obstacle turn right to avoid the obstacle
dir = RIGHT;
if(!obstacle){
count2 = 0;
obsDir = TURN;
}
break;

case TURN:
// Continue avoiding the obstacle for a small time
// and then move in the forward direction
if( count2++ > 7500 ){
obsDir = GOFORWARD;
count4 = 0;
}
if(obstacle)
obsDir = OBSTACLE;
break;

case GOFORWARD:
// Move ahead in the forward direction once
// the obstacle has been avoided
dir = FORWARD;
if( count4++ > 20000){
if(!obstacle){
obsDir = NOOBSTACLE;
count3 = 0;
}else
obsDir = OBSTACLE;
}
if(obstacle)
obsDir = OBSTACLE;
break;
}

// Check for obstacle
sensor();
// Step motors in the intended direction of movement
if(count1++ > 250){
count1 = 0;
movement();
}
}
}
}


void sensor(void){
// Reset timer
time1 = t1;

switch(state){
case RESET:
PORTC.0 = 0;
count = 0;
state = INIT;
break;
case INIT:
// Set Vin low for 70 ms
PORTC.0 = 0;
if(count++ > 700){
count = 0;
index = 0;
state = MEAS;
}
break;
case MEAS:
// Toggle Vin high and low alternately 8 times
if(count++ <= 15){
// Record serial ouput from sensor into an array
PORTC.0 = !PORTC.0;
if(PORTC.0 & (index++ < 8 ))
Ct[index] = PINC.1;
}else{
// Use array to compute distance in
PORTC.0 = 1;
count = 0;
index = 0;
state = WAIT;
Cdis = (Ct[0]*128)+(Ct[1]*64)+(Ct[2]*32)+(Ct[3]*16)+(Ct[4]*8)+(Ct[5]*4)+(Ct[6]*2)+(Ct[7]*1);
Ccm = kgC/(Cdis-koC);
ftoa(Ccm,3,C);
printf("%s",C); // debug
}
break;
case WAIT:
// Set Vin low for 1.5 msec and then continue measuring distance
if(count++ > 15){
count = 0;
state = INIT;
}
break;
default:
state = RESET;
break;
}
}