#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 <SoftwareSerial.h>


#include "img.cpp"

#define DEBUG true

#define BATTPIN 34
#define WIRE Wire
#define _TASK_STATUS_REQUEST
#define SPEEDSCREEN 1
#define MAXSPEEDSCREEN 2
#define VOLTAGESCREEN 3

Scheduler sch;

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

float input_voltage = 0.0;
float voltAdjust = 0.0;
float temp = 0.0;
float r1 = 47000.0;
float r2 = 33000.0;
bool lowVoltage = false;
float voltage = 0.0;

char newLine = 0xA;
int speed;
int maxSpeed = 0;
int currentScreen = 1;
int maxScreens = 3;

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 tick();
Task tTick(10 * TASK_MILLISECOND, TASK_FOREVER, &tick, &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()
{
	float analog_value = analogRead(BATTPIN);
	Serial.println(analog_value);
	//Serial.println(analog_value);
	//temp = analog_value / (4095.0 * 5);
	//Serial.println(temp);
	//input_voltage = temp * (r2 / (r1 + r2)) - voltAdjust;
	input_voltage = (analog_value / 4096) * 16;
	if (input_voltage < 0.1)
	{
		input_voltage = 0.0;
	}
	return input_voltage;
}

void title(String title)
{
	gpsSignal();
	display.setCursor(0, 0);
	display.setTextColor(WHITE);
	display.setTextSize(1);
	if (lowVoltage == true)
	{
		display.println("(LOW VOLT!)");
		display.println(title);
	}else{
		display.println("");
		display.println(title);
	}
}

void displaySpeed()
{
	display.clearDisplay();
	title("Speed:");
	display.setTextSize(5);
	display.print(String(speed));
	display.setTextSize(2);
	display.println(" mph");
	display.display();
}

void displayMaxSpeed()
{
	display.clearDisplay();
	title("Max speed:");
	display.setTextSize(5);
	display.print(String(maxSpeed));
	display.setTextSize(2);
	display.println(" mph");
	display.display();
}

void displayVoltage()
{
	display.clearDisplay();
	title("Battery:");
	display.setTextSize(3);
	display.print(voltage);
	display.setTextSize(2);
	display.println(" v");
	display.display();
}

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() >= 3 || DEBUG)
	{
		if (currentScreen > maxScreens)
		{
			currentScreen = SPEEDSCREEN;
		}
		else if (currentScreen < 1)
		{
			currentScreen = maxScreens;
		}
		else if (currentScreen == SPEEDSCREEN)
		{
			displaySpeed();
		}
		else if (currentScreen == MAXSPEEDSCREEN)
		{
			displayMaxSpeed();
		}
		else if (currentScreen == VOLTAGESCREEN)
		{
			displayVoltage();
		}
	}
	else
	{
		displaySearching();
	}
}

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

void buttonSingleClick()
{
	currentScreen++;
}

void buttonLongClick()
{
	if (currentScreen == MAXSPEEDSCREEN)
	{
		maxSpeed = 0;
	}
}

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

void tick()
{
	voltage = getVoltage();
	if (voltage <= 11.0)
	{
		lowVoltage = true;
	}
	else
	{
		lowVoltage = false;
	}
	
	speed = gps.speed.mph();
	if (speed > maxSpeed)
	{
		maxSpeed = speed;
	}
}

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

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