/*
Moto/moped speed limiter, based on magnetic/induction front wheel speed sensor 
*/
unsigned long wheelRoundTime = 9999999;
const unsigned long nextWheelSensorWait=40000; // 40ms. about 150km/h. Avoid false/noise triggers
unsigned long previousMicros = 0;
const float wheelDiameter=0.6; //meters 
unsigned int speedSmooth[3]={0,0,0};
unsigned int speedSmoothCounter=0;
const float adjustRange = 100; // 100km/h 
const unsigned int targetOffset=20; // 20km/h (=120km/h) 
unsigned int target=62;  
bool oneRoundBlink=true;
bool wheelSignalAllowed=false;
bool everySecondSpark=true;

// the setup function runs once when you press reset or power the board
void setup() {
  //Serial.begin(9600);
  pinMode(2, INPUT); // SpeedSensor, Hall effect sensor
  pinMode(8, OUTPUT);  // Control triac switch for ignition grounding
  pinMode(6, INPUT_PULLUP);  // Hijacked "Turn off" button, switch directly to GND, which has secret disable speedlimit feature
  pinMode(12, OUTPUT);  // Indicator led, that things are working
  digitalWrite(8, LOW); // Triac control
  // Read target speed setting once in the beginning
  delay(2000);
  readSpeedTargetSetting();
  // Interrupt on D2
  digitalWrite(12, HIGH);
  attachInterrupt(digitalPinToInterrupt(2), wheelRound, FALLING );  // speed sensor signal interrupt
}

// the loop function runs over and over again forever
void loop() 
{
  unsigned int currSpeed=getSpeed();
  if (currSpeed > target)
  {
    if (everySecondSpark)
    {
      digitalWrite(8, HIGH); // Cut spark
      everySecondSpark=false;
    }
    else
    {
      everySecondSpark=true;
    }
  }
  else 
  {
    digitalWrite(8, LOW); // Spark on
  }
  
  // Check switch-off button
  if (digitalRead(6)==LOW) // Button down
  {
    //Serial.println("Button pressed");
    long timeStamp=millis();
    long waitBetweenClicks=200;
    
    while (millis() < (timeStamp+waitBetweenClicks)) // 0.2seconds to release button
    {
       //Serial.println("Button phase 1");
       if (digitalRead(6)==HIGH) // Button up
       {
          //Serial.println("Button phase 1 released");
          while (millis() < timeStamp+waitBetweenClicks+waitBetweenClicks) // another 0.2 seconds to push button again
          {
              if (digitalRead(6)==LOW)
              {
                  //Serial.println("Button phase 2 pressed");
                  while (millis() < timeStamp+waitBetweenClicks+waitBetweenClicks+waitBetweenClicks) // another 0.2.secs to release button
                  {
                      if (digitalRead(6)==HIGH)
                      {
                        //Serial.println("Button phase 3 release, speed limit off");
                        // turn off speed limiter
                        target=500; // Allowed to go 500Km/h :-)
                        digitalWrite(12, LOW); // Led Off 
                        delay(200);
                        digitalWrite(12, HIGH); // Led On 
                        delay(200);
                        digitalWrite(12, LOW); // Led Off 
                        delay(200);
                        digitalWrite(12, HIGH); // Led On 
                        delay(200);
                        digitalWrite(12, LOW); // Led Off 
                        delay(200);
                        digitalWrite(12, HIGH); // Led On 
                        break;
                      }
                  }
              }
          }
       }
       else 
       {
          // Button was not released, proceed to normal turn off
          //break;
       }
    }

    //Serial.println("Button normal mode")
    // Normal turn off button behaviour, button was not clicked twice rapidly
    while(digitalRead(6)==LOW)
    {
      //Serial.println("Button normal mode: spark off");
      digitalWrite(8, HIGH); // Cut spark, turn off    
    }
    digitalWrite(8, LOW); // button up, spark alive   
  }
}

// Check wheel round pulse
void wheelRound()
{
  /*Serial.print("micros():");
  Serial.println(micros());
  Serial.print("micros()-previous_micros():");
  Serial.println(micros()-previousMicros);
  Serial.print("NextWheelSensorWait:       ");
  Serial.println(nextWheelSensorWait);*/
  
  if ( (micros()-previousMicros) > nextWheelSensorWait )
  {  
    wheelRoundTime=micros()-previousMicros;
    previousMicros=micros();
  
    //Serial.print("Wheel spin took time:");
    //Serial.println(wheelRoundTime);
    
    if (oneRoundBlink)
    {
      digitalWrite(LED_BUILTIN, HIGH);
      digitalWrite(12, HIGH);
      oneRoundBlink=false;
    }
    else
    {
      digitalWrite(LED_BUILTIN, LOW);
      digitalWrite(12, LOW);
      oneRoundBlink=true;
    }
  }
  else
  {
    //Serial.println("False speedwheel sensor trigger");
  }
}

int getSpeed()
{
   // Calculate time between last two wheel signals
  if (speedSmoothCounter > (sizeof(speedSmooth)/sizeof(speedSmooth[0])) )
  {
    speedSmoothCounter=0;   
  }
  float wheelRoundTimeSeconds=(float)(wheelRoundTime/((float)1000000));
  //Serial.print("wheel time in secs:");
  //Serial.println((float)wheelRoundTimeSeconds);
  float wheelTravelInMeters =(float)(wheelDiameter*3.14);
  //Serial.print("distance in meters:");
  //Serial.println((float)wheelTravelInMeters);
  float speedMs=wheelTravelInMeters/wheelRoundTimeSeconds;
  //Serial.print("Speed m/s:");
  //Serial.println(speedMs);
  
  int speedKmh = speedMs*3.6;
  /*Serial.print("Speed Km/h:");
  Serial.print(speedKmh);
  Serial.print(" (");
  Serial.print(target);
  Serial.println(")");*/
  
  speedSmooth[speedSmoothCounter]=speedKmh;
  
  unsigned long speedSum=0;
  for(int index = 0; index < sizeof(speedSmooth)/sizeof(speedSmooth[0]); index++)
  { 
    //Serial.print("speed[:");
    //Serial.print(index);
    //Serial.print("]:");
    //Serial.println(speedSmooth[index]);
    speedSum += speedSmooth[index]; 
  }

  int speed=speedSum/(sizeof(speedSmooth)/sizeof(speedSmooth[0]));

  //Serial.print("Smoothed speed km/h:");
  //Serial.println(speed);
  
  speedSmoothCounter++;
   
  speed = min(200,speed);
  speed = max(0, speed);
  return speed;
}

void readSpeedTargetSetting()
{
  float adjustment=analogRead(0);
  //Serial.print("A0:");
  //Serial.println(adjustment);
  float adjustedSpeed=(float)adjustment*(float)(adjustRange/1024);
  //Serial.print("adjustedSpeed:");
  //Serial.println(adjustedSpeed);

  target=adjustedSpeed+targetOffset;
  //Serial.print("-------------------------------target speed:");
  //Serial.println(target);
}