/*
Speeduino - Simple engine management for the Arduino Mega 2560 platform
Copyright (C) Josh Stewart

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,la
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#ifndef UNIT_TEST  // Scope guard for unit testing

#include <stdint.h> //https://developer.mbed.org/handbook/C-Data-Types
//************************************************
#include "globals.h"
#include "speeduino.h"
#include "table.h"
#include "scheduler.h"
#include "comms.h"
#include "cancomms.h"
#include "maths.h"
#include "corrections.h"
#include "timers.h"
#include "decoders.h"
#include "idle.h"
#include "auxiliaries.h"
#include "sensors.h"
#include "storage.h"
#include "crankMaths.h"
#include "init.h"
#include BOARD_H //Note that this is not a real file, it is defined in globals.h. 
#if defined (CORE_TEENSY)
#include <FlexCAN.h>
#endif

void setup()
{
  initialiseAll();
}

void loop()
{
      mainLoopCount++;
      LOOP_TIMER = TIMER_mask;
      //Check for any requets from serial. Serial operations are checked under 2 scenarios:
      // 1) Every 64 loops (64 Is more than fast enough for TunerStudio). This function is equivalent to ((loopCount % 64) == 1) but is considerably faster due to not using the mod or division operations
      // 2) If the amount of data in the serial buffer is greater than a set threhold (See globals.h). This is to avoid serial buffer overflow when large amounts of data is being sent
      //if ( (BIT_CHECK(TIMER_mask, BIT_TIMER_15HZ)) || (Serial.available() > SERIAL_BUFFER_THRESHOLD) )
      if ( ((mainLoopCount & 31) == 1) or (Serial.available() > SERIAL_BUFFER_THRESHOLD) )
      {
        if (Serial.available() > 0) { command(); }
        else if(cmdPending == true)
        {
          //This is a special case just for the tooth and composite loggers
          if (currentCommand == 'T') { command(); }
        }
        
      }
      //if can or secondary serial interface is enabled then check for requests.
      if (configPage9.enable_secondarySerial == 1)  //secondary serial interface enabled
          {
            if ( ((mainLoopCount & 31) == 1) or (CANSerial.available() > SERIAL_BUFFER_THRESHOLD) )
                {
                  if (CANSerial.available() > 0)  { canCommand(); }
                }
          }
      #if  defined(CORE_TEENSY) || defined(CORE_STM32)
          else if (configPage9.enable_secondarySerial == 2) // can module enabled
          {
            //check local can module
            // if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ) or (CANbus0.available())
            //    {
            //      CANbus0.read(rx_msg);
            //    }
          }
      #endif

    //Displays currently disabled
    // if (configPage2.displayType && (mainLoopCount & 255) == 1) { updateDisplay();}

    previousLoopTime = currentLoopTime;
    currentLoopTime = micros_safe();
    unsigned long timeToLastTooth = (currentLoopTime - toothLastToothTime);
    if ( (timeToLastTooth < MAX_STALL_TIME) || (toothLastToothTime > currentLoopTime) ) //Check how long ago the last tooth was seen compared to now. If it was more than half a second ago then the engine is probably stopped. toothLastToothTime can be greater than currentLoopTime if a pulse occurs between getting the lastest time and doing the comparison
    {
      currentStatus.longRPM = getRPM(); //Long RPM is included here
      currentStatus.RPM = currentStatus.longRPM;
      FUEL_PUMP_ON();
      currentStatus.fuelPumpOn = true; //Not sure if this is needed.
    }
    else
    {
      //We reach here if the time between teeth is too great. This VERY likely means the engine has stopped
      currentStatus.RPM = 0;
      currentStatus.PW1 = 0;
      currentStatus.VE = 0;
      currentStatus.VE2 = 0;    //[PJSC]Multi VE Map support
      currentStatus.VE3 = 0;    //[PJSC v1.01]Multi VE Map support
      currentStatus.VE4 = 0;    //[PJSC v1.01]Multi VE Map support
      toothLastToothTime = 0;
      toothLastSecToothTime = 0;
      //toothLastMinusOneToothTime = 0;
      currentStatus.hasSync = false;
      currentStatus.runSecs = 0; //Reset the counter for number of seconds running.
      secCounter = 0; //Reset our seconds counter.
      currentStatus.startRevolutions = 0;
      toothSystemCount = 0;
      secondaryToothCount = 0;
      MAPcurRev = 0;
      MAPcount = 0;
      currentStatus.rpmDOT = 0;
      AFRnextCycle = 0;
      ignitionCount = 0;
      ignitionOn = false;
      fuelOn = false;
//[PJSC v1.01]      if (fpPrimed == true) { FUEL_PUMP_OFF(); currentStatus.fuelPumpOn = false; } //Turn off the fuel pump, but only if the priming is complete
      if ( (fpPrimed == true) && (currentStatus.testActive == false) ) { FUEL_PUMP_OFF(); currentStatus.fuelPumpOn = false; }   //[PJSC v1.01]
      disableIdle(); //Turn off the idle PWM
      BIT_CLEAR(currentStatus.engine, BIT_ENGINE_CRANK); //Clear cranking bit (Can otherwise get stuck 'on' even with 0 rpm)
      BIT_CLEAR(currentStatus.engine, BIT_ENGINE_WARMUP); //Same as above except for WUE
      BIT_CLEAR(currentStatus.engine, BIT_ENGINE_RUN); //Same as above except for RUNNING status
      BIT_CLEAR(currentStatus.engine, BIT_ENGINE_ASE); //Same as above except for ASE status
      //This is a safety check. If for some reason the interrupts have got screwed up (Leading to 0rpm), this resets them.
      //It can possibly be run much less frequently.
      initialiseTriggers();
      if(configPage2.exTrigModeSelect != 0) { initialiseExternalTrigger(); }              //[PJSC] For External Trigger
      if(configPage2.dutyPulseCaptureEnabled == true) { initialiseCaptureDutyPulse(); }   //[PJSC] For capturing duty pulse
      if(configPage2.dutyPulseCaptureEnabled2 == true) { initialiseCaptureDutyPulse2(); } //[PJSC] For capturing duty pulse

      if( currentStatus.testOutputs == 0 )        //[PJSC v1.01]
      {                                           //[PJSC v1.01]
        VVT_PIN_LOW();
        DISABLE_VVT_TIMER();
        boostDisable();
      }                                           //[PJSC v1.01]
    }

    //***Perform sensor reads***
    //-----------------------------------------------------------------------------------------------------
    readMAP();

    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_15HZ)) //Every 32 loops
    {
      BIT_CLEAR(TIMER_mask, BIT_TIMER_15HZ);
      readTPS(); //TPS reading to be performed every 32 loops (any faster and it can upset the TPSdot sampling time)
//[PJSC v1.02]      if(configPage2.exValveCaptureEnabled == 1) { readExValvePosition(); }    //[PJSC] For External Trigger
      if(configPage2.analogInputPortSelection == ANALOG_EXVALVE) { readExValvePosition(); }  //[PJSC v1.02] For External Trigger

      //Check for launching/flat shift (clutch) can be done around here too
      previousClutchTrigger = clutchTrigger;
      if(configPage6.launchHiLo > 0) { clutchTrigger = digitalRead(pinLaunch); }
      else { clutchTrigger = !digitalRead(pinLaunch); }

      if(previousClutchTrigger != clutchTrigger) { currentStatus.clutchEngagedRPM = currentStatus.RPM; }

      if (configPage6.launchEnabled && clutchTrigger && (currentStatus.clutchEngagedRPM < ((unsigned int)(configPage6.flatSArm) * 100)) && (currentStatus.RPM > ((unsigned int)(configPage6.lnchHardLim) * 100)) && (currentStatus.TPS >= configPage10.lnchCtrlTPS) ) 
      { 
        //HardCut rev limit for 2-step launch control.
        currentStatus.launchingHard = true; 
        BIT_SET(currentStatus.spark, BIT_SPARK_HLAUNCH); 
      } 
      else { currentStatus.launchingHard = false; BIT_CLEAR(currentStatus.spark, BIT_SPARK_HLAUNCH); }

      if(configPage6.flatSEnable && clutchTrigger && (currentStatus.RPM > ((unsigned int)(configPage6.flatSArm) * 100)) && (currentStatus.RPM > currentStatus.clutchEngagedRPM) ) { currentStatus.flatShiftingHard = true; }
      else { currentStatus.flatShiftingHard = false; }

      //Boost cutoff is very similar to launchControl, but with a check against MAP rather than a switch
      if( (configPage6.boostCutType > 0) && (currentStatus.MAP > (configPage6.boostLimit * 2)) ) //The boost limit is divided by 2 to allow a limit up to 511kPa
      {
        switch(configPage6.boostCutType)
        {
          case 1:
            BIT_SET(currentStatus.spark, BIT_SPARK_BOOSTCUT);
            BIT_CLEAR(currentStatus.status1, BIT_STATUS1_BOOSTCUT);
            break;
          case 2:
            BIT_SET(currentStatus.status1, BIT_STATUS1_BOOSTCUT);
            BIT_CLEAR(currentStatus.spark, BIT_SPARK_BOOSTCUT);
            break;
          case 3:
            BIT_SET(currentStatus.spark, BIT_SPARK_BOOSTCUT);
            BIT_SET(currentStatus.status1, BIT_STATUS1_BOOSTCUT);
            break;
          default:
            //Shouldn't ever happen, but just in case, disable all cuts
            BIT_CLEAR(currentStatus.status1, BIT_STATUS1_BOOSTCUT);
            BIT_CLEAR(currentStatus.spark, BIT_SPARK_BOOSTCUT);
        }
      }
      else
      {
        BIT_CLEAR(currentStatus.spark, BIT_SPARK_BOOSTCUT);
        BIT_CLEAR(currentStatus.status1, BIT_STATUS1_BOOSTCUT);
      }

      //And check whether the tooth log buffer is ready
      if(toothHistoryIndex > TOOTH_LOG_SIZE) { BIT_SET(currentStatus.status1, BIT_STATUS1_TOOTHLOG1READY); }

    }
//[PJSC v1.10c]    if(BIT_CHECK(LOOP_TIMER, BIT_TIMER_30HZ)) //30 hertz
    if(BIT_CHECK(LOOP_TIMER, BIT_TIMER_30HZ) && (currentStatus.testOutputs == 0) )          //[PJSC v1.01]For MUX output test mode
    {
      BIT_CLEAR(TIMER_mask, BIT_TIMER_30HZ);
      //Most boost tends to run at about 30Hz, so placing it here ensures a new target time is fetched frequently enough
      boostControl();
    }
//[PJSC v1.10c]    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_4HZ))
    if ( BIT_CHECK(LOOP_TIMER, BIT_TIMER_4HZ) && (currentStatus.testOutputs == 0) )         //[PJSC v1.01]For MUX output test mode
    {
      BIT_CLEAR(TIMER_mask, BIT_TIMER_4HZ);
      //The IAT and CLT readings can be done less frequently (4 times per second)
      readCLT();
      readIAT();
      readO2();
//[PJSC v1.02]      readO2_2();
      readBat();
      nitrousControl();

      //*************** [PJSC v1.02] AFR sensor selection for tune analyze VE ********************
      if(configPage2.analogInputPortSelection == ANALOG_O2_SEC) { readO2_2(); }

      if(configPage2.afr_sensor_selection1) { currentStatus.afr_analyze1 = currentStatus.O2_2; }
      else { currentStatus.afr_analyze1 = currentStatus.O2; }

      if(configPage2.afr_sensor_selection2) { currentStatus.afr_analyze2 = currentStatus.O2_2; }
      else { currentStatus.afr_analyze2 = currentStatus.O2; }

      if(configPage2.afr_sensor_selection3) { currentStatus.afr_analyze3 = currentStatus.O2_2; }
      else { currentStatus.afr_analyze3 = currentStatus.O2; }

      if(configPage2.afr_sensor_selection4) { currentStatus.afr_analyze4 = currentStatus.O2_2; }
      else { currentStatus.afr_analyze4 = currentStatus.O2; }
      //*************** [PJSC v1.02] AFR sensor selection for tune analyze VE ********************

      if(eepromWritesPending == true) { writeAllConfig(); } //Check for any outstanding EEPROM writes.

      if(auxIsEnabled == true)
      {
        //TODO dazq to clean this right up :)
        //check through the Aux input channels if enabed for Can or local use
        for (byte AuxinChan = 0; AuxinChan <16 ; AuxinChan++)
        {
          currentStatus.current_caninchannel = AuxinChan;          
          
          if (((configPage9.caninput_sel[currentStatus.current_caninchannel]&12) == 4) 
              && (((configPage9.enable_secondarySerial == 1) && ((configPage9.enable_intcan == 0)&&(configPage9.intcan_available == 1)))
              || ((configPage9.enable_secondarySerial == 1) && ((configPage9.enable_intcan == 1)&&(configPage9.intcan_available == 1))&& 
              ((configPage9.caninput_sel[currentStatus.current_caninchannel]&64) == 0))
              || ((configPage9.enable_secondarySerial == 1) && ((configPage9.enable_intcan == 1)&&(configPage9.intcan_available == 0)))))              
          { //if current input channel is enabled as external & secondary serial enabled & internal can disabled(but internal can is available)
            // or current input channel is enabled as external & secondary serial enabled & internal can enabled(and internal can is available)
            //currentStatus.canin[13] = 11;  Dev test use only!
            if (configPage9.enable_secondarySerial == 1)  // megas only support can via secondary serial
            {
              sendCancommand(2,0,currentStatus.current_caninchannel,0,((configPage9.caninput_source_can_address[currentStatus.current_caninchannel]&2047)+0x100));
              //send an R command for data from caninput_source_address[currentStatus.current_caninchannel] from CANSERIAL
            }
          }  
          else if (((configPage9.caninput_sel[currentStatus.current_caninchannel]&12) == 4) 
              && (((configPage9.enable_secondarySerial == 1) && ((configPage9.enable_intcan == 1)&&(configPage9.intcan_available == 1))&& 
              ((configPage9.caninput_sel[currentStatus.current_caninchannel]&64) == 64))
              || ((configPage9.enable_secondarySerial == 0) && ((configPage9.enable_intcan == 1)&&(configPage9.intcan_available == 1))&& 
              ((configPage9.caninput_sel[currentStatus.current_caninchannel]&128) == 128))))                             
          { //if current input channel is enabled as external for canbus & secondary serial enabled & internal can enabled(and internal can is available)
            // or current input channel is enabled as external for canbus & secondary serial disabled & internal can enabled(and internal can is available)
            //currentStatus.canin[13] = 12;  Dev test use only!  
          #if defined(CORE_STM32) || defined(CORE_TEENSY)
           if (configPage9.enable_intcan == 1) //  if internal can is enabled 
           {
              sendCancommand(3,configPage9.speeduino_tsCanId,currentStatus.current_caninchannel,0,((configPage9.caninput_source_can_address[currentStatus.current_caninchannel]&2047)+0x100));  
              //send an R command for data from caninput_source_address[currentStatus.current_caninchannel] from internal canbus
           }
          #endif
          }   
          else if ((((configPage9.enable_secondarySerial == 1) || ((configPage9.enable_intcan == 1) && (configPage9.intcan_available == 1))) && (configPage9.caninput_sel[currentStatus.current_caninchannel]&12) == 8)
                  || (((configPage9.enable_secondarySerial == 0) && ( (configPage9.enable_intcan == 1) && (configPage9.intcan_available == 0) )) && (configPage9.caninput_sel[currentStatus.current_caninchannel]&3) == 2)  
                  || (((configPage9.enable_secondarySerial == 0) && (configPage9.enable_intcan == 0)) && ((configPage9.caninput_sel[currentStatus.current_caninchannel]&3) == 2)))  
          { //if current input channel is enabled as analog local pin
            //read analog channel specified
            //currentStatus.canin[13] = (configPage9.Auxinpina[currentStatus.current_caninchannel]&63);  Dev test use only!127
            currentStatus.canin[currentStatus.current_caninchannel] = readAuxanalog(configPage9.Auxinpina[currentStatus.current_caninchannel]&63);
          }
          else if ((((configPage9.enable_secondarySerial == 1) || ((configPage9.enable_intcan == 1) && (configPage9.intcan_available == 1))) && (configPage9.caninput_sel[currentStatus.current_caninchannel]&12) == 12)
                  || (((configPage9.enable_secondarySerial == 0) && ( (configPage9.enable_intcan == 1) && (configPage9.intcan_available == 0) )) && (configPage9.caninput_sel[currentStatus.current_caninchannel]&3) == 3)
                  || (((configPage9.enable_secondarySerial == 0) && (configPage9.enable_intcan == 0)) && ((configPage9.caninput_sel[currentStatus.current_caninchannel]&3) == 3)))
          { //if current input channel is enabled as digital local pin
            //read digital channel specified
            //currentStatus.canin[14] = ((configPage9.Auxinpinb[currentStatus.current_caninchannel]&63)+1);  Dev test use only!127+1
            currentStatus.canin[currentStatus.current_caninchannel] = readAuxdigital((configPage9.Auxinpinb[currentStatus.current_caninchannel]&63)+1);
          } //Channel type
        } //For loop going through each channel
      } //aux channels are enabled

      vvtControl();
      idleControl(); //Perform any idle related actions. Even at higher frequencies, running 4x per second is sufficient.
      if (configPage2.squirtDeviceType == 1) { pjscControl(); }  //[PJSC]
    } //4Hz timer
    if (BIT_CHECK(LOOP_TIMER, BIT_TIMER_1HZ)) //Once per second)
    {
      BIT_CLEAR(TIMER_mask, BIT_TIMER_1HZ);
      readBaro(); //Infrequent baro readings are not an issue.
    } //1Hz timer

    if ( currentStatus.testOutputs == 0 )         //[PJSC v1.01]For MUX output test mode
    {                                             //[PJSC v1.01]
      if( (configPage6.iacAlgorithm == IAC_ALGORITHM_STEP_OL) || (configPage6.iacAlgorithm == IAC_ALGORITHM_STEP_CL) )  { idleControl(); } //Run idlecontrol every loop for stepper idle.
    }                                             //[PJSC v1.01]

    //VE calculation was moved outside the sync/RPM check so that the fuel load value will be accurately shown when RPM=0
    currentStatus.VE = getVE();
    if ( configPage2.multiVEmapEnabled )                                          //[PJSC v1.01]Multi VE Map support
    {                                                                             //[PJSC v1.01]Multi VE Map support
      currentStatus.VE2 = getVE2();                                               //[PJSC v1.01]Multi VE Map support
      currentStatus.VE3 = getVE3();                                               //[PJSC v1.01]Multi VE Map support
      if ( configPage2.table4Usage == 0 ) { currentStatus.VE4 = getVE4(); }       //[PJSC v1.01]Multi VE Map support
    }                                                                             //[PJSC v1.01]Multi VE Map support

    //Always check for sync
    //Main loop runs within this clause
    if (currentStatus.hasSync && (currentStatus.RPM > 0))
    {
        if(currentStatus.startRevolutions >= configPage4.StgCycles)  { ignitionOn = true; fuelOn = true; } //Enable the fuel and ignition, assuming staging revolutions are complete
        //Check whether running or cranking
        if(currentStatus.RPM > currentStatus.crankRPM) //Crank RPM in the config is stored as a x10. currentStatus.crankRPM is set in timers.ino and represents the true value
        {
          BIT_SET(currentStatus.engine, BIT_ENGINE_RUN); //Sets the engine running bit
          //Only need to do anything if we're transitioning from cranking to running
          if( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) )
          {
            BIT_CLEAR(currentStatus.engine, BIT_ENGINE_CRANK);
            if(configPage4.ignBypassEnabled > 0) { digitalWrite(pinIgnBypass, HIGH); }
          }
        }
        else
        {  
          //Sets the engine cranking bit, clears the engine running bit
          BIT_SET(currentStatus.engine, BIT_ENGINE_CRANK);
          BIT_CLEAR(currentStatus.engine, BIT_ENGINE_RUN);
          currentStatus.runSecs = 0; //We're cranking (hopefully), so reset the engine run time to prompt ASE.
          if(configPage4.ignBypassEnabled > 0) { digitalWrite(pinIgnBypass, LOW); }
        }
      //END SETTING STATUSES
      //-----------------------------------------------------------------------------------------------------

      //Begin the fuel calculation
      //Calculate an injector pulsewidth from the VE
      currentStatus.corrections = correctionsFuel();

      currentStatus.advance = getAdvance();
//[PJSC]      currentStatus.PW1 = PW(req_fuel_uS, currentStatus.VE, currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);

      //[PJSC] Multi VE Map support **********************************************************************************************
      if(currentStatus.mapSelectSw) { tempVEvalue[0] = selectVE(currentStatus.veMapSelectionSw2Pri[0]); }
      else { tempVEvalue[0] = selectVE(currentStatus.veMapSelectionSw1Pri[0]); }

      if( configPage2.dualFuelEnabled )               // Dual Fuel Load
      {
        if(currentStatus.mapSelectSw) { dualFuelLoadVE = selectVE(currentStatus.veMapSelectionSw2Sec[0]); }
        else { dualFuelLoadVE = selectVE(currentStatus.veMapSelectionSw1Sec[0]); }

        dualFuelLoadVE += (unsigned int)tempVEvalue[0];
        if ( configPage2.secondaryFuelUsage )         // Additive
        {
          if( dualFuelLoadVE > 255 ) { dualFuelLoadVE = 255; }
        }
        else                                          // Multiply VE
        {
          dualFuelLoadVE = dualFuelLoadVE >> 1;
        }

        tempVEvalue[0] = (byte)dualFuelLoadVE;
      }

      currentStatus.PW1 = PW(req_fuel_uS, tempVEvalue[0], currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
      //[PJSC] Multi VE Map support **********************************************************************************************

      //Manual adder for nitrous. These are not in correctionsFuel() because they are direct adders to the ms value, not % based
      if(currentStatus.nitrous_status == NITROUS_STAGE1)
      { 
        int16_t adderRange = (configPage10.n2o_stage1_maxRPM - configPage10.n2o_stage1_minRPM) * 100;
        int16_t adderPercent = ((currentStatus.RPM - (configPage10.n2o_stage1_minRPM * 100)) * 100) / adderRange; //The percentage of the way through the RPM range
        adderPercent = 100 - adderPercent; //Flip the percentage as we go from a higher adder to a lower adder as the RPMs rise
        currentStatus.PW1 = currentStatus.PW1 + (configPage10.n2o_stage1_adderMax + percentage(adderPercent, (configPage10.n2o_stage1_adderMin - configPage10.n2o_stage1_adderMax))) * 100; //Calculate the above percentage of the calculated ms value.
      }
      if(currentStatus.nitrous_status == NITROUS_STAGE2)
      {
        int16_t adderRange = (configPage10.n2o_stage2_maxRPM - configPage10.n2o_stage2_minRPM) * 100;
        int16_t adderPercent = ((currentStatus.RPM - (configPage10.n2o_stage2_minRPM * 100)) * 100) / adderRange; //The percentage of the way through the RPM range
        adderPercent = 100 - adderPercent; //Flip the percentage as we go from a higher adder to a lower adder as the RPMs rise
        currentStatus.PW1 = currentStatus.PW1 + (configPage10.n2o_stage2_adderMax + percentage(adderPercent, (configPage10.n2o_stage2_adderMin - configPage10.n2o_stage2_adderMax))) * 100; //Calculate the above percentage of the calculated ms value.
      }

      int injector1StartAngle = 0;
      uint16_t injector2StartAngle = 0;
      uint16_t injector3StartAngle = 0;
      uint16_t injector4StartAngle = 0;
      uint16_t injector5StartAngle = 0; //For 5 cylinder testing
#if INJ_CHANNELS >= 6
      int injector6StartAngle = 0;
#endif
#if INJ_CHANNELS >= 7
      int injector7StartAngle = 0;
#endif
#if INJ_CHANNELS >= 8
      int injector8StartAngle = 0;
#endif
      int ignition1StartAngle = 0;
      int ignition2StartAngle = 0;
      int ignition3StartAngle = 0;
      int ignition4StartAngle = 0;
      int ignition5StartAngle = 0;
#if IGN_CHANNELS >= 6
      int ignition6StartAngle = 0;
#endif
#if IGN_CHANNELS >= 7
      int ignition7StartAngle = 0;
#endif
#if IGN_CHANNELS >= 8
      int ignition8StartAngle = 0;
#endif
      //These are used for comparisons on channels above 1 where the starting angle (for injectors or ignition) can be less than a single loop time
      //(Don't ask why this is needed, it's just there)
      int tempCrankAngle;
      int tempStartAngle;

      doCrankSpeedCalcs(); //In crankMaths.ino

      //Check that the duty cycle of the chosen pulsewidth isn't too high.
      unsigned long pwLimit = percentage(configPage2.dutyLim, revolutionTime); //The pulsewidth limit is determined to be the duty cycle limit (Eg 85%) by the total time it takes to perform 1 revolution
      if (CRANK_ANGLE_MAX_INJ == 720) { pwLimit = pwLimit * 2; } //For sequential, the maximum pulse time is double (2 revolutions). Wouldn't work for 2 stroke...
      else if (CRANK_ANGLE_MAX_INJ < 360) { pwLimit = pwLimit / currentStatus.nSquirts; } //Handle cases where there are multiple squirts per rev
      //Apply the pwLimit if staging is dsiabled and engine is not cranking
      if( (!BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK)) && (configPage10.stagingEnabled == false) ) { if (currentStatus.PW1 > pwLimit) { currentStatus.PW1 = pwLimit; } }

      //Calculate staging pulsewidths if used
      //To run staged injection, the number of cylinders must be less than or equal to the injector channels (ie Assuming you're running paired injection, you need at least as many injector channels as you have cylinders, half for the primaries and half for the secondaries)
      if( (configPage10.stagingEnabled == true) && (configPage2.nCylinders <= INJ_CHANNELS) )
      {
        //Scale the 'full' pulsewidth by each of the injector capacities
        currentStatus.PW1 -= inj_opentime_uS; //Subtract the opening time from PW1 as it needs to be multiplied out again by the pri/sec req_fuel values below. It is added on again after that calculation. 
        uint32_t tempPW1 = (((unsigned long)currentStatus.PW1 * staged_req_fuel_mult_pri) / 100) + inj_opentime_uS; //Opening time has to be added back on here (See above where it is subtracted)

        if(configPage10.stagingMode == STAGING_MODE_TABLE)
        {
          uint32_t tempPW3 = (((unsigned long)currentStatus.PW1 * staged_req_fuel_mult_sec) / 100) + inj_opentime_uS; //This is ONLY needed in in table mode. Auto mode only calculates the difference. As above, opening time must be readded. 

          byte stagingSplit = get3DTableValue(&stagingTable, currentStatus.MAP, currentStatus.RPM);
          currentStatus.PW1 = ((100 - stagingSplit) * tempPW1) / 100;

          if(stagingSplit > 0) { currentStatus.PW3 = (stagingSplit * tempPW3) / 100; }
          else { currentStatus.PW3 = 0; }
        }
        else if(configPage10.stagingMode == STAGING_MODE_AUTO)
        {
          currentStatus.PW1 = tempPW1;
          //If automatic mode, the primary injectors are used all the way up to their limit (Configured by the pulsewidth limit setting)
          //If they exceed their limit, the extra duty is passed to the secondaries
          if(tempPW1 > pwLimit)
          {
            uint32_t extraPW = tempPW1 - pwLimit;
            currentStatus.PW1 = pwLimit;
            currentStatus.PW3 = ((extraPW * staged_req_fuel_mult_sec) / staged_req_fuel_mult_pri) + inj_opentime_uS; //Convert the 'left over' fuel amount from primary injector scaling to secondary
          }
          else { currentStatus.PW3 = 0; } //If tempPW1 < pwLImit it means that the entire fuel load can be handled by the primaries. Simply set the secondaries to 0
        }

        //Set the 2nd channel of each stage with the same pulseWidth
        currentStatus.PW2 = currentStatus.PW1;
        currentStatus.PW4 = currentStatus.PW3;
      }
      else 
      { 
        //If staging is off, all the pulse widths are set the same (Sequential and other adjustments may be made below)
        currentStatus.PW2 = currentStatus.PW1;
        currentStatus.PW3 = currentStatus.PW1;
        currentStatus.PW4 = currentStatus.PW1;
        currentStatus.PW5 = currentStatus.PW1;
        currentStatus.PW6 = currentStatus.PW1;
        currentStatus.PW7 = currentStatus.PW1; 

        //[PJSC] Multi VE Map support **********************************************************************************************
        if ( configPage2.multiVEmapEnabled && configPage2.mapSeparationEnabled )
        {
          for (unsigned int numVe = 0; numVe < MULTI_VE_COUNT; numVe++)
          {
            if(currentStatus.mapSelectSw) { tempVEvalue[numVe] = selectVE(currentStatus.veMapSelectionSw2Pri[numVe]); }
            else { tempVEvalue[numVe] = selectVE(currentStatus.veMapSelectionSw1Pri[numVe]); }

            if( configPage2.dualFuelEnabled )               // Dual Fuel Load
            {
              if(currentStatus.mapSelectSw) { dualFuelLoadVE = selectVE(currentStatus.veMapSelectionSw2Sec[numVe]); }
              else { dualFuelLoadVE = selectVE(currentStatus.veMapSelectionSw1Sec[numVe]); }

              dualFuelLoadVE += (unsigned int)tempVEvalue[numVe];
              if ( configPage2.secondaryFuelUsage )         // Additive
              {
                if( dualFuelLoadVE > 255 ) { dualFuelLoadVE = 255; }
              }
              else                                          // Multiply VE
              {
                dualFuelLoadVE = dualFuelLoadVE >> 1;
              }

              tempVEvalue[numVe] = (byte)dualFuelLoadVE;
            }
          }

          currentStatus.PW1 = PW(req_fuel_uS, tempVEvalue[0], currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
          currentStatus.PW2 = PW(req_fuel_uS, tempVEvalue[1], currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
          currentStatus.PW3 = PW(req_fuel_uS, tempVEvalue[2], currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
          currentStatus.PW4 = PW(req_fuel_uS, tempVEvalue[3], currentStatus.MAP, currentStatus.corrections, inj_opentime_uS);
        }
        //[PJSC] Multi VE Map support **********************************************************************************************
      }

      //***********************************************************************************************
      //BEGIN INJECTION TIMING
      //Determine next firing angles
      if(!configPage2.indInjAng) 
      {
        //Forcing all injector close angles to be the same.
        configPage2.inj2Ang = configPage2.inj1Ang;
        configPage2.inj3Ang = configPage2.inj1Ang;
        configPage2.inj4Ang = configPage2.inj1Ang;
      } 
      unsigned int PWdivTimerPerDegree = div(currentStatus.PW1, timePerDegree).quot; //How many crank degrees the calculated PW will take at the current speed
      //This is a little primitive, but is based on the idea that all fuel needs to be delivered before the inlet valve opens. See http://www.extraefi.co.uk/sequential_fuel.html for more detail
      if(configPage2.inj1SquirtStartEnd) { injector1StartAngle = configPage2.inj1Ang; }                                            //[PJSC v1.01]
      else                                                                                                                         //[PJSC v1.01]
      {                                                                                                                            //[PJSC v1.01]
        if(configPage2.inj1Ang > PWdivTimerPerDegree) { injector1StartAngle = configPage2.inj1Ang - ( PWdivTimerPerDegree ); }
        else { injector1StartAngle = configPage2.inj1Ang + CRANK_ANGLE_MAX_INJ - PWdivTimerPerDegree; } //Just incase 
      }                                                                                                                            //[PJSC v1.01]
//[PJSC v1.02]      if(injector1StartAngle > CRANK_ANGLE_MAX_INJ) { injector1StartAngle -= CRANK_ANGLE_MAX_INJ; }
      while(injector1StartAngle > CRANK_ANGLE_MAX_INJ) { injector1StartAngle -= CRANK_ANGLE_MAX_INJ; }                             //[PJSC v1.02]

      //Repeat the above for each cylinder
      switch (configPage2.nCylinders)
      {
        //Single cylinder
        case 1:
          //The only thing that needs to be done for single cylinder is to check for staging. 
          if( (configPage10.stagingEnabled == true) && (currentStatus.PW3 > 0) )
          {
            PWdivTimerPerDegree = div(currentStatus.PW3, timePerDegree).quot; //Need to redo this for PW3 as it will be dramatically different to PW1 when staging
            injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);
          }
          break;
        //2 cylinders
        case 2:
          injector2StartAngle = calculateInjector2StartAngle(PWdivTimerPerDegree);

          if( (configPage2.injLayout == INJ_SEMISEQUENTIAL) || (configPage2.injLayout == INJ_SEQUENTIAL) )       //[PJSC v1.02]
          {                                                                                                      // |
            injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);                             // |
            injector4StartAngle = calculateInjector4StartAngle(PWdivTimerPerDegree);                             // |
          }                                                                                                      // V
          else if( (configPage10.stagingEnabled == true) && (currentStatus.PW3 > 0) )                            //[PJSC v1.02]
//[PJSC v1.02]          if( (configPage10.stagingEnabled == true) && (currentStatus.PW3 > 0) )
          {
            PWdivTimerPerDegree = div(currentStatus.PW3, timePerDegree).quot; //Need to redo this for PW3 as it will be dramatically different to PW1 when staging
            injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);

            injector4StartAngle = injector3StartAngle + (CRANK_ANGLE_MAX_INJ / 2); //Phase this either 180 or 360 degrees out from inj3 (In reality this will always be 180 as you can't have sequential and staged currently)
            if(injector4StartAngle < 0) {injector4StartAngle += CRANK_ANGLE_MAX_INJ;}
            if(injector4StartAngle > (uint16_t)CRANK_ANGLE_MAX_INJ) { injector4StartAngle -= CRANK_ANGLE_MAX_INJ; }
          }
          break;
        //3 cylinders
        case 3:
          injector2StartAngle = calculateInjector2StartAngle(PWdivTimerPerDegree);
          injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);
          break;
        //4 cylinders
        case 4:
          injector2StartAngle = calculateInjector2StartAngle(PWdivTimerPerDegree);

          if(configPage2.injLayout == INJ_SEQUENTIAL)
          {
            injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);
            injector4StartAngle = calculateInjector4StartAngle(PWdivTimerPerDegree);

//[PJSC v1.01]            if(configPage6.fuelTrimEnabled > 0)
//[PJSC v1.01]            {
//[PJSC v1.01]              unsigned long pw1percent = 100 + (byte)get3DTableValue(&trim1Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
//[PJSC v1.01]              unsigned long pw2percent = 100 + (byte)get3DTableValue(&trim2Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
//[PJSC v1.01]              unsigned long pw3percent = 100 + (byte)get3DTableValue(&trim3Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
//[PJSC v1.01]              unsigned long pw4percent = 100 + (byte)get3DTableValue(&trim4Table, currentStatus.MAP, currentStatus.RPM) - OFFSET_FUELTRIM;
//[PJSC v1.01]
//[PJSC v1.01]              if (pw1percent != 100) { currentStatus.PW1 = (pw1percent * currentStatus.PW1) / 100; }
//[PJSC v1.01]              if (pw2percent != 100) { currentStatus.PW2 = (pw2percent * currentStatus.PW2) / 100; }
//[PJSC v1.01]              if (pw3percent != 100) { currentStatus.PW3 = (pw3percent * currentStatus.PW3) / 100; }
//[PJSC v1.01]              if (pw4percent != 100) { currentStatus.PW4 = (pw4percent * currentStatus.PW4) / 100; }
//[PJSC v1.01]            }
          }
          else if( (configPage10.stagingEnabled == true) && (currentStatus.PW3 > 0) )
          {
            PWdivTimerPerDegree = div(currentStatus.PW3, timePerDegree).quot; //Need to redo this for PW3 as it will be dramatically different to PW1 when staging
            injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);

            injector4StartAngle = injector3StartAngle + (CRANK_ANGLE_MAX_INJ / 2); //Phase this either 180 or 360 degrees out from inj3 (In reality this will always be 180 as you can't have sequential and staged currently)
            if(injector4StartAngle < 0) {injector4StartAngle += CRANK_ANGLE_MAX_INJ;}
            if(injector4StartAngle > (uint16_t)CRANK_ANGLE_MAX_INJ) { injector4StartAngle -= CRANK_ANGLE_MAX_INJ; }
          }
          break;
        //5 cylinders
        case 5:
          injector2StartAngle = calculateInjector2StartAngle(PWdivTimerPerDegree); //Note the shared timing with INJ2
          injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);
          injector4StartAngle = calculateInjector4StartAngle(PWdivTimerPerDegree);
          injector5StartAngle = calculateInjector5StartAngle(PWdivTimerPerDegree);
          break;
        //6 cylinders
        case 6:
          injector2StartAngle = calculateInjector2StartAngle(PWdivTimerPerDegree);
          injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);
#if INJ_CHANNELS >= 6
          if(configPage2.injLayout == INJ_SEQUENTIAL)
          {
            injector4StartAngle = (configPage2.inj1Ang + channel4InjDegrees - ( PWdivTimerPerDegree ));
            if(injector4StartAngle > CRANK_ANGLE_MAX_INJ) {injector4StartAngle -= CRANK_ANGLE_MAX_INJ;}
            injector5StartAngle = (configPage2.inj2Ang + channel5InjDegrees - ( PWdivTimerPerDegree ));
            if(injector5StartAngle > CRANK_ANGLE_MAX_INJ) {injector5StartAngle -= CRANK_ANGLE_MAX_INJ;}
            injector6StartAngle = (configPage2.inj3Ang + channel6InjDegrees - ( PWdivTimerPerDegree ));
            if(injector6StartAngle > CRANK_ANGLE_MAX_INJ) {injector6StartAngle -= CRANK_ANGLE_MAX_INJ;}
          }
#endif
          break;
        //8 cylinders
        case 8:
          injector2StartAngle = calculateInjector2StartAngle(PWdivTimerPerDegree);
          injector3StartAngle = calculateInjector3StartAngle(PWdivTimerPerDegree);
          injector4StartAngle = calculateInjector4StartAngle(PWdivTimerPerDegree);
          break;

        //Will hit the default case on 1 cylinder or >8 cylinders. Do nothing in these cases
        default:
          break;
      }

      //***********************************************************************************************
      //| BEGIN IGNITION CALCULATIONS
      if (currentStatus.RPM > ((unsigned int)(configPage4.HardRevLim) * 100) ) { BIT_SET(currentStatus.spark, BIT_SPARK_HRDLIM); } //Hardcut RPM limit
      else { BIT_CLEAR(currentStatus.spark, BIT_SPARK_HRDLIM); }


      //Set dwell
      //Dwell is stored as ms * 10. ie Dwell of 4.3ms would be 43 in configPage4. This number therefore needs to be multiplied by 100 to get dwell in uS
      if ( BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) ) { currentStatus.dwell =  (configPage4.dwellCrank * 100); }
      else { currentStatus.dwell =  (configPage4.dwellRun * 100); }
      currentStatus.dwell = correctionsDwell(currentStatus.dwell);

      int dwellAngle = timeToAngle(currentStatus.dwell, CRANKMATH_METHOD_INTERVAL_REV); //Convert the dwell time to dwell angle based on the current engine speed

      //Calculate start angle for each channel
      //1 cylinder (Everyone gets this)
      ignition1EndAngle = CRANK_ANGLE_MAX_IGN - currentStatus.advance;
      if(ignition1EndAngle > CRANK_ANGLE_MAX_IGN) {ignition1EndAngle -= CRANK_ANGLE_MAX_IGN;}
      ignition1StartAngle = ignition1EndAngle - dwellAngle; // 360 - desired advance angle - number of degrees the dwell will take
      if(ignition1StartAngle < 0) {ignition1StartAngle += CRANK_ANGLE_MAX_IGN;}

      //This test for more cylinders and do the same thing
      switch (configPage2.nCylinders)
      {
        //2 cylinders
        case 2:
          ignition2EndAngle = channel2IgnDegrees - currentStatus.advance;
          if(ignition2EndAngle > CRANK_ANGLE_MAX_IGN) {ignition2EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition2StartAngle = ignition2EndAngle - dwellAngle;
          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}
          break;
        //3 cylinders
        case 3:
          ignition2EndAngle = channel2IgnDegrees - currentStatus.advance;
          if(ignition2EndAngle > CRANK_ANGLE_MAX_IGN) {ignition2EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition2StartAngle = ignition2EndAngle - dwellAngle;
          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}

          ignition3EndAngle = channel3IgnDegrees - currentStatus.advance;
          if(ignition3EndAngle > CRANK_ANGLE_MAX_IGN) {ignition3EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition3StartAngle = channel3IgnDegrees - dwellAngle;
          if(ignition3StartAngle < 0) {ignition3StartAngle += CRANK_ANGLE_MAX_IGN;}
          break;
        //4 cylinders
        case 4:
          ignition2EndAngle = channel2IgnDegrees - currentStatus.advance;
          if(ignition2EndAngle > CRANK_ANGLE_MAX_IGN) {ignition2EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition2StartAngle = ignition2EndAngle - dwellAngle;
          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}

          if(configPage4.sparkMode == IGN_MODE_SEQUENTIAL)
          {
            ignition3EndAngle = channel3IgnDegrees - currentStatus.advance;
            if(ignition3EndAngle > CRANK_ANGLE_MAX_IGN) {ignition3EndAngle -= CRANK_ANGLE_MAX_IGN;}
            ignition3StartAngle = ignition3EndAngle - dwellAngle;
            if(ignition3StartAngle < 0) {ignition3StartAngle += CRANK_ANGLE_MAX_IGN;}

            ignition4EndAngle = channel4IgnDegrees - currentStatus.advance;
            if(ignition4EndAngle > CRANK_ANGLE_MAX_IGN) {ignition4EndAngle -= CRANK_ANGLE_MAX_IGN;}
            ignition4StartAngle = ignition4EndAngle - dwellAngle;
            if(ignition4StartAngle < 0) {ignition4StartAngle += CRANK_ANGLE_MAX_IGN;}
          }
          else if(configPage4.sparkMode == IGN_MODE_ROTARY)
          {
            if(configPage10.rotaryType == ROTARY_IGN_FC)
            {
              byte splitDegrees = 0;
              if (configPage2.fuelAlgorithm == LOAD_SOURCE_MAP) { splitDegrees = table2D_getValue(&rotarySplitTable, currentStatus.MAP/2); }
              else { splitDegrees = table2D_getValue(&rotarySplitTable, currentStatus.TPS/2); }

              //The trailing angles are set relative to the leading ones
              ignition3EndAngle = ignition1EndAngle + splitDegrees;
              ignition3StartAngle = ignition3EndAngle - dwellAngle;
              if(ignition3StartAngle > CRANK_ANGLE_MAX_IGN) {ignition3StartAngle -= CRANK_ANGLE_MAX_IGN;}
              if(ignition3StartAngle < 0) {ignition3StartAngle += CRANK_ANGLE_MAX_IGN;}

              ignition4EndAngle = ignition2EndAngle + splitDegrees;
              ignition4StartAngle = ignition4EndAngle - dwellAngle;
              if(ignition4StartAngle > CRANK_ANGLE_MAX_IGN) {ignition4StartAngle -= CRANK_ANGLE_MAX_IGN;}
              if(ignition4StartAngle < 0) {ignition4StartAngle += CRANK_ANGLE_MAX_IGN;}
            }
          }
          break;
        //5 cylinders
        case 5:
          ignition2EndAngle = channel2IgnDegrees - currentStatus.advance;
          if(ignition2EndAngle > CRANK_ANGLE_MAX_IGN) {ignition2EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition2StartAngle = ignition2EndAngle - dwellAngle;
          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}

          ignition3EndAngle = channel3IgnDegrees - currentStatus.advance;
          if(ignition3EndAngle > CRANK_ANGLE_MAX_IGN) {ignition3EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition3StartAngle = ignition3EndAngle - dwellAngle;
          if(ignition3StartAngle < 0) {ignition3StartAngle += CRANK_ANGLE_MAX_IGN;}

          ignition4EndAngle = channel4IgnDegrees - currentStatus.advance;
          if(ignition4EndAngle > CRANK_ANGLE_MAX_IGN) {ignition4EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition4StartAngle = ignition4EndAngle - dwellAngle;
          if(ignition4StartAngle < 0) {ignition4StartAngle += CRANK_ANGLE_MAX_IGN;}

          ignition5EndAngle = channel5IgnDegrees - currentStatus.advance - dwellAngle;
          if(ignition5EndAngle > CRANK_ANGLE_MAX_IGN) {ignition5EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition5StartAngle = ignition5EndAngle - dwellAngle;
          if(ignition5StartAngle < 0) {ignition5StartAngle += CRANK_ANGLE_MAX_IGN;}

          break;
        //6 cylinders
        case 6:
          ignition2EndAngle = channel2IgnDegrees - currentStatus.advance;
          if(ignition2EndAngle > CRANK_ANGLE_MAX_IGN) {ignition2EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition2StartAngle = ignition2EndAngle - dwellAngle;
          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}

          ignition3EndAngle = channel3IgnDegrees - currentStatus.advance;
          if(ignition3EndAngle > CRANK_ANGLE_MAX_IGN) {ignition3EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition3StartAngle = ignition3EndAngle - dwellAngle;
          if(ignition3StartAngle < 0) {ignition3StartAngle += CRANK_ANGLE_MAX_IGN;}
          break;
        //8 cylinders
        case 8:
          ignition2EndAngle = channel2IgnDegrees - currentStatus.advance;
          if(ignition2EndAngle > CRANK_ANGLE_MAX_IGN) {ignition2EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition2StartAngle = ignition2EndAngle - dwellAngle;
          if(ignition2StartAngle < 0) {ignition2StartAngle += CRANK_ANGLE_MAX_IGN;}

          ignition3EndAngle = channel3IgnDegrees - currentStatus.advance;
          if(ignition3EndAngle > CRANK_ANGLE_MAX_IGN) {ignition3EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition3StartAngle = ignition3EndAngle - dwellAngle;
          if(ignition3StartAngle < 0) {ignition3StartAngle += CRANK_ANGLE_MAX_IGN;}

          ignition4EndAngle = channel4IgnDegrees - currentStatus.advance;
          if(ignition4EndAngle > CRANK_ANGLE_MAX_IGN) {ignition4EndAngle -= CRANK_ANGLE_MAX_IGN;}
          ignition4StartAngle = ignition4EndAngle - dwellAngle;
          if(ignition4StartAngle < 0) {ignition4StartAngle += CRANK_ANGLE_MAX_IGN;}
          break;

        //Will hit the default case on 1 cylinder or >8 cylinders. Do nothing in these cases
        default:
          break;
      }
      //If ignition timing is being tracked per tooth, perform the calcs to get the end teeth
      //This only needs to be run if the advance figure has changed, otherwise the end teeth will still be the same
      if( (configPage2.perToothIgn == true) && (lastToothCalcAdvance != currentStatus.advance) ) { triggerSetEndTeeth(); }

      //***********************************************************************************************
      //| BEGIN FUEL SCHEDULES
      //Finally calculate the time (uS) until we reach the firing angles and set the schedules
      //We only need to set the shcedule if we're BEFORE the open angle
      //This may potentially be called a number of times as we get closer and closer to the opening time

      //Determine the current crank angle
      int crankAngle = getCrankAngle();
      while(crankAngle > CRANK_ANGLE_MAX_INJ ) { crankAngle = crankAngle - CRANK_ANGLE_MAX_INJ; } //Continue reducing the crank angle by the max injection amount until it's below the required limit. This will usually only run (at most) once, but in cases where there is sequential ignition and more than 2 squirts per cycle, it may run up to 4 times. 

      if(Serial && false)
      {
        if(ignition1StartAngle > crankAngle)
        {
          noInterrupts();
          Serial.print("Time2LastTooth:"); Serial.println(micros()-toothLastToothTime);
          Serial.print("elapsedTime:"); Serial.println(elapsedTime);
          Serial.print("CurAngle:"); Serial.println(crankAngle);
          Serial.print("RPM:"); Serial.println(currentStatus.RPM);
          Serial.print("Tooth:"); Serial.println(toothCurrentCount);
          Serial.print("timePerDegree:"); Serial.println(timePerDegree);
          Serial.print("IGN1Angle:"); Serial.println(ignition1StartAngle);
          Serial.print("TimeToIGN1:"); Serial.println(angleToTime((ignition1StartAngle - crankAngle), CRANKMATH_METHOD_INTERVAL_REV));
          interrupts();
        }
      }

#if INJ_CHANNELS >= 1
      if( configPage2.squirtDeviceType == 1 )                      //[PJSC]
      {                                                            //[PJSC]
        pjscControl();                                             //[PJSC]
      }                                                            //[PJSC]
      else{                                                        //[PJSC]
        if (fuelOn && !BIT_CHECK(currentStatus.status1, BIT_STATUS1_BOOSTCUT))
        {
          if(currentStatus.PW1 >= inj_opentime_uS)
          {
            if ( (injector1StartAngle <= crankAngle) && (fuelSchedule1.Status == RUNNING) ) { injector1StartAngle += CRANK_ANGLE_MAX_INJ; }
            if (injector1StartAngle > crankAngle)
            {
              setFuelSchedule1(
                        ((injector1StartAngle - crankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW1
                        );
            }
          }
#endif

        /*-----------------------------------------------------------------------------------------
        | A Note on tempCrankAngle and tempStartAngle:
        |   The use of tempCrankAngle/tempStartAngle is described below. It is then used in the same way for channels 2, 3 and 4 on both injectors and ignition
        |   Essentially, these 2 variables are used to realign the current crank angle and the desired start angle around 0 degrees for the given cylinder/output
        |   Eg: If cylinder 2 TDC is 180 degrees after cylinder 1 (Eg a standard 4 cylidner engine), then tempCrankAngle is 180* less than the current crank angle and
        |       tempStartAngle is the desired open time less 180*. Thus the cylinder is being treated relative to its own TDC, regardless of its offset
        |
        |   This is done to avoid problems with very short of very long times until tempStartAngle.
        |   This will very likely need to be rewritten when sequential is enabled
        |------------------------------------------------------------------------------------------
        */
#if INJ_CHANNELS >= 2
          if( (channel2InjEnabled) && (currentStatus.PW2 >= inj_opentime_uS) )
          {
            tempCrankAngle = crankAngle - channel2InjDegrees;
            if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
            tempStartAngle = injector2StartAngle - channel2InjDegrees;
            if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule2.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( tempStartAngle > tempCrankAngle )
            {
              setFuelSchedule2(
                        ((tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW2
                        );
            }
          }
#endif

#if INJ_CHANNELS >= 3
          if( (channel3InjEnabled) && (currentStatus.PW3 >= inj_opentime_uS) )
          {
            tempCrankAngle = crankAngle - channel3InjDegrees;
            if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
            tempStartAngle = injector3StartAngle - channel3InjDegrees;
            if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule3.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( tempStartAngle > tempCrankAngle )
            {
              setFuelSchedule3(
                        ((tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW3
                        );
            }
          }
#endif

#if INJ_CHANNELS >= 4
          if( (channel4InjEnabled) && (currentStatus.PW4 >= inj_opentime_uS) )
          {
            tempCrankAngle = crankAngle - channel4InjDegrees;
            if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
            tempStartAngle = injector4StartAngle - channel4InjDegrees;
            if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule4.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( tempStartAngle > tempCrankAngle )
            {
              setFuelSchedule4(
                        ((tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW4
                        );
            }
          }
#endif

#if INJ_CHANNELS >= 5
          if( (channel5InjEnabled) && (currentStatus.PW4 >= inj_opentime_uS) )
          {
            tempCrankAngle = crankAngle - channel5InjDegrees;
            if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
            tempStartAngle = injector5StartAngle - channel5InjDegrees;
            if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if (tempStartAngle <= tempCrankAngle && fuelSchedule5.schedulesSet == 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( tempStartAngle > tempCrankAngle )
            {
              //Note the hacky use of fuel schedule 3 below
              /*
              setFuelSchedule3(openInjector3and5,
                        ((unsigned long)(tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW1,
                        closeInjector3and5
                      );*/
              setFuelSchedule3(
                        ((tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW1
                        );
            }
          }
#endif

#if INJ_CHANNELS >= 6
          if( (channel6InjEnabled) && (currentStatus.PW6 >= inj_opentime_uS) )
          {
            tempCrankAngle = crankAngle - channel6InjDegrees;
            if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
            tempStartAngle = injector6StartAngle - channel6InjDegrees;
            if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule6.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( tempStartAngle > tempCrankAngle )
            {
              setFuelSchedule6(
                        ((tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW6
                        );
            }
          }
#endif

#if INJ_CHANNELS >= 7
          if( (channel7InjEnabled) && (currentStatus.PW7 >= inj_opentime_uS) )
          {
            tempCrankAngle = crankAngle - channel7InjDegrees;
            if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
            tempStartAngle = injector7StartAngle - channel7InjDegrees;
            if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule7.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( tempStartAngle > tempCrankAngle )
            {
              setFuelSchedule7(
                        ((tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW7
                        );
            }
          }
#endif

#if INJ_CHANNELS >= 8
          if( (channel8InjEnabled) && (currentStatus.PW8 >= inj_opentime_uS) )
          {
            tempCrankAngle = crankAngle - channel8InjDegrees;
            if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_INJ; }
            tempStartAngle = injector8StartAngle - channel8InjDegrees;
            if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( (tempStartAngle <= tempCrankAngle) && (fuelSchedule8.Status == RUNNING) ) { tempStartAngle += CRANK_ANGLE_MAX_INJ; }
            if ( tempStartAngle > tempCrankAngle )
            {
              setFuelSchedule8(
                        ((tempStartAngle - tempCrankAngle) * (unsigned long)timePerDegree),
                        (unsigned long)currentStatus.PW8
                        );
            }
          }
#endif
        }
      }  //[PJSC]
      //***********************************************************************************************
      //| BEGIN IGNITION SCHEDULES
      //Same as above, except for ignition

      //fixedCrankingOverride is used to extend the dwell during cranking so that the decoder can trigger the spark upon seeing a certain tooth. Currently only available on the basic distributor and 4g63 decoders.
      if ( configPage4.ignCranklock && BIT_CHECK(currentStatus.engine, BIT_ENGINE_CRANK) && (decoderHasFixedCrankingTiming == true) )
      {
        fixedCrankingOverride = currentStatus.dwell * 3;
        //This is a safety step to prevent the ignition start time occuring AFTER the target tooth pulse has already occcured. It simply moves the start time forward a little, which is compensated for by the increase in the dwell time
        if(currentStatus.RPM < 250)
        {
          ignition1StartAngle -= 5;
          ignition2StartAngle -= 5;
          ignition3StartAngle -= 5;
          ignition4StartAngle -= 5;
        }
      }
      else { fixedCrankingOverride = 0; }

      //Perform an initial check to see if the ignition is turned on (Ignition only turns on after a preset number of cranking revolutions and:
      //Check for any of the hard cut rev limits being on
      if(currentStatus.launchingHard || BIT_CHECK(currentStatus.spark, BIT_SPARK_BOOSTCUT) || BIT_CHECK(currentStatus.spark, BIT_SPARK_HRDLIM) || currentStatus.flatShiftingHard)
      {
        if(configPage2.hardCutType == HARD_CUT_FULL) { ignitionOn = false; }
        else 
        { 
          if(rollingCutCounter >= 2) //Vary this number to change the intensity of the roll. The higher the number, the closer is it to full cut
          { 
            //Rolls through each of the active ignition channels based on how many revolutions have taken place
            //curRollingCut = ( (currentStatus.startRevolutions / 2) % maxIgnOutputs) + 1;
            rollingCutCounter = 0;
            ignitionOn = true;
            curRollingCut = 0;
          }
          else
          {
            if(rollingCutLastRev == 0) { rollingCutLastRev = currentStatus.startRevolutions; } //
            if (rollingCutLastRev != currentStatus.startRevolutions)
            {
              rollingCutLastRev = currentStatus.startRevolutions;
              rollingCutCounter++;
            }
            ignitionOn = false; //Finally the ignition is fully cut completely
          }
        } 
      }
      else { curRollingCut = 0; } //Disables the rolling hard cut

      //if(ignitionOn && !currentStatus.launchingHard && !BIT_CHECK(currentStatus.spark, BIT_SPARK_BOOSTCUT) && !BIT_CHECK(currentStatus.spark, BIT_SPARK_HRDLIM) && !currentStatus.flatShiftingHard)
      if(ignitionOn)
      {
        //Refresh the current crank angle info
        //ignition1StartAngle = 335;
        crankAngle = getCrankAngle(); //Refresh with the latest crank angle
        if (crankAngle > CRANK_ANGLE_MAX_IGN ) { crankAngle -= 360; }

#if IGN_CHANNELS >= 1
        if ( (ignition1StartAngle > crankAngle) && (curRollingCut != 1) )
        {
            if(ignitionSchedule1.Status != RUNNING)
            {
              setIgnitionSchedule1(ign1StartFunction,
                        //((unsigned long)(ignition1StartAngle - crankAngle) * (unsigned long)timePerDegree),
                        angleToTime((ignition1StartAngle - crankAngle), CRANKMATH_METHOD_INTERVAL_REV),
                        currentStatus.dwell + fixedCrankingOverride, //((unsigned long)((unsigned long)currentStatus.dwell* currentStatus.RPM) / newRPM) + fixedCrankingOverride,
                        ign1EndFunction
                        );
            }
        }
#endif

#if defined(USE_IGN_REFRESH)
        if( (ignitionSchedule1.Status == RUNNING) && (ignition1EndAngle > crankAngle) && (configPage4.StgCycles == 0) && (configPage2.perToothIgn != true) )
        {
          unsigned long uSToEnd = 0;

          crankAngle = getCrankAngle(); //Refresh with the latest crank angle
          if (crankAngle > CRANK_ANGLE_MAX_IGN ) { crankAngle -= 360; }
          
          //ONLY ONE OF THE BELOW SHOULD BE USED (PROBABLY THE FIRST):
          //*********
          if(ignition1EndAngle > crankAngle) { uSToEnd = fastDegreesToUS( (ignition1EndAngle - crankAngle) ); }
          else { uSToEnd = fastDegreesToUS( (360 + ignition1EndAngle - crankAngle) ); }
          //*********
          //uSToEnd = ((ignition1EndAngle - crankAngle) * (toothLastToothTime - toothLastMinusOneToothTime)) / triggerToothAngle;
          //*********

          refreshIgnitionSchedule1( uSToEnd + fixedCrankingOverride );
        }
  #endif
        


#if IGN_CHANNELS >= 2
        tempCrankAngle = crankAngle - channel2IgnDegrees;
        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
        tempStartAngle = ignition2StartAngle - channel2IgnDegrees;
        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
        //if (tempStartAngle > tempCrankAngle)
        {
            unsigned long ignition2StartTime = 0;
            if(tempStartAngle > tempCrankAngle) { ignition2StartTime = angleToTime((tempStartAngle - tempCrankAngle), CRANKMATH_METHOD_INTERVAL_REV); }
            //else if (tempStartAngle < tempCrankAngle) { ignition2StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
            else { ignition2StartTime = 0; }

            if( (ignition2StartTime > 0) && (curRollingCut != 2) )
            {
              setIgnitionSchedule2(ign2StartFunction,
                        ignition2StartTime,
                        currentStatus.dwell + fixedCrankingOverride,
                        ign2EndFunction
                        );
            }
        }
#endif

#if IGN_CHANNELS >= 3
        tempCrankAngle = crankAngle - channel3IgnDegrees;
        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
        tempStartAngle = ignition3StartAngle - channel3IgnDegrees;
        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
        //if (tempStartAngle > tempCrankAngle)
        {
            long ignition3StartTime = 0;
            if(tempStartAngle > tempCrankAngle) { ignition3StartTime = angleToTime((tempStartAngle - tempCrankAngle), CRANKMATH_METHOD_INTERVAL_REV); }
            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
            else { ignition3StartTime = 0; }

            if( (ignition3StartTime > 0) && (curRollingCut != 3) )
            {
              setIgnitionSchedule3(ign3StartFunction,
                        ignition3StartTime,
                        currentStatus.dwell + fixedCrankingOverride,
                        ign3EndFunction
                        );
            }
        }
#endif

#if IGN_CHANNELS >= 4
        tempCrankAngle = crankAngle - channel4IgnDegrees;
        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
        tempStartAngle = ignition4StartAngle - channel4IgnDegrees;
        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
        //if (tempStartAngle > tempCrankAngle)
        {

            long ignition4StartTime = 0;
            if(tempStartAngle > tempCrankAngle) { ignition4StartTime = angleToTime((tempStartAngle - tempCrankAngle), CRANKMATH_METHOD_INTERVAL_REV); }
            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
            else { ignition4StartTime = 0; }

            if( (ignition4StartTime > 0) && (curRollingCut != 4) )
            {
              setIgnitionSchedule4(ign4StartFunction,
                        ignition4StartTime,
                        currentStatus.dwell + fixedCrankingOverride,
                        ign4EndFunction
                        );
            }
        }
#endif

#if IGN_CHANNELS >= 5
        tempCrankAngle = crankAngle - channel5IgnDegrees;
        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
        tempStartAngle = ignition5StartAngle - channel5IgnDegrees;
        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
        //if (tempStartAngle > tempCrankAngle)
        {

            long ignition5StartTime = 0;
            if(tempStartAngle > tempCrankAngle) { ignition5StartTime = angleToTime((tempStartAngle - tempCrankAngle), CRANKMATH_METHOD_INTERVAL_REV); }
            //else if (tempStartAngle < tempCrankAngle) { ignition4StartTime = ((long)(360 - tempCrankAngle + tempStartAngle) * (long)timePerDegree); }
            else { ignition5StartTime = 0; }

            if( (ignition5StartTime > 0) && (curRollingCut != 5) ) {
            setIgnitionSchedule5(ign5StartFunction,
                      ignition5StartTime,
                      currentStatus.dwell + fixedCrankingOverride,
                      ign5EndFunction
                      );
            }
        }
#endif

#if IGN_CHANNELS >= 6
        tempCrankAngle = crankAngle - channel6IgnDegrees;
        if( tempCrankAngle < 0) { tempCrankAngle += CRANK_ANGLE_MAX_IGN; }
        tempStartAngle = ignition6StartAngle - channel6IgnDegrees;
        if ( tempStartAngle < 0) { tempStartAngle += CRANK_ANGLE_MAX_IGN; }
        {
            unsigned long ignition6StartTime = 0;
            if(tempStartAngle > tempCrankAngle) { ignition6StartTime = angleToTime((tempStartAngle - tempCrankAngle), CRANKMATH_METHOD_INTERVAL_REV); }
            else { ignition6StartTime = 0; }

            if( (ignition6StartTime > 0) && (curRollingCut != 2) )
            {
              setIgnitionSchedule6(ign6StartFunction,
                        ignition6StartTime,
                        currentStatus.dwell + fixedCrankingOverride,
                        ign6EndFunction
                        );
            }
        }
#endif

      } //Ignition schedules on

      if ( (!BIT_CHECK(currentStatus.status3, BIT_STATUS3_RESET_PREVENT)) && (resetControl == RESET_CONTROL_PREVENT_WHEN_RUNNING) ) 
      {
        //Reset prevention is supposed to be on while the engine is running but isn't. Fix that.
        digitalWrite(pinResetControl, HIGH);
        BIT_SET(currentStatus.status3, BIT_STATUS3_RESET_PREVENT);
      }
    } //Has sync and RPM
    else if ( (BIT_CHECK(currentStatus.status3, BIT_STATUS3_RESET_PREVENT) > 0) && (resetControl == RESET_CONTROL_PREVENT_WHEN_RUNNING) )
    {
      digitalWrite(pinResetControl, LOW);
      BIT_CLEAR(currentStatus.status3, BIT_STATUS3_RESET_PREVENT);
    }
} //loop()

/*
  This function retuns a pulsewidth time (in us) given the following:
  REQ_FUEL
  VE: Lookup from the main fuel table. This can either have been MAP or TPS based, depending on the algorithm used
  MAP: In KPa, read from the sensor (This is used when performing a multiply of the map only. It is applicable in both Speed density and Alpha-N)
  GammaE: Sum of Enrichment factors (Cold start, acceleration). This is a multiplication factor (Eg to add 10%, this should be 110)
  injDT: Injector dead time. The time the injector take to open minus the time it takes to close (Both in uS)
*/
uint16_t PW(int REQ_FUEL, byte VE, long MAP, int corrections, int injOpen)
{
  //Standard float version of the calculation
  //return (REQ_FUEL * (float)(VE/100.0) * (float)(MAP/100.0) * (float)(TPS/100.0) * (float)(corrections/100.0) + injOpen);
  //Note: The MAP and TPS portions are currently disabled, we use VE and corrections only
  uint16_t iVE, iCorrections;
  uint16_t iMAP = 100;
  uint16_t iAFR = 147;

  //100% float free version, does sacrifice a little bit of accuracy, but not much.
  iVE = ((unsigned int)VE << 7) / 100;
  if ( configPage2.multiplyMAP == true ) {
    iMAP = ((unsigned int)MAP << 7) / currentStatus.baro;  //Include multiply MAP (vs baro) if enabled
  }
  if ( (configPage2.includeAFR == true) && (configPage6.egoType == 2)) {
    iAFR = ((unsigned int)currentStatus.O2 << 7) / currentStatus.afrTarget;  //Include AFR (vs target) if enabled
  }
  iCorrections = (corrections << 7) / 100;


  unsigned long intermediate = ((long)REQ_FUEL * (long)iVE) >> 7; //Need to use an intermediate value to avoid overflowing the long
  if ( configPage2.multiplyMAP == true ) {
    intermediate = (intermediate * (unsigned long)iMAP) >> 7;
  }
  if ( (configPage2.includeAFR == true) && (configPage6.egoType == 2) ) {
    intermediate = (intermediate * (unsigned long)iAFR) >> 7;  //EGO type must be set to wideband for this to be used
  }
  intermediate = (intermediate * (unsigned long)iCorrections) >> 7;
  if (intermediate != 0)
  {
    //If intermeditate is not 0, we need to add the opening time (0 typically indicates that one of the full fuel cuts is active)
    intermediate += injOpen; //Add the injector opening time
    if ( intermediate > 65535)
    {
      intermediate = 65535;  //Make sure this won't overflow when we convert to uInt. This means the maximum pulsewidth possible is 65.535mS
    }
  }
  return (unsigned int)(intermediate);
}

byte getVE()
{
  byte tempVE = 100;
  if (configPage2.fuelAlgorithm == LOAD_SOURCE_MAP) //Check which fuelling algorithm is being used
  {
    //Speed Density
    currentStatus.fuelLoad = currentStatus.MAP;
  }
  else if (configPage2.fuelAlgorithm == LOAD_SOURCE_TPS)
  {
    //Alpha-N
    currentStatus.fuelLoad = currentStatus.TPS;
  }
  else if (configPage2.fuelAlgorithm == LOAD_SOURCE_IMAPEMAP)
  {
    //IMAP / EMAP
    currentStatus.fuelLoad = (currentStatus.MAP * 100) / currentStatus.EMAP;
  }
  else { currentStatus.fuelLoad = currentStatus.MAP; } //Fallback position
  tempVE = get3DTableValue(&fuelTable, currentStatus.fuelLoad, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value

  return tempVE;
}

//[PJSC v1.01] Multi VE Map support *************************************************************************************
byte getVE2()
{
  byte tempVE2 = 100;
  if (configPage2.fuelAlgorithm2 == LOAD_SOURCE_MAP) //Check which fuelling algorithm is being used
  {
    //Speed Density
    currentStatus.fuelLoad2 = currentStatus.MAP;
  }
  else if (configPage2.fuelAlgorithm2 == LOAD_SOURCE_TPS)
  {
    //Alpha-N
    currentStatus.fuelLoad2 = currentStatus.TPS;
  }
  else if (configPage2.fuelAlgorithm2 == LOAD_SOURCE_IMAPEMAP)
  {
    //IMAP / EMAP
    currentStatus.fuelLoad2 = (currentStatus.MAP * 100) / currentStatus.EMAP;
  }
  else { currentStatus.fuelLoad2 = currentStatus.MAP; } //Fallback position
  tempVE2 = get3DTableValue(&fuelTable2, currentStatus.fuelLoad2, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value

  return tempVE2;
}

byte getVE3()
{
  byte tempVE3 = 100;
  if (configPage2.fuelAlgorithm3 == LOAD_SOURCE_MAP) //Check which fuelling algorithm is being used
  {
    //Speed Density
    currentStatus.fuelLoad3 = currentStatus.MAP;
  }
  else if (configPage2.fuelAlgorithm3 == LOAD_SOURCE_TPS)
  {
    //Alpha-N
    currentStatus.fuelLoad3 = currentStatus.TPS;
  }
  else if (configPage2.fuelAlgorithm3 == LOAD_SOURCE_IMAPEMAP)
  {
    //IMAP / EMAP
    currentStatus.fuelLoad3 = (currentStatus.MAP * 100) / currentStatus.EMAP;
  }
  else { currentStatus.fuelLoad3 = currentStatus.MAP; } //Fallback position
  tempVE3 = get3DTableValue(&fuelTable3, currentStatus.fuelLoad3, currentStatus.RPM); //Perform lookup into fuel map for RPM vs MAP value

  return tempVE3;
}

byte getVE4()
{
  byte tempVE4 = 100;
  if (configPage2.ignAlgorithm == LOAD_SOURCE_MAP) //Check which fuelling algorithm is being used
  {
    //Speed Density
    currentStatus.ignLoad = currentStatus.MAP;
  }
  else if (configPage2.ignAlgorithm == LOAD_SOURCE_TPS)
  {
    //Alpha-N
    currentStatus.ignLoad = currentStatus.TPS;
  }
  else if (configPage2.ignAlgorithm == LOAD_SOURCE_IMAPEMAP)
  {
    //IMAP / EMAP
    currentStatus.ignLoad = (currentStatus.MAP * 100) / currentStatus.EMAP;
  }
  else { currentStatus.ignLoad = currentStatus.MAP; } //Fallback position
  tempVE4 = get3DTableValue(&ignitionTable, currentStatus.ignLoad, currentStatus.RPM) - OFFSET_IGNITION; //Perform lookup into fuel map for RPM vs MAP value

  return tempVE4;
}

//[PJSC v1.01] Multi VE Map support *************************************************************************************

byte getAdvance()
{
  byte tempAdvance = 0;
  if (configPage2.ignAlgorithm == LOAD_SOURCE_MAP) //Check which fuelling algorithm is being used
  {
    //Speed Density
    currentStatus.ignLoad = currentStatus.MAP;
  }
  else if(configPage2.ignAlgorithm == LOAD_SOURCE_TPS)
  {
    //Alpha-N
    currentStatus.ignLoad = currentStatus.TPS;

  }
  else if (configPage2.fuelAlgorithm == LOAD_SOURCE_IMAPEMAP)
  {
    //IMAP / EMAP
    currentStatus.ignLoad = (currentStatus.MAP * 100) / currentStatus.EMAP;
  }
  tempAdvance = get3DTableValue(&ignitionTable, currentStatus.ignLoad, currentStatus.RPM) - OFFSET_IGNITION; //As above, but for ignition advance
  tempAdvance = correctionsIgn(tempAdvance);

  return tempAdvance;
}

uint16_t calculateInjector2StartAngle(unsigned int PWdivTimerPerDegree)
{
  uint16_t tempInjector2StartAngle = (configPage2.inj2Ang + channel2InjDegrees); //This makes the start angle equal to the end angle

  if( configPage2.inj2SquirtStartEnd == false )                                                                                //[PJSC v1.01]
  {                                                                                                                            //[PJSC v1.01]
    if(tempInjector2StartAngle < PWdivTimerPerDegree) { tempInjector2StartAngle += CRANK_ANGLE_MAX_INJ; }
    tempInjector2StartAngle -= PWdivTimerPerDegree; //Subtract the number of degrees the PW will take to get the start angle
  }                                                                                                                            //[PJSC v1.01]

  if(tempInjector2StartAngle > (uint16_t)CRANK_ANGLE_MAX_INJ) { tempInjector2StartAngle -= CRANK_ANGLE_MAX_INJ; }

  return tempInjector2StartAngle;
}
uint16_t calculateInjector3StartAngle(unsigned int PWdivTimerPerDegree)
{
  uint16_t tempInjector3StartAngle = (configPage2.inj3Ang + channel3InjDegrees);

  if( configPage2.inj3SquirtStartEnd == false )                                                                                //[PJSC v1.01]
  {                                                                                                                            //[PJSC v1.01]
    if(tempInjector3StartAngle < PWdivTimerPerDegree) { tempInjector3StartAngle += CRANK_ANGLE_MAX_INJ; }
    tempInjector3StartAngle -= PWdivTimerPerDegree;
  }                                                                                                                            //[PJSC v1.01]

  if(tempInjector3StartAngle > (uint16_t)CRANK_ANGLE_MAX_INJ) { tempInjector3StartAngle -= CRANK_ANGLE_MAX_INJ; }

  return tempInjector3StartAngle;
}
uint16_t calculateInjector4StartAngle(unsigned int PWdivTimerPerDegree)
{
  uint16_t tempInjector4StartAngle = (configPage2.inj4Ang + channel4InjDegrees);

  if( configPage2.inj4SquirtStartEnd == false )                                                                                //[PJSC v1.01]
  {                                                                                                                            //[PJSC v1.01]
    if(tempInjector4StartAngle < PWdivTimerPerDegree) { tempInjector4StartAngle += CRANK_ANGLE_MAX_INJ; }
    tempInjector4StartAngle -= PWdivTimerPerDegree;
  }                                                                                                                            //[PJSC v1.01]

  if(tempInjector4StartAngle > (uint16_t)CRANK_ANGLE_MAX_INJ) { tempInjector4StartAngle -= CRANK_ANGLE_MAX_INJ; }

  return tempInjector4StartAngle;
}
uint16_t calculateInjector5StartAngle(unsigned int PWdivTimerPerDegree)
{
  uint16_t tempInjector5StartAngle = (configPage2.inj1Ang + channel4InjDegrees); //Note the use of inj1Ang here

  if( configPage2.inj1SquirtStartEnd == false )                                                                                //[PJSC v1.01]
  {                                                                                                                            //[PJSC v1.01]
    if(tempInjector5StartAngle < PWdivTimerPerDegree) { tempInjector5StartAngle += CRANK_ANGLE_MAX_INJ; }
    tempInjector5StartAngle -= PWdivTimerPerDegree;
  }                                                                                                                            //[PJSC v1.01]

  if(tempInjector5StartAngle > (uint16_t)CRANK_ANGLE_MAX_INJ) { tempInjector5StartAngle -= CRANK_ANGLE_MAX_INJ; }

  return tempInjector5StartAngle;
}

//========== [PJSC v1.01] For x4 Multi map support ==========
byte selectVE(byte selectedVe)
{
  byte selectedVEvalue = currentStatus.VE;

  switch (selectedVe) {
    case SELECT_VE1:
      selectedVEvalue = currentStatus.VE;
      break;

    case SELECT_VE2:
      selectedVEvalue = currentStatus.VE2;
      break;

    case SELECT_VE3:
      selectedVEvalue = currentStatus.VE3;
      break;

    case SELECT_VE4:
      selectedVEvalue = currentStatus.VE4;
      break;

    default:
      selectedVEvalue = currentStatus.VE;
      break;
  }

  return selectedVEvalue;
}
//========== [PJSC v1.01] For x4 Multi map support ==========

#endif //Unit testing scope guard
