#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <string.h>
#include <SPI.h>
#include <Wire.h>
#include <TinyGPSPlus.h>
#include <TaskScheduler.h>
#include <OneButton.h>

#include "dir.cpp"
#include "img.cpp"

#define BATTPIN 34
#define VOLTAGEADJUST 0.64
#define WIRE Wire
#define _TASK_STATUS_REQUEST 
#define SPEEDSCREEN 0
#define MAXSPEEDSCREEN 1
#define HOMESCREEN 2
#define VOLTAGESCREEN 3

Scheduler sch;

Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &WIRE);
TinyGPSPlus gps;
OneButton button(12, true);

float input_voltage = 0.0;
float temp=0.0;
float r1=97000.0;
float r2=10000.0;

char newLine = 0xA;
int speed;
int maxSpeed = 0;
int currentScreen = 0;
int maxScreens = 4;

double homeLAT = NULL;
double homeLONG = NULL;

void displayScreen();
Task tDisplayScreen(10 * TASK_MILLISECOND, TASK_FOREVER, &displayScreen, &sch,true);

void feedGPS();
Task tFeedGPS(10 * TASK_MILLISECOND, TASK_FOREVER, &feedGPS, &sch, true);

void buttonTick();
Task tbuttonTick(10 * TASK_MILLISECOND, TASK_FOREVER, &buttonTick, &sch, true);

void buttonSingleClick();
void buttonLongClick();

void gpsSignal(){
	if(gps.satellites.value() >=0 && gps.satellites.value() <= 4){
		display.drawXBitmap(120,0, signalOne_bits, signalOne_width, signalOne_height,16);
	}else if(gps.satellites.value() >= 5 && gps.satellites.value() <= 16){
		display.drawXBitmap(120,0, signalTwo_bits, signalTwo_width, signalTwo_height,16);
	}else if(gps.satellites.value() >= 17){
		display.drawXBitmap(120,0, signalThree_bits, signalThree_width, signalThree_height,16);
	}
}

float getVoltage(){
	int analog_value = analogRead(BATTPIN);     
	temp = (analog_value * 5.0) / 4095.0;   
	input_voltage = (temp / (r2/(r1+r2))) - VOLTAGEADJUST;        
	if (input_voltage < 0.1)   	{     
		input_voltage=0.0;    
	}     
	return input_voltage;
}

void displaySpeed(){
	speed = gps.speed.mph();
	display.clearDisplay();
	display.setCursor(0,0);
	display.setTextColor(WHITE);
	display.setTextSize(2);
	display.println("Speed:");
	display.setTextSize(5);
	display.print(String(speed));
	display.setTextSize(2);
	display.println(" mph");
	gpsSignal();
	display.display();
}

void displayMaxSpeed(){
	display.clearDisplay();
	display.setCursor(0,0);
	display.setTextColor(WHITE);
	display.setTextSize(2);
	display.println("Max Speed:");
	display.setTextSize(5);
	display.print(String(maxSpeed));
	display.setTextSize(2);
	display.println(" mph");
	gpsSignal();
	display.display();
}

void displayHomeScreen(){
	display.clearDisplay();
	display.setCursor(0,0);
	display.setTextColor(WHITE);
	display.setTextSize(1);
	display.print("Lat: ");
	display.println(gps.location.lat());
	display.print("Long:");
  display.println(gps.location.lng());
	if(homeLAT == NULL && homeLONG == NULL){
		display.setTextSize(2);
		display.println("Long press");
		display.println("to set ");
		display.println("home.");
	}else{
		display.setTextSize(3);
		display.println();
		
		String dir = gps.cardinal(gps.courseTo(gps.location.lat(), gps.location.lng(), homeLAT, homeLONG));
		if(dir == "N"){
			display.drawXBitmap(32,32,NORTH_bits,NORTH_width, NORTH_height, 2);
		}else if(dir == "NE"){
			display.drawXBitmap(32,32,NE_bits,NE_width, NE_height, 2);
		}else if(dir == "E"){
			display.drawXBitmap(32,32,EAST_bits,EAST_width, EAST_height, 2);
		}else if(dir == "SE"){
			display.drawXBitmap(32,32,SE_bits,SE_width, SE_height, 2);
		}else if(dir == "S"){
			display.drawXBitmap(32,32,SOUTH_bits,SOUTH_width, SOUTH_height, 2);
		}else if(dir == "SW"){
			display.drawXBitmap(32,32,SW_bits,SW_width, SW_height, 2);
		}else if(dir == "W"){
			display.drawXBitmap(32,32,WEST_bits,WEST_width, WEST_height, 2);
		}else if(dir == "NW"){
			display.drawXBitmap(32,32,NW_bits,NW_width, NW_height, 2);
		}else if(dir == "ENE"){
			display.drawXBitmap(32,32,ENE_bits,ENE_width, ENE_height, 2);
		}else if(dir == "ESE"){
			display.drawXBitmap(32,32,ESE_bits,ESE_width, ESE_height, 2);
		}else if(dir == "NNE"){
			display.drawXBitmap(32,32,NNE_bits,NNE_width, NNE_height, 2);
		}else if(dir == "NNW"){
			display.drawXBitmap(32,32,NNW_bits,NNW_width, NNW_height, 2);
		}else if(dir == "SSE"){
			display.drawXBitmap(32,32,SSE_bits,SSE_width, SSE_height, 2);
		}else if(dir == "SSW"){
			display.drawXBitmap(32,32,SSW_bits,SSW_width, SSW_height, 2);
		}else if(dir == "WNW"){
			display.drawXBitmap(32,32,WNW_bits,WNW_width, WNW_height, 2);
		}else if(dir == "WSW"){
			display.drawXBitmap(32,32,WSW_bits,WSW_width, WSW_height, 2);
		}else{
			display.println(dir);
		}
	}
	gpsSignal();
	display.display();
}

void displayVoltage(){
	display.clearDisplay();
	display.setCursor(0,0);
	display.setTextColor(WHITE);
	display.setTextSize(2);
	display.println("Battery:");
	display.setTextSize(3);
	display.print(getVoltage());
	display.setTextSize(2);
	display.println(" v");
	gpsSignal();
	display.display();
	delay(500);
}

void displaySearching(){
	display.clearDisplay();
	display.setCursor(0,0);
	display.setTextColor(WHITE);
	display.setTextSize(1);
	display.print(gps.satellites.value());
	display.println(" satellites");
	display.println("");
	display.setTextSize(2);
	display.println("Searching for");
	display.println("satellites..");
	gpsSignal();
	display.display();
}

void displayScreen(){
	if(gps.satellites.value() >= 4){
		if(currentScreen > maxScreens - 1){
			currentScreen = SPEEDSCREEN;
		}else if(currentScreen < 0){
			currentScreen = maxScreens - 1;
		}else if(currentScreen == SPEEDSCREEN){
			displaySpeed();
		}else if(currentScreen == MAXSPEEDSCREEN){
			displayMaxSpeed();
		}else if(currentScreen == HOMESCREEN){
			displayHomeScreen();
		}else if(currentScreen == VOLTAGESCREEN){
			displayVoltage();
		}
		speed = gps.speed.mph();
		if(speed > maxSpeed){
			maxSpeed = speed;
		}
	}else{
		displaySearching();
	}
}

void feedGPS(){
	while(Serial.available() > 0){
		gps.encode(Serial.read());
	}
}

void buttonSingleClick(){
	currentScreen++;
}

void buttonLongClick(){
	if(currentScreen == MAXSPEEDSCREEN){
		maxSpeed = 0;
	}else if(currentScreen == HOMESCREEN){
		homeLAT = gps.location.lat();
    homeLONG = gps.location.lng();
	}
}

void buttonTick(){
	button.tick();
}

void setup() {
  	Serial.begin(9600);
	display.begin(SSD1306_SWITCHCAPVCC, 0x3C); 
	button.attachClick(buttonSingleClick);
	button.attachLongPressStop(buttonLongClick);

}

void loop() {
	sch.execute();
}