#include <TinyStepper_28BYJ_48.h>
#include <ss_oled.h> 
#include <VL53L1X.h> //Download this library from https://github.com/pololu/vl53l1x-arduino
#include <Wire.h>
#include <FS.h>
#include <SD.h>
#include <SPI.h>
#include <TFLI2C.h>  // TFLuna-I2C Library v.0.2.0

//****************************************************
//************** definitions  *************
//****************************************************

//Development
#define debugmode 1

//General
#define MODE_RUN 0
#define MODE_OTA 1
#define pinLaser 13
#define BTNstart 12
#define SWIlimit 34   //BTNspare



//Steppers
#define MOTOR_IN1_PIN 26
#define MOTOR_IN2_PIN 25
#define MOTOR_IN3_PIN 33
#define MOTOR_IN4_PIN 32
#define STEPS_PER_REVOLUTION 2048
#define stepsPerDegree 5.688888889  //the number of step values per degree  eg 2048/360

#define moveToTheLeft 1
#define moveToTheRight 2
#define moveCloser -1
#define moveNotRequired 0

//TOF Sensors
#define between(x, a, b)  (((a) <= (x)) && ((x) <= (b)))
#define pinXSHUT_LEFT 18
#define pinXSHUT_FLOOR 23
#define TOFtimeout 500
#define TOFinterval 45
#define LIDARdepth 1.00   //distance between the bacl of the LIDAR and the front of the TOFleft

//595 SIO for LEDs
#define LEDleftG 1  // Left Green
#define LEDleftR 2  // left RED
#define LEDrightG 4 // Right Green
#define LEDrightR 8 // Right RED

//OLED Display
//#define OLED_I2C_ADDRESS 0x3C
//#define OLED_RST_PIN -1
#define pinOLED_SDA -1
#define pinOLED_SCL -1
#define pinOLED_RESET -1
#define OLED_FLIPPED 1
#define OLED_INVERTED 0
// Use bit banging to get higher speed output
#define OLED_HARDWARE_I2C 1
#define OLED_WIDTH 132
#define OLED_HEIGHT 64
#define OLED_INVERT 1
#define OLED_NORMAL 0
#define OLED_RENDER 1

#define OLED_TOF_L_TAB 18
#define OLED_TOF_R_TAB 84
#define OLED_TOF_Line 3
#define OLED_SD_Line 5
#define OLED_STATUS_Line 7

// ------------------74HC595 SIPO to drive the LEDS---------------
//we send a byte out to the 74HC595 and each BIT represents an LED.
//for example sending 129 will light up buttons 1 and 8

#define pinLatch_SIPO 15  // ST_CP pin 12
#define pinClock_SIPO 2   // SH_CP pin 11
#define pinData_SIPO 4    // DS pin 14

//SPI Bus for SDcard
int pinSck = 5;
int pinMiso = 19;
int pinMosi = 27;
int pinCs = 14;

//LIDAR definitions
int16_t  tfAddr = TFL_DEF_ADR;    // default I2C address
uint16_t tfFrame = TFL_DEF_FPS;   // default frame rate
// LIDAR device variables passed back by getData
int16_t  tfDist = 0 ;   // distance in centimeters
int16_t  tfFlux = 0 ;   // signal quality in arbitrary units
int16_t  tfTemp = 0 ;   // temperature in 0.01 degree Celsius
uint16_t tfTime = 0;    // device clock in milliseconds

//TOF sensors
VL53L1X TOFsensorLeft; //Create the sensor object
VL53L1X TOFsensorFloor; //Create the sensor object
int TOFdelay=500;        //ms to add to interval to ensure that we do not read to fast

//Define the hardware
SSOLED oled;
TFLI2C tflI2C;
TinyStepper_28BYJ_48 stepper;

//****************************************************
//****************** var definitions  ****************
//****************************************************

unsigned long TOFstartTime = millis(); //used for our timing loop

//float tolerance=2;  //in CM.  DONT FORGET TO /10 WHEN READING IT
int maxTOFdistance=200;  //in CM
bool runMode=MODE_RUN;
bool STATUS_bootsequence;
bool TOFfail=false;
bool SDfail=false;

int16_t sweepData [360];  //Hold the data from the LIDAR sweep
int numberOfReadings=4;   //4= every 90 degrees.  8 = every 45 degrees.  Max 360

/* WiFi Details */

char OTAssid[51];
char OTApass[51];
char WSssid[51];
char WSpass[51];

//WebServer server(80);

//****************************************************
//************** end of var definitions  *************
//****************************************************

void setup() {
  String temp;
  String temp2;
  String tempSSID;
  int oledResult=0;

  STATUS_bootsequence=true;
  tempSSID=String(random(10000,99999));

  Serial.begin(115200);

  //Setup the I2C interface
  Wire.begin(); 
  Wire.setClock(400000); // use 400 kHz I2C
  
  //Setup the OLED
  oledResult = oledInit(&oled, OLED_132x64, 0x3c, OLED_FLIPPED, OLED_INVERTED, OLED_HARDWARE_I2C, pinOLED_SDA, pinOLED_SCL, pinOLED_RESET, 400000L);
  oledFill(&oled, 0,1);
  OLEDinitialiseDisplay();

  //Setup the SPI for the SD card
  SPI.begin(pinSck, pinMiso, pinMosi, pinCs);
  SDfail=OLEDShowSDData();

  //Set the pin modes
  pinMode(pinXSHUT_LEFT ,OUTPUT);
  pinMode(pinXSHUT_FLOOR ,OUTPUT);
  pinMode(pinLaser,OUTPUT);
  pinMode(BTNstart,INPUT);

  // connect and configure the stepper motor to its IO pins
  //
  stepper.connectToPins(MOTOR_IN1_PIN, MOTOR_IN2_PIN, MOTOR_IN3_PIN, MOTOR_IN4_PIN);
  // set the speed and acceleration rates for the stepper motor
  //
  stepper.setSpeedInStepsPerSecond(256);
  stepper.setAccelerationInStepsPerSecondPerSecond(512);
  stepper.moveToPositionInSteps(0); //Move to position zero
  
  // if (!LittleFS.begin(FORMAT_LITTLEFS_IF_FAILED)) {
  //   #if (useSerialOutput)
  //       Serial.println("LittleFS Mount Failed");
  //   #endif
  //       return;
  // } else {
  //   #if (useSerialOutput)
  //       Serial.println("LittleFS Mounted OK");
  //   #endif
  //   if (!LittleFS.exists(dataFolder)) {    //There is no data folder
  //     #if (useSerialOutput)
  //       Serial.print("creating data folder....");
  //     #endif
  //     if(LittleFS.mkdir(dataFolder)){
  //       #if (useSerialOutput)
  //         Serial.println("New data folder created");
  //       #endif
  //     } else {
  //       #if (useSerialOutput)
  //         Serial.println("New data folder creation FAILED");
  //       #endif
  //     }
  //   }
  // }

  //  //If there is no SSID and PASS, set one up so that the user can actually log in for the first time.
  // if ((strlen(WSssid)>0) && (strlen(WSpass)>0)) {
  //     //all is good
  // } else {
  //   Serial.println("SSID does not exist");
  //   temp = "Quiz_buttons_"+tempSSID;
  //   memset(WSssid, 0, 50);  //null everything
  //   temp.toCharArray(WSssid, 50);
  //   memset(WSpass, 0, 50);  //null everything
  //   tempSSID+=tempSSID;       //The password is now xxxxxyyyyy
  //   tempSSID.toCharArray(WSpass, 50);
  //   Serial.print("WSS=");
  //   Serial.print(WSssid);
  //   Serial.println(":");
  //   Serial.print("WSPASS=");
  //   Serial.print(WSpass);
  //   Serial.println(":");
  // }

  // //If there is no SSID and PASS, set one up so that the user can actually log in for the first time.
  // if ((strlen(OTAssid)>0) && (strlen(OTApass)>0)) {
  //   //all is good
  // } else {
  //   temp = "NONE SET";
  //   memset(OTAssid, 0, 50);  //null everything
  //   temp.toCharArray(OTAssid, 50);
  //   temp = "NONE_SET";
  //   memset(OTApass, 0, 50);  //null everything
  //   temp.toCharArray(OTApass, 50);
  // }

//   //now setup the wifi webserver

//   if (digitalRead(swiOTA) == true) {  //the OTA switch is on
//     runMode = MODE_OTA;
//     WiFi.mode(WIFI_STA);
//     WiFi.begin(OTAssid, OTApass);
//     TFTwaitingforWiFiScreen();
//     #if (useSerialOutput)
//         Serial.println("");
//         Serial.print("Connected to ");
//         Serial.print(OTAssid);
//         Serial.println("");
//         Serial.print("IP address: ");
//         Serial.println(WiFi.localIP());
//         Serial.println("");
//     #endif
//     server.on("/", []() {
//       server.send(200, "text/plain", "Log on using /update or /logo");
//     });

//     ElegantOTA.begin(&server);  // Start ElegantOTA
//     // ElegantOTA callbacks
//     ElegantOTA.onStart(onOTAStart);
//     ElegantOTA.onProgress(onOTAProgress);
//     ElegantOTA.onEnd(onOTAEnd);
//     server.begin();
//     #if (useSerialOutput)
//         Serial.println("HTTP server started");
//     #endif
//   } else {
//     runMode = MODE_RUN;
//     #if (useSerialOutput)
//         Serial.println("Setting up WiFi for webserver");
//     #endif
//     WiFi.mode(WIFI_AP);
//     setWiFisoftAP(WSssid, WSpass);
//     //WiFi.softAPConfig(local_ip, gateway, subnet);
//     delay(1000);

//     //this points the webserver to specific routines, depending on the POST target detected
//     server.on("/", handle_OnConnect);
//     server.on("/index", HTTP_POST, handle_OnConnect);
//     server.on("/setup", handle_setupindex);
//     server.on("/setup", HTTP_POST, handle_setupindex);
//     server.on("/gensetup", HTTP_POST, handle_showgensetup);
//     server.on("/savenames", HTTP_POST, handle_savenames);
//     server.on("/gensetupdone", HTTP_POST, handle_savegensetup);
//     server.on("/wifisetup", HTTP_POST, handle_showwifisetup);
//     server.on("/wifisetupdone", HTTP_POST, handle_savewifisetup);
//     server.on("/manage", HTTP_POST, handle_filemanagement);
//     server.on("/delfiles", HTTP_POST, handle_deletefiles);
//     server.on("/logo", HTTP_POST, handle_getLogo);
//     server.on("/fupload",  HTTP_POST, []() {server.send(200);}, handleUploadFile);
//     server.onNotFound(handle_NotFound);

//     server.begin();
// #if (useSerialOutput)
//     Serial.println("HTTP server started");
//     Serial.println("");
//     Serial.print("Local mode Connected to ");
//     Serial.println(WSssid);
//     Serial.print("IP address: ");
//     Serial.println(WiFi.softAPIP());
// #endif
//   }

  //Turn all TOF's off so that we can configure them
  digitalWrite(pinXSHUT_LEFT, LOW);
  digitalWrite(pinXSHUT_FLOOR, LOW);

  //-----------------------------------------------------------------
  //FIRST WE WILL CONFIGURE AND SETUP TOFsensorLeft
  //-----------------------------------------------------------------
  delay(50);
  digitalWrite(pinXSHUT_LEFT, HIGH); //Turn TOFsensorLeft on
  delay(50);
  
  TOFsensorLeft.setTimeout(TOFtimeout); //Set the sensors timeout
  
  if (!TOFsensorLeft.init())//try to initilise the sensor
  {
      //Sensor does not respond within the timeout time
      TOFfail=true;
      oledWriteString(&oled, 0,OLED_TOF_L_TAB,OLED_TOF_Line,(char *)"FAIL", FONT_8x8, OLED_INVERT, OLED_RENDER);
      #if (debugmode)
        Serial.println("TOFsensorLeft is not responding, check your wiring");
      #endif

  }
  else
  {
      TOFsensorLeft.setAddress(42); //Set the sensors I2C address
      TOFsensorLeft.setDistanceMode(VL53L1X::Long); //Set the sensor to maximum range of 4 meters
      TOFsensorLeft.setMeasurementTimingBudget(40000); //Set its timing budget in microseconds longer timing budgets will give more accurate measurements
      TOFsensorLeft.startContinuous(TOFinterval); //Sets the interval where a measurement can be requested in milliseconds
      oledWriteString(&oled, 0,OLED_TOF_L_TAB,OLED_TOF_Line,(char *)"RDY", FONT_8x8, OLED_NORMAL, OLED_RENDER);
      #if (debugmode)
        Serial.println("TOFsensorLeft is online");
      #endif
  }   

  //-----------------------------------------------------------------
  //NOW CONFIGURE AND SETUP TOFsensorFloor
  //-----------------------------------------------------------------
  delay(50);
  digitalWrite(pinXSHUT_FLOOR, HIGH); //Turn TOFsensorFloor on
  delay(50);
  
  TOFsensorFloor.setTimeout(TOFtimeout); //Set the sensors timeout
  
  if (!TOFsensorFloor.init())//try to initilise the sensor
  {
      //Sensor does not respond within the timeout time
      TOFfail=true;
      oledWriteString(&oled, 0,OLED_TOF_R_TAB,OLED_TOF_Line,(char *)"FAIL", FONT_8x8, OLED_INVERT, OLED_RENDER);
      #if (debugmode)
        Serial.println("TOFsensorFloor is not responding, check your wiring");
      #endif
  }
  else
  {
      TOFsensorFloor.setAddress(43); //Set the sensors I2C address
      TOFsensorFloor.setDistanceMode(VL53L1X::Long); //Set the sensor to maximum range of 4 meters
      TOFsensorFloor.setMeasurementTimingBudget(40000); //Set its timing budget in microseconds longer timing budgets will give more accurate measurements
      TOFsensorFloor.startContinuous(TOFinterval); //Sets the interval where a measurement can be requested in milliseconds
      oledWriteString(&oled, 0,OLED_TOF_R_TAB,OLED_TOF_Line,(char *)"RDY", FONT_8x8, OLED_NORMAL, OLED_RENDER);
      #if (debugmode)
        Serial.println("TOFsensorFloor is online");
      #endif
  }  

  // ------------------SETUP THE 74HC595 SIPO that drives the LEDs ---------------
  pinMode(pinLatch_SIPO, OUTPUT);
  pinMode(pinClock_SIPO, OUTPUT);
  pinMode(pinData_SIPO, OUTPUT);

  // if (runMode == MODE_RUN) {
  //   TFTshowPrepareScreen();
  //   processPrepareScreen();
  //   TFTshowReadyScreen();
  //   waitForReadyToPlay();
  //   TFTshowWaitingScreen();
  // } else {
  //   TFTshowOTAScreen();
  // }
  STATUS_bootsequence=false;
  TOFstartTime = millis(); //used for our timing loop
}

// void setWiFisoftAP(const char *TEMPssid, const char *TEMPpass) {
//   WiFi.softAP(TEMPssid, TEMPpass);
// }


void loop() {
byte temp;
byte nextAction;

 //Handle any webserver requests
  //server.handleClient();
  if (runMode == MODE_OTA) {
 //   ElegantOTA.loop();
  } else {
    if (!TOFfail) {
      while (digitalRead(BTNstart)==0) {            //Wait for someone to press the "Go" Button
        delay(10);
      }

      //1. Align parallel to the wall.
      //2. When aligned, do the scan

      nextAction=performAlign();
      if (nextAction==moveNotRequired) { //the sensor is aligned and ready
        OLEDstatus("WAIT",OLED_NORMAL);
        delay(1000);
        doScan(0);
      }
    }
  } 
}

byte performAlign() {
  //read the TOF sensor LEFT and the LIDAR on the right, then align to the wall
  //returns TRUE if it is aligned within tolerance, FALSE if it is out of tolerance or 2 if it is unread (eg read too fast)
float temp;
float TOFleft;
float TOFright;
int moveDirection=moveToTheLeft;    //Force the first loop iteration
int LEDdisplay=(LEDleftG | LEDrightG);
float moDi[3]={0,-stepsPerDegree,stepsPerDegree};
float multFactor=1;

    //If we request a measurement before the last measurement has been taken the 
    //code will be blocked until the measurement is complete. In order to stop this from happening we
    //must ensure that time between measurement requests is greater than the timing budget and the argument 
    //given in the startContinuous() function. In our case our measurement time must be greater than 50mS.
  digitalWrite(pinLaser,true);
  #if (debugmode)
    Serial.println("Start of Alignment");
  #endif
  OLEDstatus("Align START",OLED_NORMAL);
  while ((moveDirection==moveToTheLeft) || (moveDirection==moveToTheRight)) {
  // #if (debugmode)
  // Serial.println("H1");
  // #endif
    if((millis()- TOFstartTime) > TOFinterval+TOFdelay) {
      #if (debugmode)
        Serial.println("H2");
      #endif
      TOFstartTime = millis();
      temp=LIDARdepth+TOFsensorLeft.read();  //Read the LEFT sensor
      //LIDAR reads to the back of the sensor not the front of the sensor, so add in the difference
      TOFleft=int(temp/10.0); //We only want the CMs
      
      tflI2C.getData( tfDist, tfFlux, tfTemp, tfAddr);  //Read the right LIDAR
      TOFright=tfDist;        //This lidar does not go to mm, only cm.
      #if (debugmode)
        Serial.print("Raw data ");
        Serial.print(TOFleft);
        Serial.print(" ");
        Serial.print(TOFright);
        Serial.println();
      #endif
      //if (between(TOFleft,TOFright-(tolerance/2),TOFright+(tolerance/2))) {
        if (TOFleft==TOFright) {
        //Everything is good, we have two green lights
        OLEDwriteFloat(TOFleft,OLED_TOF_L_TAB,OLED_TOF_Line,0);
        OLEDwriteFloat(TOFright,OLED_TOF_R_TAB,OLED_TOF_Line,0);
        #if (debugmode)
          Serial.println("ALIGNED "+String(TOFleft)+"/"+String(TOFright));
        #endif
        LEDsByByte(LEDleftG | LEDrightG);   //Both LEDs green
        digitalWrite(pinLaser,false);       //turn off the laser
        moveDirection=moveNotRequired;      //movenotRequired is already set

      } else {  //we need to move it

        LEDdisplay=0;                                                           //zero LED values

  	    //Check the max distances in the next two statements.
        //If LEDdisplay is zero, no red lights

        if ((TOFleft>maxTOFdistance) || (TOFleft==0)) { //The left sensor is too far away, 
          LEDdisplay=LEDleftR;    
          moveDirection=moveToTheLeft;
        }

        if ((TOFright>maxTOFdistance) || (TOFright==0)) {  //The right sensor is too far away
          LEDdisplay=(LEDdisplay | LEDrightR);
          moveDirection=moveToTheRight;
        } 

        if (LEDdisplay==0) {                          //We know that the device is not too far away
          //if (TOFleft>TOFright+(tolerance/2)) {
          if (TOFleft>TOFright) {
              LEDdisplay=(LEDleftR | LEDrightG);      //left is too far away
              moveDirection=moveToTheLeft;
          } else {
              LEDdisplay=(LEDrightR | LEDleftG);      //right is too far away
              moveDirection=moveToTheRight;
          }

          //work out how muh to move the sensor
          temp=abs(TOFleft-TOFright);    //How far apart are the sensors
          multFactor=1;
          if (temp>30) {
            multFactor=20; 
          } else if (temp>15) {
            multFactor=10; 
          } else if(temp>10) {
            multFactor=3;
          } 
        
          OLEDwriteFloat(TOFleft,OLED_TOF_L_TAB,OLED_TOF_Line,0);
          OLEDwriteFloat(TOFright,OLED_TOF_R_TAB,OLED_TOF_Line,0);

          #if (debugmode)
            Serial.println("BAD "+String(TOFleft)+"/"+String(TOFright)+"/DIF "+String(temp)+" /MF "+String(multFactor)+" MDir"+String(moveDirection)+"for "+ String(moDi[moveDirection]*multFactor));
          #endif
          LEDsByByte(LEDdisplay);
          stepper.moveRelativeInSteps(moDi[moveDirection]*multFactor);
          while(!stepper.motionComplete()) {
            stepper.processMovement();
          }
        } else {                //we are out of tolerance or too far away
          OLEDprintAt("MOVE",OLED_TOF_L_TAB,OLED_TOF_Line,true);
          OLEDprintAt("     ",OLED_TOF_R_TAB,OLED_TOF_Line,true);

          #if (debugmode)
            Serial.println("MOVE/    ");
          #endif
          LEDdisplay=(LEDrightR | LEDleftG | LEDrightG | LEDleftR);   //light up all LEDS
          LEDsByByte(LEDdisplay);
          moveDirection=moveCloser;
        }
      }
    }
  }
    stepper.setCurrentPositionInSteps(0); //Tell the stepper motor that we are at pos zero now
    digitalWrite(pinLaser,false);
    return moveDirection;   //this will either be modeNotRequired or moveCloser
}

// byte readTOF() {
//   //reads the TOF sensors
//   //returns TRUE if it is aligned within tolerance, FALSE if it is out of tolerance or 2 if it is unread (eg read too fast)
// float temp;
// float TOFleft;
// float TOFright;
// byte OKtoContinue=2;
// int LEDdisplay=(LEDleftG | LEDrightG);

//     //We have to be careful here. If we request a measurement before the measurement has been taken your
//     //code will be blovked until the measurement is complete. In order to stop this from happening we
//     //must ensure that time between measurement requests is greater than the timing budget and the argument 
//     //given in the startContinuous() function. In our case our measurement time must be greater than 50mS.

//     if((millis()- TOFstartTime) > TOFreadDelay) {
//       temp=TOFsensorLeft.read();
//       TOFleft=temp/10.0;
//       temp=TOFsensorFloor.read();
//       TOFright=temp/10.0;       
//       if (between(TOFleft,TOFright-(tolerance/2),TOFright+(tolerance/2))) {
//         OLEDwriteFloat(TOFleft,OLED_TOF_L_TAB,OLED_TOF_Line,0);
//         OLEDwriteFloat(TOFright,OLED_TOF_R_TAB,OLED_TOF_Line,0);
//         #if (debugmode)
//           Serial.println("OK "+String(TOFleft)+"/"+String(TOFright));
//         #endif
//         LEDsByByte(LEDdisplay); //we have already defaulted both green
//         OKtoContinue=true;
//       } else {
//         LEDdisplay=0;                                           //zero LED values
//         OKtoContinue=false;                                   //tuen off the continue flag
//         if ((TOFleft>maxTOFdistance) || (TOFleft==0)) {LEDdisplay=LEDleftR;}    //The left sensor is too far away, 
//         if ((TOFright>maxTOFdistance) || (TOFright==0)) {LEDdisplay=(LEDdisplay | LEDrightR);} //The right sensor is too far away
//         if (LEDdisplay==0) {  //We know that the device is not too far away
//           if (TOFleft>TOFright+(tolerance/2)) {
//               LEDdisplay=(LEDleftR | LEDrightG);      //left is too far away
//           } else
//               LEDdisplay=(LEDrightR | LEDleftG);      //right is too far away
//           }
//         OLEDwriteFloat(TOFleft,OLED_TOF_L_TAB,OLED_TOF_Line,0);
//         OLEDwriteFloat(TOFright,OLED_TOF_R_TAB,OLED_TOF_Line,0);
//         #if (debugmode)
//           Serial.println("BAD "+String(TOFleft)+"/"+String(TOFright));
//         #endif
//         LEDsByByte(LEDdisplay);
//       }
//       TOFstartTime = millis();
//     }
//     return OKtoContinue;
// }

void LEDsByBit(byte whichbit, byte bitvalue) { 
  byte temp;
  bitWrite(temp, whichbit, bitvalue);                     //set the lamp
  digitalWrite(pinLatch_SIPO, LOW);                       // ST_CP LOW to keep LEDs from changing while reading serial data
  shiftOut(pinData_SIPO, pinClock_SIPO, MSBFIRST, temp);  // Shift out the bits
  digitalWrite(pinLatch_SIPO, HIGH);                      // ST_CP HIGH change LEDs
}

void LEDsByByte(byte ledToLight) {                              // Turn all LEDS on or off at once without affecting the global
  digitalWrite(pinLatch_SIPO, LOW);                             // ST_CP LOW to keep LEDs from changing while reading serial data
  shiftOut(pinData_SIPO, pinClock_SIPO, MSBFIRST, ledToLight);  // Shift out the bits
  digitalWrite(pinLatch_SIPO, HIGH);                            // ST_CP HIGH change LEDs
}

void OLEDinitialiseDisplay() { //

  oledFill(&oled, 0,1);
  oledWriteString(&oled, 0,0,0,(char *)"ICON Survey Tool", FONT_8x8, OLED_NORMAL, OLED_RENDER);
  oledWriteString(&oled, 0,0,OLED_TOF_Line,(char *)"L:", FONT_8x8, OLED_NORMAL, OLED_RENDER);
  oledWriteString(&oled, 0,66,OLED_TOF_Line,(char *)"F:", FONT_8x8, OLED_NORMAL, OLED_RENDER);
  
  //sprintf(tempchar,"%02d",EP);
  //oledWriteString(&oled, 0,OLEDepTab,7,(char *)tempchar, FONT_8x8, OLED_NORMAL, OLED_RENDER);
}

void OLEDwriteFloat(float thisval,int x,int y, byte invert) { //
char tempchar[30];
  
  sprintf(tempchar,"%3.2f",thisval);
  oledWriteString(&oled, 0,x,y,(char *)tempchar, FONT_8x8, OLED_INVERT,OLED_RENDER);
}

void OLEDprintAt(String thisval,int x,int y, byte invert) { //
char tempchar[30];
  
  thisval.toCharArray(tempchar,30);
  oledWriteString(&oled, 0,x,y,(char *)tempchar, FONT_8x8, OLED_INVERT,OLED_RENDER);
}

void OLEDstatus(String thisval, byte invert) { //
char tempchar[30];
  
  thisval.toCharArray(tempchar,30);
  oledWriteString(&oled, 0,0,OLED_STATUS_Line,(char *)tempchar, FONT_8x8, OLED_INVERT,OLED_RENDER);
}

bool OLEDShowSDData() {
String SDdesc="U/K";
bool testPassed=true;
char tempchar[30];
uint8_t cardType;
uint64_t cardSize;

  if (!SD.begin(pinCs)) {  //the SD Mount has failed.  Check the card.
    strcpy(tempchar,"SD Card failed");
    testPassed=false;
  } else {
    cardType = SD.cardType();
    if (cardType == CARD_NONE) {
      strcpy(tempchar,"No SD card attached");
      testPassed=false;
    }
  }

  if (testPassed) {
    switch (cardType) {
      case CARD_MMC:
        SDdesc="MMC";
        break;
      case CARD_SD:
        SDdesc="SDSC";
        break;
      case CARD_SDHC:
        SDdesc="SDHC";
      break;
    }
    cardSize = SD.cardSize() / (1024 * 1024);
    sprintf(tempchar,"%s %03d GB",SDdesc,cardSize*0.0009765625); 
  }
  oledWriteString(&oled, 0,0,OLED_SD_Line,(char *)tempchar, FONT_8x8, OLED_NORMAL, OLED_RENDER);
  return testPassed; //return that there was no failure
}

void doScan(int sweepNumber) {
  //this does a 360 sweep,recording the data in sweep (x,sweepnumber)
int count;
int numberOfSteps;
float dgrs=0;

  numberOfSteps=360/numberOfReadings;
 

  for (count=0; count<numberOfReadings; count++) {
    dgrs=(count*numberOfSteps)*stepsPerDegree;
    stepper.moveToPositionInSteps(dgrs);
    while(!stepper.motionComplete()) {
      stepper.processMovement();
    }
    if( tflI2C.getData( tfDist, tfFlux, tfTemp, tfAddr)) {
   
        Serial.print("Dist: ");      // ...print distance,
        Serial.print(tfDist);
        Serial.print(" | Flux: ");   // ...print quality
        Serial.print(tfFlux);
    } else {
      tflI2C.printStatus();        // else, print error status.
    }

    sweepData[count]=tfDist;
    Serial.print(" Deg:");
    Serial.print(dgrs);
    Serial.print(" Val:");
    Serial.println(tfDist);
  }
    for (count=numberOfReadings;count>=0;count--) {
      dgrs=(count*numberOfSteps)*stepsPerDegree;
      stepper.moveToPositionInSteps(dgrs);
      while(!stepper.motionComplete()) {
        stepper.processMovement();
      }
    if( tflI2C.getData( tfDist, tfFlux, tfTemp, tfAddr)) {
   
        Serial.print("Dist: ");      // ...print distance,
        Serial.print(tfDist);
        Serial.print(" | Flux: ");   // ...print quality
        Serial.print(tfFlux);
    } else {
      tflI2C.printStatus();        // else, print error status.
    }
    sweepData[count]=(sweepData[count]+tfDist)/2;
    Serial.print(" Deg:");
    Serial.print(dgrs);
    Serial.print(" Val:");
    Serial.println(tfDist);
  }
}



// void handle_getLogo() {  //This shows the upload logo button
// float spaceLeft;
// float divs=1024;
// float divs2=100;

//   listLogoFiles(jpgCountOnly);

//   spaceLeft=float(LittleFS.totalBytes()-LittleFS.usedBytes())-200;    //a little wriggle room
//   spaceLeft=int((spaceLeft/divs)*divs2)/divs2;    //to 2 dp in KB

//   #if (useSerialOutput)
//     Serial.println("ON get logo file");
//   #endif

//   // send the HTTP response body
//   String html = String(HTML_HEADER) + String(HTML_GETLOGO);

//   html.replace("%SPACE%", String(spaceLeft));    // Show the space that is left
//   server.send(200, "text/html", html);
//   delay(10);
// }

// void listLogoFiles(byte which) {    //collect the jpeg data and put it in GLOBALfilefield

// String fName;
// String fExtn;
// String Dsize;
// String Sel;

// float fSize;
// float divs=1024;
// float divs2=100;
// float totsize;
// bool haserror;

//   jpgCount=0;
//   GLOBALfilefield="";
//   haserror=false;

//   File root = LittleFS.open(dataFolder);
//   if(!root){
//       Serial.println("- failed to open directory "+dataFolder);
//       showMessage("Failed to open directory "+dataFolder,"",WarningSetupScreen);
//       haserror=true;
//   }

//   if (!haserror) {
//     File file = root.openNextFile();
//     Serial.println("Generating File List");
//     while(file) {
//       if(file.isDirectory()){
//           // Do nothing
//       } else {
//         fName=file.name();
//         fExtn = fName.substring(fName.length()-3, fName.length());
//         if (fExtn=="jpg") {
//           jpgCount++;
//           switch (which) {
//             case jpgList:
//               fSize=file.size();
//               if (fSize<1000) {
//                 Dsize=String(int(fSize))+" bytes";
//               } else {
//                 fSize=int((fSize/divs)*divs2)/divs2;
//                 Dsize=String(fSize)+" kb";    //Round to 2dp.
//               } 
//               // <tr><td>%fname1%</td><td>%fsize1%</td><td><input type="checkbox" name="%del1%" id="%del1%"></td></tr>
//               GLOBALfilefield+="<tr><td><input type='hidden' name='%fn"+String(jpgCount)+"%' id='%fn"+String(jpgCount)+"%' value='"+fName+"'>"+fName+"</td><td>"+Dsize+"</td><td><input type='checkbox' name='%del"+String(jpgCount)+"' id='%del"+String(jpgCount)+"' value='0'></td></tr>";
//               break;
//             case jpgOption:
//               Sel="";
//               if (FNlogo==fName) {
//                 Serial.println("selected ="+fName);
//                 Sel=" selected";
//               }
//               GLOBALfilefield+="<option value='"+fName+"'"+Sel+">"+fName+"</option>";
//               break;
//             case jpgCountOnly:
//               //no action
//             break;
//           }
//         }
//       }
//       file = root.openNextFile();
//     }
//   }
// }


// void handle_filemanagement() {

//   listLogoFiles(jpgList);

//   if (jpgCount==0) {
//     showMessage("No logo files found","",WarningSetupScreen);
//   } else {
//     Serial.println("Sending web page");
//     String html = String(HTML_HEADER) + String(HTML_GETMANAGE);
//     html.replace("%FILEDATA%", GLOBALfilefield);  
//     server.send(200, "text/html", html);
//     delay(10);
//   }
// }


// void handleUploadFile() {
// static File fsUploadFile;
// static String newfname;
// byte fnstart;
// //static bool usethislogo;
// bool haserror;
// long spaceLeft;
// static size_t uploadsize;


//   HTTPUpload& uploadfile = server.upload();
//   haserror=false;
//   if (uploadfile.status == UPLOAD_FILE_START)  {
//     String filename = uploadfile.filename;

//     if (!filename.startsWith("/")) filename = dataFolder + filename;
//     fnstart=filename.lastIndexOf("/");
//     newfname=filename.substring(fnstart+1);


//     Serial.println("Filename="+filename);
//     Serial.println("New Filename="+newfname);
//     Serial.println("Upload size="+String(uploadsize));

//     if (LittleFS.exists(filename)) {    //check to see if the file exists
//       if (!LittleFS.remove(filename)) {
//         showMessage("File exists and could not be removed","",WarningSetupScreen);
//         return;
//       } else {
//         Serial.println("Previous file of the same name deleted");
//       }
//     }

//     //if (!haserror) {
//       fsUploadFile = LittleFS.open(filename, "w");
//       Serial.println("starting "+filename);
//       if (!fsUploadFile) {
//         Serial.println("Upload Failed");
//         showMessage("The file upload failed.","",WarningSetupScreen);
//         haserror=true;
//       }
//     //}
//   } else if (uploadfile.status == UPLOAD_FILE_WRITE)  {
//     if (fsUploadFile) {
//       Serial.println("writing");
//       fsUploadFile.write(uploadfile.buf, uploadfile.currentSize); // Write the received bytes to the file
//     }
//   }
//   else if (uploadfile.status == UPLOAD_FILE_END)  {
//     if (fsUploadFile) {        // If the file was successfully created
//       Serial.println("Ending");
//       fsUploadFile.close();   // Close the file again
//       Serial.println("Upload Complete successful");
//       //if (usethislogo){ CHANGED - use the new logo by default
//           Serial.println("writing new logo default");
//           FNlogo=newfname;
//           saveVars();
//       //}
//       showMessage("The logo has been uploaded successfully.","",CompleteSetupScreen);
//     }
//     else {
//         showMessage("The file upload was not successful","",WarningSetupScreen);
//         haserror=true;
//     }
//   }
// }

// void handle_deletefiles() {  //this routine is called when the user hits the "save names" button
//   byte lp;
//   byte cnt;
//   String chk;
//   String Fname;
//   bool errs;
//   String msg1;
//   String msg2;
//   byte Mstyle;

//     #if (useSerialOutput)
//       Serial.println("ON deleting files");
//     #endif
//     errs=false;
//     //Shuffle all of the names up together
//     for (lp = 0; lp < jpgCount; lp++) {
//       chk="%del"+String(lp);
//       Serial.println(chk+" Value="+server.arg(chk));
//       if(server.arg(chk)=="0") {      //this is backwards, but it does the job!
//         Serial.println("ticked yes");
//         Fname=server.arg("%fn"+String(lp)+"%");
//         Serial.println("Trying to delete "+dataFolder+Fname);
//         if (!LittleFS.remove(dataFolder+Fname)) {
//           Serial.println("File "+Fname+" could not be removed");
//           errs=true;
//         }
//       }
//     }

//     if (errs) {
//         msg1="There were errors deleting the files";
//         Mstyle=WarningSetupScreen;
//     } else {
//         msg1="No errors were encountered";
//         Mstyle=CompleteSetupScreen;
//     }
//     showMessage(msg1,"",Mstyle);
//   } 

// void onOTAStart() {
//   // Log when OTA has started
// #if (useSerialOutput)
//   Serial.println("OTA update started!");
// #endif
//   tft.fillScreen(ILI9341_BLACK);
//   tft.setTextColor(ILI9341_WHITE);
//   printat("OTA update started", 0, 0, TFTtextMedium, TFTnoErase, TFTcentre);
//   printat("------------------", 0, 1, TFTtextMedium, TFTnoErase, TFTcentre);
// }

// void onOTAProgress(size_t current, size_t final) {

//   if (millis() - ota_progress_millis > 500) {
//     ota_progress_millis = millis();
//     tft.setTextColor(ILI9341_WHITE);
//     printat(String(current)+" bytes of " + String(final)+"  ", 0, 6, TFTtextMedium, ILI9341_BLACK, TFTleft);
//   }
// }

// void onOTAEnd(bool success) {
//   // Log when OTA has finished
//   tft.fillScreen(ILI9341_BLACK);
//   if (success) {
//     tft.setTextColor(ILI9341_GREEN);
//     printat("OTA update completed", 0, 3, TFTtextMedium, TFTnoErase, TFTcentre);
//     printat("successfully!", 0, 4, TFTtextMedium, TFTnoErase, TFTcentre);
// #if (useSerialOutput)
//     Serial.println("OTA update finished successfully!");
// #endif
//   } else {
//     tft.setTextColor(ILI9341_RED);
//     printat("OTA update ERROR!", 0, 3, TFTtextMedium, TFTnoErase, TFTcentre);
// #if (useSerialOutput)
//     Serial.println("OTA update ERROR!");
// #endif
//   }
//   tft.setTextColor(ILI9341_WHITE);
//   buttonLabels("", "", "Restart");
//   do {
//     buttonResult = digitalRead(btnBottom);
//     //server.handleClient();        //check for webserver events
//     //ElegantOTA.loop();
//   } while (buttonResult == 0);  //wait for the Unlock button.
//   ESP.restart();
// }


// void handle_OnConnect() {  //This builds the INDEX web page (eg on connect)
//   #if (useSerialOutput)
//     Serial.println("ON Connect/names");
//   #endif
//   byte lp;
//   String currentname;

//   // send the HTTP response body
//   String html = String(HTML_HEADER) + String(HTML_NAMES);
//   html.replace("%OWNER%", Owner);    // Put in the owners name
//   html.replace("%OWNERS%", Owners);  // Put in the owners name
//   html.replace("TN1p", teams[0]);    // Put in the Teams A name
//   html.replace("TN2p", teams[1]);    // Put in the Teams B name

//   for (lp = 1; lp < 9; lp++) {
//     currentname = playerNames[lp - 1];
//     html.replace("PN" + String(lp) + "p", currentname);  // replace the marker with a real name.  We do this in case something needs editing
//   }
//   server.send(200, "text/html", html);

//   delay(10);
// }

// void handle_setupindex() {  //This builds the INDEX web page (eg on connect)
//   #if (useSerialOutput)
//     Serial.println("ON Setup Index");
//   #endif

//   // send the HTTP response body
//   String html = String(HTML_HEADER) + String(HTML_SETUPINDEX);
//   html.replace("%OWNER%", Owner);    // Put in the owners name
//   html.replace("%OWNERS%", Owners);  // Put in the owners name
  
//   server.send(200, "text/html", html);

//   delay(10);
// }

// void handle_summary() {  //this builds the web page for the name display
//   // byte lp;
//   // String currentname;
//   // String currentScore;
//   // byte totScore[2];


//   //   #if (useSerialOutput)
//   //     Serial.println("ON summary");
//   //   #endif
//   //   // send the HTTP response body
//   //   String html = String(HTML_HEADER)+String(HTML_SUMMARY);
//   //   html.replace("%OWNER%", Owner); // Put in the owners name
//   //   html.replace("%OWNERS%", Owners); // Put in the owners name
//   //   html.replace("%TN1%", teams[0]); // Put in the Teams A name
//   //   html.replace("%TN2%", teams[1]); // Put in the Teams B name
//   //   totScore[0]=0;
//   //   totScore[1]=0;
//   //   for (lp=1;lp<9;lp++) {
//   //     totScore[lp>4]+=playerScores[lp-1];
//   //     currentname=playerNames[lp-1];
//   //     currentScore=String(playerScores[lp-1]);
//   //      html.replace("%P"+String(lp)+"P%", currentname); // replace the marker with a real name.  We do this in case something needs editing
//   //      html.replace("%P"+String(lp)+"S%", currentScore); // replace the marker with the score
//   //    }
//   //   html.replace("%TT1%", String(totScore[0])); // Put in the Teams A name
//   //   html.replace("%TT2%", String(totScore[1])); // Put in the Teams B name
//   //   server.send(200, "text/html",html);
//   //   delay(10);
// }

// void handle_savenames() {  //this routine is called when the user hits the "save names" button
//   byte lp;
//   byte cnt;
//   String currentname;

// #if (useSerialOutput)
//   Serial.println("ON Saving Names");
// #endif

//   //Shuffle all of the names up together
//   for (lp = 0; lp < 8; lp++) {
//     currentname = server.arg("p" + String(lp + 1) + "p");  //get the POST arguments from the webserver. we dont have any other processing todo
//     playerNames[lp] = currentname;
//   }
//   teams[0] = server.arg("tn1p");  //get the Team names from the webserver.
//   teams[1] = server.arg("tn2p");  //get the Team names from the webserver.

//   showMessage("You can now close this webpage","or return to the name screen",CompleteGeneralScreen);
// }


// void handle_savegensetup() {  //thi is called when the SAVE buttn is pressed on the SETUP screen
//   byte lp;
//   String temp;
//   String temp2;
//   char tValue[10];
//   uint16_t Cvalue;
//   byte r;
//   byte g;
//   byte b;

// #if (useSerialOutput)
//   Serial.println("ON Saving Setup");
// #endif

//   temp = server.arg("owner");  //get the argument we need.  Quizzmaster name
//   setOwner(temp);

//   temp = server.arg("teamAfn");  //get the argument we need.  Team A buzzer sound filename
//   teamFN[0] = temp;

//   temp = server.arg("teamBfn");  //get the argument we need.  Team B buzzer sound filename
//   teamFN[1] = temp;

//   temp = server.arg("Stime");  //secondary timer delay value
//   TMRsecondary = temp.toInt();

//   temp = server.arg("Ltime");  //Initial Logo display time (in ms)
//   TMRlogoDisplayTime = temp.toInt();

//   temp = server.arg("volume");  //Speaker volume

//   DFvolume = temp.toInt();
//   #if (useDFPlayer)
//     myDFPlayer.volume(DFvolume);  //Set volume value. From 0 to 30
//   #endif

//   temp = server.arg("colour1");  //Winner 1 colour
// #if (useSerialOutput)
//   Serial.println(temp);
// #endif
//   TFThex[0] = temp;

//   temp = server.arg("colour2");  //Winner 1 colour
// #if (useSerialOutput)
//   Serial.println(temp);
// #endif
//   TFThex[1] = temp;

//   temp = server.arg("colour3");  //Winner 1 colour
// #if (useSerialOutput)
//   Serial.println(temp);
// #endif
//   TFThex[2] = temp;

// temp = server.arg("Logoname");  //Winner 1 colour
// #if (useSerialOutput)
//   Serial.println(temp);
// #endif
//   FNlogo = temp;


//   saveVars();  //save them to a small internal file.
//   showMessage("","",CompleteSetupScreen);
// }


// void handle_showgensetup() {  //this is called when we want to show the setup page, so we need to populate the values
//   byte lp;
//   String currentname;

// #if (useSerialOutput)
//   Serial.println("ON Show Setup");
// #endif

//   listLogoFiles(jpgOption);
//   //READ SPIFFS HERE
//   // send the HTTP response body
//   String html = String(HTML_HEADER) + String(HTML_GENSETUP);
//   html.replace("%OWNER%", Owner);                       // Put in the owners name
//   html.replace("%OWNERS%", Owners);                     // Put in the owners name
//   html.replace("%TEAMAFN%", teamFN[0]);                 // Put in the data
//   html.replace("%TEAMBFN%", teamFN[1]);                 // Put in the data
//   html.replace("%STIME%", String(TMRsecondary));        // Put in the data
//   html.replace("%LTIME%", String(TMRlogoDisplayTime));  // Put in the data
//   html.replace("%VOL%", String(DFvolume));              // Put in the data
//   html.replace("%P1%", TFThex[0]);                      // Put in the data
//   html.replace("%P2%", TFThex[1]);                      // Put in the data
//   html.replace("%P3%", TFThex[2]);                      // Put in the data
//   html.replace("%LOGOLIST%", GLOBALfilefield);  // Put in the data
//   server.send(200, "text/html", html);
//   delay(10);
// }

// void handle_showwifisetup() {  //this is called when we want to show the setup page, so we need to populate the values
//   byte lp;
//   String temp;

// #if (useSerialOutput)
//   Serial.println("ON Show WiFi Setup");
// #endif
//   //READ SPIFFS HERE
//   // send the HTTP response body
//   String html = String(HTML_HEADER) + String(HTML_WIFISETUP);
//   html.replace("%OWNER%", Owner);    // Put in the owners name
//   html.replace("%OWNERS%", Owners);  // Put in the owners name
//   temp = String(OTAssid);
//   html.replace("%WSSID%", temp);  // Put in the data
//   temp = String(OTApass);
//   html.replace("%WPASS%", temp);  // Put in the data
//   temp = String(WSssid);
//   html.replace("%SSSID%", temp);  // Put in the data
//   temp = String(WSpass);
//   html.replace("%SPASS%", temp);  // Put in the data
//   temp = String(TMRwifiResponseTime);
//   html.replace("%WIFIWAIT%", temp);  // Put in the data
//   server.send(200, "text/html", html);
//   delay(10);
// }

// void handle_savewifisetup() {  //thi is called when the SAVE buttn is pressed on the SETUP screen
//   byte lp;
//   String temp;
//   String temp2;
//   char tValue[10];
//   uint16_t Cvalue;
//   byte r;
//   byte g;
//   byte b;


// #if (useSerialOutput)
//   Serial.println("ON Saving Wi-Fi Setup");
// #endif

//   temp = server.arg("wssid");
//   memset(OTAssid, 0, 50);
//   temp.toCharArray(OTAssid, 50);

//   temp = server.arg("wpass");
//   memset(OTApass, 0, 50);
//   temp.toCharArray(OTApass, 50);

//   temp = server.arg("sssid");
//   memset(WSssid, 0, 50);
//   temp.toCharArray(WSssid, 50);

//   temp = server.arg("spass");
//   memset(WSpass, 0, 50);
//   temp.toCharArray(WSpass, 50);

//   temp = server.arg("wifiwait");
//   TMRwifiResponseTime = temp.toInt();

//   saveVars();  //save them to a small internal file.
//   showMessage("","",CompleteSetupScreen);
// }

// void handle_NotFound() {
//   server.send(404, "text/plain", "Not found");
// }

// void showMessage(String msg1, String msg2, byte style) {
// String action;
// String msg0;
// String html=String(HTML_HEADER)+String(HTML_COMPLETE);

//   msg0="The data has been saved to the controller %OWNER%.";  //this is the same for the first two, so default it

//     switch (style) {
//     case CompleteGeneralScreen: 
//       action="/index";
//       break;
//     case CompleteSetupScreen: 
//       action="/setup";
//       break;
//     case WarningSetupScreen:
//       action="/setup";
//       msg0="WARNING %OWNER%.";
//       break;
//     }
  
//   html.replace("%ACTION%", action);       // Put in the correct action
//   html.replace("%NEWHEADER%", msg0);       // Put in the new heading
//   html.replace("%OWNER%", Owner);       // Put in the owners name
//   html.replace("%OWNERS%", Owners);       // Put in the owners name
//   html.replace("%NEWDATA1%", msg1+"&nbsp");  // Put in a blank
//   html.replace("%NEWDATA2%", msg2+"&nbsp");  // Put in a blank
//   server.send(200, "text/html", html);
//   delay(10);
// }