/*
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x3F,16,2);
*/

//stepper motor stuff
#include <SwitecX25.h>
SwitecX25 motor1(945, 13,14,15,16);
SwitecX25 motor2(945, 17,18,19,12);



//gear pins
int gear1=1;
int gear2=3;
int gear3=4;
int gear4=5;
int gear5=6;
int gear6=7;
int gear0=8;

int wheeloffset=16;

//interrupt pin
int ignitionpin=2;

//ignition values
long deltat=0;
long RPM=0;
long currentRPM=0;
int pulsecounter=0;
long timea=0;
long timeb=0;

//needle variable
int needleposition=0;

//array    -- modify RPMarraysize and RPMrr to alter needle smoothness
const int RPMarraysize=4;
int RPMrr=20; //display RPM refresh rate

int RPMarray[RPMarraysize];
int arrayposition=0;

//tachsweeping
int tachswept=0;
long sweeptime;

void setup() {
/*
  // start 16x2 parallel lcd
  lcd.init();
  lcd.backlight();
*/

//ignition pulse interrupt on digital pin 2
pinMode(ignitionpin, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ignitionpin),ignitionpulse, FALLING);


//gear sensor pins
pinMode(gear1, INPUT_PULLUP);
pinMode(gear2, INPUT_PULLUP);
pinMode(gear3, INPUT_PULLUP);
pinMode(gear4, INPUT_PULLUP);
pinMode(gear5, INPUT_PULLUP);
pinMode(gear6, INPUT_PULLUP);
pinMode(gear0, INPUT_PULLUP);



//zero the stepper motor
motor1.currentStep = motor1.steps;
motor2.currentStep = motor2.steps;
motor1.setPosition(0);
motor2.setPosition(0);
while (motor1.currentStep > 0 || motor2.currentStep > 0) {
motor1.update();
motor2.update();
}



  delay(100);
  motor1.setPosition(690);
 motor2.setPosition(663);
  //motor2.setPosition(0+wheeloffset);
  sweeptime=millis();
}



void loop() {
/*
lcd.setCursor(0,0);
lcd.print(currentRPM);
lcd.print("     ");

lcd.setCursor(0,1);
lcd.print(RPM);
lcd.print("     ");
*/


if (tachswept==0){
  tachsweeper();
}

if (tachswept==1){
   checkRPM();
   checkgear();
}





motor1.update();
motor2.update();

}


void tachsweeper(){

if (millis()-sweeptime > 900){
  tachswept=1;
  motor1.setPosition(0);
  motor2.setPosition(0);
}

}


void checkRPM(){
 if (millis()-timea >= 150){
  currentRPM=0;
  timea=millis(); //this line can be removed???
 }

 if (millis()-timeb >= RPMrr){

    RPMarray[arrayposition]=currentRPM;
    arrayposition=arrayposition+1;

    if (arrayposition > RPMarraysize-1){
      arrayposition=0;

      RPM=0;
      for (int x=0; x<RPMarraysize; x++){
        RPM=RPM+RPMarray[x];
      }
      RPM=RPM/RPMarraysize;
    }
    timeb=millis();

 }

   needleposition=(690*RPM/12500);//set the needle to position. motor position 690=12500RPM if 0=0.

   if (needleposition > 944){ //protect against running off the scale. Max RPM that can be displayed is 17119rpm
    needleposition=944;
   }

  motor1.setPosition(needleposition);
}



void ignitionpulse(){

  currentRPM=60000/(millis()-deltat);
  deltat=millis();
  timea=millis();

}



void checkgear(){

if (digitalRead(gear1) == LOW){
  motor2.setPosition(124);
}
if (digitalRead(gear2) == LOW){
  motor2.setPosition(232);
}
if (digitalRead(gear3) == LOW){
  motor2.setPosition(340);
}
if (digitalRead(gear4) == LOW){
  motor2.setPosition(448);
}
if (digitalRead(gear5) == LOW){
  motor2.setPosition(557);
}
if (digitalRead(gear6) == LOW){
  motor2.setPosition(663);
}
if (digitalRead(gear0) == LOW){
  motor2.setPosition(16);
}


}