#include <joyos.h>
#include "driving.h"
#include "sensing.h"

int8_t counter;

int8_t Orientation;
int8_t Stop_Condition;

int8_t right_color;
int8_t middle_color;
int8_t left_color;

uint16_t Front_Distance;
uint16_t Left_Distance;
uint16_t Right_Distance;

uint8_t Front_Bump;
uint8_t Left_Bump;
uint8_t Right_Bump;

uint16_t Average_Encoder_Read(){
	return (uint16_t)(encoder_read(RightEncoder) + encoder_read(LeftEncoder))/2;
}

int8_t convert_LED_measurement(uint16_t measurement){

	if(measurement < ColorTolerance){
		return white;
	}
	else{
		return black;
	}
}

uint16_t find_color_measurement(uint8_t sensor){
	
	int8_t IR_counter = 0;
	
	for(int8_t i=0; i < 5;i++){
		
		IR_counter += analog_read(sensor);
		pause(1);
	}
	
	return convert_LED_measurement((uint16_t)IR_counter/5);
}

uint16_t find_distance_measurement(uint8_t sensor){
	
	int8_t Distance_counter = 0;
	
	for(int8_t i=0; i < 5;i++){
		
		Distance_counter += irdist_read(sensor);
		pause(1);
	}
	
	return ((uint16_t)Distance_counter/5);
}

uint8_t find_bump_measurement(uint8_t sensor){
	
	int8_t Bump_counter = 0;
	
	for(int8_t i=0; i < 5;i++){
		
		Bump_counter += analog_read(sensor);
		pause(1);
	}
	
	return ((uint8_t)Bump_counter/5);
}

void set_all_colors(){
	
	uint16_t right_LED_counter = 0;
	uint16_t middle_LED_counter = 0;
	uint16_t left_LED_counter = 0;
	
	for(int8_t i=0; i < 5;i++){
		right_LED_counter += analog_read(Right_LED);
		middle_LED_counter += analog_read(Middle_LED);
		left_LED_counter += analog_read(Left_LED);
		pause(1);
	}
	
	right_color = convert_LED_measurement((uint16_t)right_LED_counter/5);
	middle_color = convert_LED_measurement((uint16_t)middle_LED_counter/5);
	left_color = convert_LED_measurement((uint16_t)left_LED_counter/5);
}

void set_all_distances(){
	
	uint16_t front_distance_counter = 0;
	uint16_t left_distance_counter = 0;
	uint16_t right_distance_counter = 0;
	
	for(int8_t i=0;i < 5;i++){
	
	front_distance_counter += irdist_read(Front_Distance_Sensor);
	left_distance_counter += irdist_read(Left_Distance_Sensor);
	right_distance_counter += irdist_read(Right_Distance_Sensor);
	pause(1);
	}
	
	Front_Distance = (uint16_t) front_distance_counter/5;
	Left_Distance = (uint16_t) left_distance_counter/5;
	Right_Distance = (uint16_t) right_distance_counter/5;
}

void set_all_bumps(){
	
	uint8_t front_bump_counter =0;
	uint8_t left_bump_counter =0;
	uint8_t right_bump_counter =0;
	
	for(int8_t i=0;i < 5;i++){
	
	front_bump_counter += digital_read(Front_Bump_Sensor);
	left_bump_counter += digital_read(Left_Bump_Sensor);
	right_bump_counter += digital_read(Right_Bump_Sensor);
	pause(1);
	}
	
	Front_Bump = (uint8_t) front_bump_counter/5;
	Left_Bump = (uint8_t) left_bump_counter/5;
	Right_Bump = (uint8_t) right_bump_counter/5;
}

void orient_gameboard(){
	
	CoastMotors();
	pause(100);
	
	set_all_distances();
	
	if(Front_Distance < ShortDistanceTolerance && Left_Distance < ShortDistanceTolerance){
	
		Orientation = West;
	}
	else if((Front_Distance <= ShortDistanceTolerance) && (LongDistanceTolerance <  Left_Distance)){
	
		Orientation = South;
	}
	else if((Left_Distance < ShortDistanceTolerance) && (Front_Distance > LongDistanceTolerance)){
	
		Orientation = North;
	}
	else{
	
		Orientation = East;
	}
}

int8_t detect_line(){                                      // Returns 0 is no line detected, 1 if detected

	set_all_colors();
	
	if(left_color == black || middle_color == black || right_color == black){
		return 1;              
	}
	
	else{
		return 0;            
	}
}