#include <Adafruit_SSD1306.h>
#include <Wire.h>
//#include <SPI.h>
//#include <SD.h>

#define pi 3.14 
#define brakelight 8
#define diameter .66
#define numberofreadings 6
#define cutoff 3
#define velocitycutoff 1.2   // in m/s
#define displaychangeafter 5000

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

const int chipSelect= 9;

int n=0;
int count=0;
double instant=0;
double previnstant=0;
double prevprevinstant=0;
int wheelcount=0;
float velocity=0;
float accleration=0;
float distance=0;
int temp; 
bool unit1=0;
bool unit2=1;
bool unit3=0;
int unitmillis1=0;
int unitmillis2=0;
int unitmillis3=0;



void setup()
{
  pinMode(2,INPUT_PULLUP);
  pinMode(brakelight,OUTPUT);
  attachInterrupt(digitalPinToInterrupt(2),cross,FALLING);

  display.begin(SSD1306_SWITCHCAPVCC , 0x3C);
  display.setTextColor(WHITE);
  display.setTextSize(3);
  
/*
 
    if (!SD.begin(chipSelect)) {

    display.setTextSize(1);
    display.setCursor(0,23);
    display.print("No SD Card");    
  }
  display.display();
  display.setTextSize(3);

  File dataFile = SD.open("datalog.txt",FILE_WRITE);

  if(dataFile)
  {
  dataFile.println("RESTART,RESTART,RESTART,RESTART");
  dataFile.println("TIME,Accleration,Speed,Distance");
  dataFile.close();
  }
  */
}

void loop()
{

  if(0>accleration)
  {
    digitalWrite(brakelight,HIGH);   
  }
  
  if(0<accleration)
  {
    digitalWrite(brakelight,LOW);
  }


  
  {

    if(unit1!=1&&unit2!=1&&unit3!=1)
    {
      display.clearDisplay();
      display.setCursor(0,0);
      display.print(velocity*18/5);
      display.setTextSize(2);
      display.setCursor(67,18);
      display.print("km/hr");
      display.display();
      display.setTextSize(3);
      display.display();
    }

    else
    {
      display.clearDisplay();
      display.setCursor(0,0);
      display.print(distance);
      display.setTextSize(1);
      display.setCursor(90,25);
      if(distance==0){
        display.print("START");
      }
      else{
        display.print("meters");
      }
      display.display();
      display.setTextSize(3);
    }
     
  /*
    File dataFile = SD.open("datalog.txt", FILE_WRITE);
   if(dataFile)
    {
      dataFile.print(instant);
      dataFile.print(",");
      dataFile.print(accleration);
      dataFile.print(",");
      dataFile.print(velocity);
      dataFile.print(",");
      dataFile.println(distance);
      dataFile.close();
    }
  */
    
  }

  if(unitmillis1!=millis()/displaychangeafter+1)
  {   
    unitmillis1=millis()/displaychangeafter+1;
    unit1=1;
  }
  if(unitmillis2!=millis()/displaychangeafter+2)
  {
    unitmillis2=millis()/displaychangeafter+2;
    unit2=1;
  }
  if(unitmillis3!=millis()/displaychangeafter)
  {
    unitmillis3=millis()/displaychangeafter;
    unit3=1;
  }
  
}

void cross()
{

  prevprevinstant=previnstant;
  previnstant=instant;
  instant=micros()/1000.0;
  wheelcount++;


  if(wheelcount>2)
  {
    distance=diameter*wheelcount*pi;
    velocity=diameter/(instant-previnstant)*pi;
    accleration=2*diameter*pi*(1/(instant-previnstant)-1/(previnstant-prevprevinstant))/(instant-prevprevinstant);
    velocity=velocity*1000; 
    accleration=accleration*100000000;
    
    unit3=0;
    unit2=0;
    unit1=0; 
    
  }

  
}