#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);

#define TPSpin A0
#define MPHpin A1
#define Apin 10
#define Bpin 9
#define Lpin 6
#define Ppin 5
#define NOTpin A2
#define WOTpin A3

#define DownShiftSlack 0.8	//difference between upshift and downshift

#define LowShift 30 	//mph idle shift into 4th gear
#define HighShift 120 	//mph wot shift into 4th gear

#define LockDelay 5 		//add mph after 4th
#define LineMinPres 30 			//idle line duty
#define LineMaxTPS 50 			//max tps for line reduction

int TPS;
int MPH;
int Gear;
int Lock;
int Line;
int TPSmin;
int TPSmax;
int NOToffset;
int WOToffset;

float GearRatio[5] {0, 3.06, 1.63, 1.00, 0.70};

void printxy (int value, int x, int y) {
  lcd.setCursor(x, y);
  lcd.print(value);
  if (value < 100) lcd.print(" ");
  if (value < 10)  lcd.print(" ");
}

void printxyS (int value, int x, int y) {
  lcd.setCursor(x, y);
  lcd.print(value);
}

int ScaleTPS (int TPSraw) {
  if (TPSraw < TPSmin) TPSmin = TPSraw;
  if (TPSraw > TPSmax) TPSmax = TPSraw;
  return map(TPSraw, TPSmin, TPSmax, 0, 100);
}

void GearSol() {
  if (Gear == 1) {
    digitalWrite(Apin, HIGH);
    digitalWrite(Bpin, HIGH);
  }
  if (Gear == 2) {
    digitalWrite(Apin, LOW);
    digitalWrite(Bpin, HIGH);
  }
  if (Gear == 3) {
    digitalWrite(Apin, LOW);
    digitalWrite(Bpin, LOW);
  }
  if (Gear == 4) {
    digitalWrite(Apin, HIGH);
    digitalWrite(Bpin, LOW);
  }

}  

int ArrayMPH (int WhatGear) {
  float GearOffset = 1 / GearRatio[WhatGear];
  int GearLow  = LowShift  * GearOffset * NOToffset / 100;
  int GearHigh = HighShift * GearOffset * WOToffset / 100;
  if (TPS<15) return GearLow;
  if (TPS>85) return GearHigh;
  return map(TPS, 15, 85, GearLow, GearHigh);
}
  
void GearSelector () {
  if (Gear == 4 && MPH < ArrayMPH(3)*DownShiftSlack) Gear=3;
  if (Gear == 3 && MPH < ArrayMPH(2)*DownShiftSlack) Gear=2;
  if (Gear == 2 && MPH < ArrayMPH(1)*DownShiftSlack) Gear=1;
  if (Gear == 1 && MPH > ArrayMPH(1) )				       Gear=2;
  if (Gear == 2 && MPH > ArrayMPH(2) )				       Gear=3;
  if (Gear == 3 && MPH > ArrayMPH(3) )				       Gear=4;
}

void LockUpSelector () {
  if (Gear == 4) {
        if (MPH > (ArrayMPH(3)                + LockDelay)) Lock = 1;
    	if (MPH < (ArrayMPH(3)*DownShiftSlack + LockDelay)) Lock = 0;
        }
  else Lock = 0;
}
      
void LockUpSol (){
  if (Lock == 1) digitalWrite(Lpin, HIGH);
  else digitalWrite(Lpin, LOW);
}


void LineSelector () {
  if (TPS > LineMaxTPS) Line=0; 
  else Line=LineMinPres - map(TPS, 0, LineMaxTPS, 0, LineMinPres);
}
            
void LineSol () {
  analogWrite(Ppin, Line);
}

void setup()
{
  lcd.init();
  lcd.backlight();
     
  lcd.setCursor(0,0);
  lcd.print("T=");
  
  lcd.setCursor(0,1);
  lcd.print("M=");
  
  lcd.setCursor(5, 0);
  lcd.print("G=");
  
  lcd.setCursor(11, 0);
  lcd.print("Ln=");

  lcd.setCursor(5, 1);
  lcd.print("L=");

  lcd.setCursor(11, 1);
  lcd.print("H=");
  
  Gear = 1;
  Lock = 0;
  TPSmin = 200;
  TPSmax = 800;
  
  pinMode(Apin, OUTPUT);
  pinMode(Bpin, OUTPUT);
  pinMode(Lpin, OUTPUT);
  pinMode(Ppin, OUTPUT);
  pinMode(TPSpin, INPUT_PULLUP);
  pinMode(MPHpin, INPUT_PULLUP);
  pinMode(NOTpin, INPUT_PULLUP);
  pinMode(WOTpin, INPUT_PULLUP);

}

void loop()
{
  TPS = ScaleTPS(analogRead(TPSpin));
  MPH = analogRead(MPHpin)/6.6;

  NOToffset = map(analogRead(NOTpin), 0, 1023, 50, 150) ;
  WOToffset = map(analogRead(WOTpin), 0, 1023, 50, 150); 
 
  GearSelector (); 
  LockUpSelector ();
  LineSelector ();
  
  GearSol();
  LockUpSol();
  LineSol();

  printxy (TPS, 2, 0);
  printxy (MPH, 2, 1);
  printxyS(Lock, 9,0);
  printxyS(Gear, 7, 0);
  printxy (Line, 14, 0);
  printxy (NOToffset, 7, 1);
  printxy (WOToffset, 13, 1);
 }