#include "HC12.H"
#include "DBug12.h"
#include "Robot.h"

#define N_equeue 10

//#define OLDWF
#define NEWWF

int front_error;
char room_count = 0;
char fire_room = 0;

main()
{
	int i;
	unsigned char tone;

	setup();

	while(1){		/* wait for tone decoder */	
		DBug12FNP->printf("no tone (tone=%d)\n\r", tone);
		if(!(PORTT & 0x08))
			tone++;
		else
			tone= 0;
		if(tone>=7)
			break;
	}		DBug12FNP->printf("tone\n\r");

	for (i = 0; i < 1000; i++)
		wallfollow(LEFT_DIST, LEFT, FRONT_DIST);

        while(room_count < 4) {
		while(!white_line_sensor())
			wallfollow(LEFT_DIST,LEFT,FRONT_DIST);

		room_count++;
		stop();
		if(hamamatsu(room_count)) {
			fire_room = room_count;
			switch(room_count) {
			case 1:
				fire1();
				break;
			case 2:
				fire2();
				break;
			case 3:
				fire3();
				break;
			}
			break;
		} else {
			while(FRONT_DIST > 200)
				move_robot(45, -45);
			canned_turn(4, LEFT);
			canned_turn(90, RIGHT);
			move_robot(DESIRED_SPEED, DESIRED_SPEED);
			delay(400);
			canned_turn(90, LEFT);
			move_robot(DESIRED_SPEED, DESIRED_SPEED);
			delay(650);
		}
		if(room_count == 3) {
			room4();
		}
        }

	go_home();

	stop();
}

void setup()
{
        PWCTL &= ~0x08;         /* left-aligned */
        PWCLK = 0x12; /* 8bit mode N=2 for all channels */
        PWSCAL1 = 10;

        PWCTL &= ~0x08;
        PWPOL &= 0xf0;          /* -ve polarity for Allegro 2998 */   
        PWPOL = (PWPOL & ~0x30) | 0x80;   /* CH 0 & 1: Clock A, CH 3: S1 */
	PWPER0 = 199;
	PWPER1 = 199;
        PWPER3 = 199;
        PWEN = 0x0b;            /* enable bits 0,1, and 3 of port P for PWM */
	DDRP = ~0x20;
	PORTP = 0x50;
	RIGHT_FWD;
	LEFT_FWD;
	PWDTY3 = 100;

	DDRT &= ~0x88;		/* set up bits 3 & 7 on port T as input */

        ATDCTL2 = 0xc0;         /* Power up ATD subsystem */
        ATDCTL4 = 0x04;         /* set timer prescaler */
        ATDCTL5 = 0x70;         /* set 8 bit, scan, & sequential channels */

	DDRS = 0xff;		/* set port S as output */
	PORTS = 0x00;		/* clear port S */
}
unsigned char hamamatsu(unsigned char rooms)
 {
  unsigned int firecount, scan_time;

  firecount = 0;
  if((rooms == 2) || (rooms == 3))
   {
    scan_time = 18000;
    scan_time /= (unsigned short) 10;
    firecount = count_hamamatsu(scan_time);
   }
  else
   {
    scan_time = 9000;
    scan_time /= (unsigned short) 10;
    firecount = count_hamamatsu(scan_time);
   }

  if(firecount > 2) return 1;
  else return 0;
 }


/* Count hamamatsu pulses for time_interval (in msec): */
int count_hamamatsu(const unsigned int time_interval)
 {
  int time, rtiticks;
  int timerhandle;
  /*
   (a) 
   Enable PACTL (Set bit PAEN in the PACTL register) 
   (b) 
   Set mode to event count (Clear bit PAMOD in the PACTL register) 
   (c) 
   Choose edge with PEDGE bit of PACTL register: 
   PEDGE = 0 => count falling edges 
   PEDGE = 1 => count rising edges 
   (d) 
   Clear 16-bit PACNT register (if desired) 
   (e) 
   If desired, enable interrupt on any selected edge - set PAI bit of PACTL register 
   (f) 
   If desired, enable interrupt on PACNT overflow - set PAOVI bit of PACTL register 
   (g) 
   When desired, read PACNT register to determine how many events have occurred. 
  */
  /* TSCR = 0x80; */ /* Enable Timer */

  /* (a) */
  PACTL |= 0x40; /* Enable PACTL (set PAEN bit 6) */
  /* (b) */
  PACTL &= ~0x20; /* Enable event counter mode (clear PAMOD bit 5) */
  /* (c) */
  PACTL |= 0x10; /* Count rising edges (set PEDGE bit 4) */
  /* (e),(f)... yes, this is out of order. */
  PACTL &= ~0x03; /* Disable pulse accumulator related interrupts */

  /* (d) */
  PACNT = 0; /* Start the pulse accumulator count to 0 */

  //rti_delay(time_interval);
  delay(time_interval);

  return PACNT;
 }



#ifdef OLDWF
void wallfollow(int dist, int side, int front)
{
	static int eprev[N_equeue] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
	static int iq = 0;
	int iqprev;
	int e, dedt, dangle;
        signed int dist_error, left_speed, right_speed;
	const int k1 = 3;
	const int k2 = 10;


	iqprev = (iqprev >= N_equeue) ? 0 : iq + 1;
        e = (signed int)DESIRED_DIST - (signed int)dist;
        front_error = (front < 180) ? 180-front : 0;
	dedt = e - eprev[iqprev];

	if(side == LEFT)
	{
		dangle = k1*(k2*dedt + e);
	        left_speed = (signed int)DESIRED_SPEED + dangle + front_error*front_error;
        	right_speed = (signed int)DESIRED_SPEED - dangle - front_error*front_error;
        	if (dist > 230)
                	left_speed = 9;
	}
	else
	{
		dangle = k1*(k2*dedt + e);
	        left_speed = (signed int)DESIRED_SPEED - dangle - front_error*front_error;
        	right_speed = (signed int)DESIRED_SPEED + dangle + front_error*front_error;
        	if (dist > 230)
                	right_speed = 9;
	}

	//DBug12FNP->printf("left motor = %d right motor = %d\n\r", PWDTY0, PWDTY0);
        //PWDTY0 = (unsigned char)left_speed;
        //PWDTY1 = (unsigned char)right_speed;
	
	eprev[iq++] = e;
	if(iq >= N_equeue) iq -= N_equeue;
	
	delay(1);
	
	move_robot(left_speed,right_speed);
}
#endif


#ifdef NEWWF
void wallfollow(int dist, int side, int front)
{
	static int eprev[N_equeue] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
	static int iq = 0;
	int iqprev;
	int e, dedt, dangle;
        signed int dist_error, left_speed, right_speed;
	const int k1 = 3;
	const int k2 = 8;
//	const int k2 = 0;

//	iqprev = (iqprev >= N_equeue) ? 0 : iq + 1;
	iqprev = (iq >= (N_equeue - 1)) ? 0 : iq + 1;
//        e = (signed int)DESIRED_DIST - (signed int)dist;
	e = (signed int)dist - (signed int)DESIRED_DIST;
        front_error = (front < 180) ? 180-front : 0;
	dedt = e - eprev[iqprev];


	if(side == LEFT)
	{
		dangle = k1*(k2*dedt + e);
	        left_speed = (signed int)DESIRED_SPEED - dangle + front_error*front_error;
        	right_speed = (signed int)DESIRED_SPEED + dangle - front_error*front_error;
        	if (dist > 230)
                	left_speed = 9;
		if (front_error > 2)
		 {
			//left_speed = DESIRED_SPEED;
			//right_speed = -DESIRED_SPEED;
			canned_turn(90, RIGHT);
		 }
	}
	else
	{
		dangle = k1*(k2*dedt + e);
	        left_speed = (signed int)DESIRED_SPEED + dangle - front_error*front_error;
        	right_speed = (signed int)DESIRED_SPEED - dangle + front_error*front_error;
        	if (dist > 230)
                	right_speed = 9;
		if (front_error > 2)
		 {
			//left_speed = DESIRED_SPEED;
			//right_speed = -DESIRED_SPEED;
			canned_turn(90, LEFT);
		 }
	}
	
	eprev[iq++] = e;
	if(iq >= N_equeue) iq -= N_equeue;
	
	delay(1);
	
	move_robot(left_speed,right_speed);
}
#endif




/* Function to control the robot's velocity */
/* The max speed allowed is +-255 (i.e. 9-bit resolution)	*/
void move_robot(signed int left_speed, signed int right_speed)     
{
unsigned char l_speed, r_speed;

    	l_speed = (unsigned char) MINIMUM( INT_ABS(left_speed), 199 ) ;
	PWDTY0 = l_speed - 1 ;	/* Set speed & fix ofsett prob. in pwm */
    	if(left_speed >= 0) /* Set the direction bit */
 			LEFT_FWD ;
    	else
			LEFT_REV ;

	r_speed = (unsigned char) MINIMUM( INT_ABS(right_speed), 199 ) ;
	PWDTY1 = r_speed - 1 ;	/* Set speed & fix ofsett prob. in pwm */
    	if(right_speed >= 0)/* Set the direction bit */
			RIGHT_FWD ;
    	else
			RIGHT_REV ;
}


void canned_turn(unsigned char degrees, unsigned char direction)
{
	signed int left_speed, right_speed;
	unsigned int n;
	if(direction == RIGHT){
		for(n = 0; n <= (degrees *230); n++){
			move_robot(180, -180);
		}
	}else {
		for (n = 0; n <= (degrees * 230); n++){
			move_robot(-180, 180);
		}
	}
	
	move_robot(0,0);
}

char room4()
{
	unsigned int i;

	while(LEFT_DIST < 230)
		wallfollow(LEFT_DIST, LEFT, FRONT_DIST);
	move_robot(DESIRED_SPEED, DESIRED_SPEED);
	while(FRONT_DIST > 190) ;
	stop();
	canned_turn(90, LEFT);
	while(!white_line_sensor())
		wallfollow(RIGHT_DIST, RIGHT, FRONT_DIST);
	room_count++;
	stop();
	fire_room = room_count;
	if(hamamatsu(room_count))
		fire4();
	return(0);
}

void stop()
{
	move_robot(-30, -30);
	delay(150);
	move_robot(0, 0);
}

void go_home()
{
	unsigned int i,j;
	switch (room_count) {
	case 1:
		for(i = 0; i < 100; i++)
			wallfollow(RIGHT_DIST, RIGHT, FRONT_DIST);
		while(!white_line_sensor())
			wallfollow(RIGHT_DIST,RIGHT,FRONT_DIST);
		break;
	case 2:
		for(i=0;i<1000;i++)
			for(j=0;j<2;j++)
				wallfollow(LEFT_DIST,LEFT,FRONT_DIST);
		delay(500);
		while(LEFT_DIST < 235)
			wallfollow(LEFT_DIST,LEFT,FRONT_DIST);
		move_robot(DESIRED_SPEED, DESIRED_SPEED - 1);
		delay(500);
		while(RIGHT_DIST < 235)
			wallfollow(RIGHT_DIST,RIGHT,FRONT_DIST);
		delay(500);
		while(!white_line_sensor())
			wallfollow(LEFT_DIST,LEFT,FRONT_DIST);
		break;
	case 3:
		move_robot(DESIRED_SPEED,DESIRED_SPEED);
		while(FRONT_DIST < 190);
		stop();
		canned_turn(100,LEFT);
		move_robot(DESIRED_SPEED,DESIRED_SPEED);
		while(LEFT_DIST > 230);
		stop();
		while(!white_line_sensor())
			wallfollow(LEFT_DIST,LEFT,FRONT_DIST);
		break;
	case 4:
		for(i = 0; i < 5000; i++)
			wallfollow(LEFT_DIST, LEFT, FRONT_DIST);
		while(!white_line_sensor())
			wallfollow(RIGHT_DIST,RIGHT,FRONT_DIST);
		break;
	}		
}

void follow_fire()
{
	int fire_error, left_speed, right_speed;

	move_robot(FIRE_SPEED, FIRE_SPEED);
	delay(50);
	while(!white_line_sensor()) {
		fire_error = (int)(LEFT_IR - RIGHT_IR);

		left_speed = FIRE_SPEED + fire_error/6;
		right_speed = FIRE_SPEED - fire_error/6;

		left_speed = MINIMUM(left_speed,70);
		right_speed = MINIMUM(right_speed,70);

		move_robot(left_speed,right_speed);
	}
	stop();
//	move_robot(130, 130);
//	delay(100);
//	stop();
	fan();	
	return;
}

int move_robot_room(signed int left_speed, signed int right_speed, unsigned int delay1)
{
	unsigned char l_speed, r_speed;
	unsigned int i,j;
	int wline = 0;

	l_speed = (unsigned char) MINIMUM(INT_ABS(right_speed), 199);
	PWDTY0 = l_speed -1;
	if(left_speed >= 0)
		LEFT_FWD;
	else
		LEFT_REV;
	r_speed = (unsigned char) MINIMUM(INT_ABS(right_speed), 199);
	PWDTY1 = r_speed -1;
	if(right_speed >= 0)
		RIGHT_FWD;
	else
		RIGHT_REV;

	wline = white_line_sensor();
	for(i = 0; i < delay1 && !wline; i++)
		for(j = 0; (j < WLINE_DELAY/4) && !wline; j++)
			wline = white_line_sensor();

	DBug12FNP->printf("move_robot, i:%d\n\r",i);
	stop();
	return(white_line_sensor());
}

unsigned int mini_scan(unsigned char degrees, int dir)
{
	unsigned int fire, high_val = 0;
	unsigned int i;

	fire = 0;
	i = 0;
	if(dir == RIGHT)
		move_robot(30, -30);
	else if(dir == LEFT)
		move_robot(-30, 30);
	for(i = 0; i < 200 * degrees; i++)
	{
		int j;
		for(j = 0; j < 15; j++)
		{
			high_val = (LEFT_IR + RIGHT_IR) / 2;
			if(high_val > FIRE_THR)
			{
				fire = 1;
				goto EXIT_FIRE;
			}
		}
	}
EXIT_FIRE:
	canned_turn(4, !dir);
	return fire;
}

int early_line()
{
	stop();
	if(mini_scan(80, LEFT))
	{
		fan();
		return(1);			// put it out here
	}
	canned_turn(70, RIGHT);
	canned_turn(4, LEFT);
	if(mini_scan(90, RIGHT))
	{
			fan();
			return(1);			// put it out here too
	}
	fan();
	return(1);
}

int white_line_sensor()
{
	int white_line, i;

	white_line = PORTP & 0x20;
	for(i = 0; i < WLINE_DELAY; i++);
	return (white_line && (PORTP & 0x20));
}

void fire1()
{
	move_robot(DESIRED_SPEED, DESIRED_SPEED);
	delay(200);

	if (move_robot_room(DESIRED_SPEED, DESIRED_SPEED, 200)) {
		early_line();
		canned_turn(190,RIGHT);
		move_robot(DESIRED_SPEED,DESIRED_SPEED);
		delay(300);
		stop();
		return;
	}
	if (mini_scan(150,RIGHT)) {
		canned_turn(4, LEFT);
		follow_fire();

		goto END_FIRE1;
	}
	canned_turn(140,LEFT);
	canned_turn(4, RIGHT);
	if (mini_scan(50,LEFT)) {
		follow_fire();
		goto END_FIRE1;
	}

	canned_turn(60, RIGHT);
	canned_turn(4, LEFT);
	if (move_robot_room(DESIRED_SPEED, DESIRED_SPEED, 400)) {
		early_line();
		canned_turn(190,RIGHT);
		move_robot(DESIRED_SPEED,DESIRED_SPEED);
		while(FRONT_DIST > 190);
		while(!white_line_sensor())
			wallfollow(RIGHT_DIST,RIGHT,FRONT_DIST);
		goto END_FIRE1;
	}

	if (mini_scan(110, LEFT)) {
		follow_fire();
		goto END_FIRE1;
	}

	canned_turn(65,RIGHT);
	canned_turn(4, LEFT);
	if (mini_scan(110,RIGHT)) {
		follow_fire();
	}

END_FIRE1:
	canned_turn(90, LEFT);
	move_robot(DESIRED_SPEED, DESIRED_SPEED);
	while(FRONT_DIST > 190);
	stop();
	canned_turn(90, LEFT);
	delay(1000);
	while(!white_line_sensor())
		wallfollow(RIGHT_DIST, RIGHT, FRONT_DIST);
	return;
}

void fire2()
{
	move_robot(DESIRED_SPEED, DESIRED_SPEED);
	delay(300);

	if (move_robot_room(DESIRED_SPEED, DESIRED_SPEED, 200)) {
		early_line();
		canned_turn(90, RIGHT);
		goto END_FIRE2;
	}

	if (mini_scan(60,LEFT)) {
		canned_turn(4, RIGHT);
		follow_fire();
		canned_turn(90, RIGHT);
		goto END_FIRE2;
	}

	canned_turn(60,RIGHT);
	canned_turn(4, LEFT);
	if (mini_scan(100,RIGHT)) {
		follow_fire();
		canned_turn(70, RIGHT);
		goto END_FIRE2;
	}
	canned_turn(50, LEFT);
	canned_turn(4, RIGHT);
	if (move_robot_room(DESIRED_SPEED, DESIRED_SPEED, 550)) {
		early_line();
		canned_turn(60,RIGHT);
		while(FRONT_DIST > 190);
		goto END_FIRE2;
	}

	if (mini_scan(120, LEFT)) {
		follow_fire();
		canned_turn(90, RIGHT);
		goto END_FIRE2;
	}

	canned_turn(110,RIGHT);
	canned_turn(4, LEFT);
	if (mini_scan(120,RIGHT)) {
		follow_fire();
		canned_turn(100, RIGHT);
	}

END_FIRE2:
	move_robot(DESIRED_SPEED,DESIRED_SPEED);
	while(FRONT_DIST > 180);
	while(!white_line_sensor())
		wallfollow(LEFT_DIST, LEFT, FRONT_DIST);
	return;
}

void fire3()
{
	canned_turn(20, RIGHT);
	move_robot(FIRE_SPEED, FIRE_SPEED);
	delay(150);
	if(move_robot_room(DESIRED_SPEED, DESIRED_SPEED, 300))
	{
		early_line();
		canned_turn(180, RIGHT);
		move_robot(DESIRED_SPEED, DESIRED_SPEED);
		delay(200);
		goto END_FIRE3;
	}
	if(mini_scan(50, LEFT))
	{
		follow_fire();
		canned_turn(100, RIGHT);
		move_robot(DESIRED_SPEED, DESIRED_SPEED);
		while(FRONT_DIST > 190) ;
		goto END_FIRE3;
	}
	canned_turn(40, RIGHT);
	canned_turn(4, LEFT);
	if(mini_scan(80, RIGHT))
	{
		follow_fire();
		canned_turn(100, RIGHT);
		move_robot(DESIRED_SPEED, DESIRED_SPEED);
		while(FRONT_DIST > 190) ;
		goto END_FIRE3;
	}
	if(mini_scan(60, RIGHT))
	{
		follow_fire();
		canned_turn(130, RIGHT);
		move_robot(97, 100);
		delay(250);
	}
END_FIRE3:
	while(!white_line_sensor())
		wallfollow(LEFT_DIST, LEFT, FRONT_DIST);
	return;
}

void fire4()
{
	move_robot(DESIRED_SPEED, DESIRED_SPEED);
	delay(100);
	if(move_robot_room(DESIRED_SPEED, DESIRED_SPEED, 200))
	{
		early_line();
		canned_turn(180, LEFT);
		while(!white_line_sensor())
			move_robot(DESIRED_SPEED, DESIRED_SPEED);
		move_robot(DESIRED_SPEED, DESIRED_SPEED);
		delay(100);
		goto END_FIRE4;
	}
	if(mini_scan(90, RIGHT))
	{
		follow_fire();
		canned_turn(170, RIGHT);
		while(!white_line_sensor())
			move_robot(DESIRED_SPEED, DESIRED_SPEED);
		goto END_FIRE4;
	}
	canned_turn(80, LEFT);
	canned_turn(3, RIGHT);
	if(mini_scan(140, LEFT))
	{
		follow_fire();
		canned_turn(110, RIGHT);
		move_robot(DESIRED_SPEED, DESIRED_SPEED);
		while(FRONT_DIST > 160) ;
		while(!white_line_sensor())
			wallfollow(LEFT_DIST, LEFT, FRONT_DIST);
		move_robot(DESIRED_SPEED, (DESIRED_SPEED - 5));
		delay(200);
	}
END_FIRE4:
	return;
}

void fan()
{
	int i, j, inc = 0;

	PORTS = 0x04;			/* TURN ON FAN */
	do
	{
		if(inc > 200)
			break;
		delay(1000 + inc);
		canned_turn(10, RIGHT);
		delay(500 + inc);
		canned_turn(10, RIGHT);
		delay(500 + inc);
		canned_turn(10, LEFT);
		delay(500 + inc);
		canned_turn(10, LEFT);
		delay(500 + inc);
		canned_turn(10, LEFT);
		delay(500 + inc);
		canned_turn(10, LEFT);
		delay(500 + inc);
		canned_turn(10, RIGHT);
		delay(500 + inc);
		canned_turn(10, RIGHT);
		delay(500 + inc);
		for(i=0; ((i<6) && ((LEFT_IR+RIGHT_IR)/2 < FIRE_THR)); i++)
			for(j=0; ((j<10000) && ((LEFT_IR+RIGHT_IR)/2 < FIRE_THR)); j++) ;
		inc += 200;
//	} while(((LEFT_IR + RIGHT_IR)/2) > (FIRE_THR - 10));
	} while(((LEFT_IR + RIGHT_IR)/2) > (FIRE_THR - 10) || (count_hamamatsu(500) > 2));

	PORTS = 0;			/* TURN THE FAN OFF */

}

/* Function that implements a delay for 0 < "num" < 64k ms */
void delay(unsigned int num)
{
    unsigned int i;

    while (num>0){       /* Outer loop delays "num" ms */
        i = 2000;        /* Inner loop delays for 1 ms */
        while (i > 0){
            i = i-1;
        }
        num = num - 1;
    }
}