#include <Servo.h>
#include <SSD1306Ascii.h>
#include <SSD1306AsciiAvrI2c.h>

#define PIN_POT A0
#define PIN_ESC 5
#define PIN_THERM A7
#define PIN_BAT A2

#define MIN_SPEED 900
#define MAX_SPEED 1700
#define ZERO_SPEED 900
#define POT_MIN 183
#define POT_MAX 717

#define STEP 10
#define DISPLAY_I2C 0x3C
#define DISPLAY_CHARS 11
#define THERM_RESISTOR 10000

#define THRESHOLD 5
#define DELAY 50

//#define USE_DISPLAY

Servo esc;
#ifdef USE_DISPLAY
SSD1306AsciiAvrI2c display;
#endif

void setup() {
  Serial.begin(115200);
  Serial.println("Hello");

#ifdef USE_DISPLAY
  Serial.println("Looking for i3c 0x"+String(DISPLAY_I2C, 16)+"...");
  display.begin(&Adafruit128x32, DISPLAY_I2C);
  display.setFont(Adafruit5x7);
  display.clear();
  display.set2X();
  display.print("Hello");
#endif
  
  pinMode(PIN_POT, INPUT);
  pinMode(PIN_THERM, INPUT);
  pinMode(PIN_BAT, INPUT);
  
  esc.attach(PIN_ESC);
  esc.writeMicroseconds(ZERO_SPEED);
  delay(4000);
  Serial.println("ready");
}

int prev = -1;

float measureTemp() {
  int val = analogRead(PIN_THERM);
  float r2 = (THERM_RESISTOR / 10.0) / (1023.0 / (float)val - 1.0);
  float logR2 = log(r2);
  float c1 = 1.009249522e-03, c2 = 2.378405444e-04, c3 = 2.019202697e-07;
  float t = (1.0 / (c1 + c2*logR2 + c3*logR2*logR2*logR2));
  float celsius = t - 273.15;
  return celsius;
}

int measureBat() {
  int val = analogRead(PIN_BAT);
  float volt = val * (5.0 / 1023.0);
  Serial.println(volt);
  return 50;
}

void printTwo(String s1, String s2) {
#ifdef USE_DISPLAY
  int nspaces = DISPLAY_CHARS - (s1.length() + s2.length());
  char spaces[DISPLAY_CHARS + 1];
  memset(spaces, ' ', DISPLAY_CHARS);
  spaces[nspaces] = '\0';
  display.println(s1+String(spaces)+s2);
#endif
}

void showStats(int actualSpeed, int potmeterVal) {
  int throttle = ((double)(actualSpeed - MIN_SPEED) / (double)(MAX_SPEED - MIN_SPEED)) * 100;

#ifdef USE_DISPLAY
  display.setCursor(0, 0);
  display.set2X();
#endif

  String pot = String(potmeterVal);
  String thr = String(throttle)+"%";
  String spd = String(actualSpeed);
  String tmp = String(round(measureTemp()))+"C";
  String bat = String(measureBat())+"%";

  printTwo(thr, pot+" ");
  printTwo(tmp, bat+" ");
}

void loop() {
  int pressed = 0;
  int reachedMax = 0;

  pressed = 1;
  int pot = analogRead(PIN_POT);
  int speed = map(pot, POT_MIN, POT_MAX, 0, MAX_SPEED - MIN_SPEED);
  speed = speed / STEP;
  speed = speed * STEP;

  int actualSpeed = speed + MIN_SPEED;

  if (abs(actualSpeed - ZERO_SPEED) < THRESHOLD)
    actualSpeed = ZERO_SPEED;
  else if (actualSpeed > MAX_SPEED)
    actualSpeed = MAX_SPEED;
  else if (actualSpeed < MIN_SPEED)
    actualSpeed = MIN_SPEED;

  if (actualSpeed != prev) {
    esc.writeMicroseconds(actualSpeed);
    prev = actualSpeed;
  }
  
  showStats(actualSpeed, pot);
  delay(DELAY);
}