#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);  
//Screen init

#define TPSpin A0     // TPS
#define MPHpin A1     // MPH input
#define Apin 8        // Solenoid A
#define Bpin 7        // Solenoid B
#define Tpin 9       // Solenoid(s) TCC
#define Dpin 10       // Solenoid Line Pressure PWM
#define Spin 6        // Solenoid 3-2 downshift
#define FSTpin A6
#define LOTpin A2
#define WOTpin A3
#define SCRpin 12

const float DownShiftSlack = 0.9;	// difference between upshift and downshift MPH
const int ShiftPoint = 45;   	    // low throttle 3>4 shift
const float GearSpread = 3;     // (GearSpread)*(ShiftPoint)=theoretical MAX MPH in 3rd

const int WOTthresh = 80;         // WOT threshhold for 1-2 shift

const int FirstRangeLow = 10;     //Fst knob range
const int FirstRangeHigh = 60;    //Fst knob range
const int ShiftRangeLow = 50;     // Low & High shift knob low range
const int ShiftRangeHigh = 150;   // Low & High shift knob high range

const int LockDelay = 5;   		    // MPH adder for lockup after 4th
const int LineMinPres = 30; 			// idle line duty
const int LineMaxTPS = 50;  			// max TPS for line reduction

int TPS;
int MPH;
int Gear = 1;
int Lock = 0;
int Line = 0;
int TPSmin = 200;                  // selfcal TPS 
int TPSmax = 800;                  // selfcal TPS 
int FSToffset;                     // dynamic 1st gear slip
int LOToffset;                     // dynamic low throttle shift
int WOToffset;                     // dynamic WOT shift

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

void PrintXY_n (int value, int x, int y) {
  display.setCursor(x, y);
  display.print(value);
}

void PrintXY_s (String value, int x, int y) {
  display.setCursor(x, y);
  display.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  = ShiftPoint * GearOffset * LOToffset / 100;               //always devide last in int calc
  int GearHigh = ShiftPoint * GearOffset * WOToffset * GearSpread / 100;
  if (TPS<15) return GearLow;
  if (TPS>85) return GearHigh;
  return map(TPS, 15, 85, GearLow, GearHigh);
}

int ConvSlip (){
  return 100 - FSToffset;
}

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 && TPS > WOTthresh && MPH > ArrayMPH(1)*ConvSlip()/100 ) Gear = 2; 
  else 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(Tpin, HIGH);
  else digitalWrite(Tpin, LOW);
}


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

void DownShiftSol(){
  if (Gear == 1) analogWrite(Spin, 0); //if in first gear 0%
  else analogWrite(Spin, 230);         //all other gears 90%
}

void print1 (){
  display.clearDisplay();
  PrintXY_s ("TPS=", 0, 0);
  PrintXY_n (TPS, 24, 0);

  PrintXY_s ("MPH=",49, 0);
  PrintXY_n (MPH, 73, 0);
  
  PrintXY_s("Gear=", 49, 11);
  PrintXY_n(Gear, 79, 11);
  
  PrintXY_s("L=", 103, 0);
  PrintXY_n(Lock,117, 0);

  PrintXY_s("Line=", 0, 11);
  PrintXY_n (Line, 30, 11);
 
  PrintXY_s ("ConverterSlip=", 0, 33);
  PrintXY_n (FSToffset, 85, 33);
  PrintXY_s ("LOT=", 0, 44);
  PrintXY_n (LOToffset, 24, 44);
  PrintXY_s ("WOT=", 49, 44);
  PrintXY_n (WOToffset, 73, 44);
  display.display();
}



void setup()
{

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);    
  
  TCCR0B = TCCR0B & B11111000 | B00000101; // for PWM frequency of 61.04 Hz D5 & D6

  pinMode(Apin, OUTPUT);
  pinMode(Bpin, OUTPUT);
  pinMode(Tpin, OUTPUT);
  pinMode(Dpin, OUTPUT);
  pinMode(TPSpin, INPUT_PULLUP);
  pinMode(MPHpin, INPUT_PULLUP);
  pinMode(LOTpin, INPUT_PULLUP);
  pinMode(WOTpin, INPUT_PULLUP);
  pinMode(FSTpin, INPUT_PULLUP);
  pinMode(SCRpin, INPUT_PULLUP);
}

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

  FSToffset = map(analogRead(FSTpin), 0, 1023, FirstRangeLow, FirstRangeHigh);
  LOToffset = map(analogRead(LOTpin), 0, 1023, ShiftRangeLow, ShiftRangeHigh);
  WOToffset = map(analogRead(WOTpin), 0, 1023, ShiftRangeLow, ShiftRangeHigh); 
 
  GearSelector (); 
  LockUpSelector ();
  LineSelector ();
  
  GearSol();
  LockUpSol();
  LineSol();
  DownShiftSol();
  
 print1();
 
 }