/* PacoWagon v1.6.4-Dutch
   (c) Paco Canada Feb.2018
   http://usuaris.tinet.cat/fmco/

This software and associated files is a DIY project not intended for commercial use.

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

This software uses libraries with different licenses, follow all their different terms included.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

// v1.6.4   01nov18 SSID and password accepts 64 characters
// v1.6.3   07jul18 Added 180º OLED rotation option. Changed speed calculation to micros().
// v1.6.2   16jun18 Solving network timeouts
// v1.6.1   08may18 Solving network and gyro fails
// v1.6     15apr18 Added MPU6050 gyro/accelerometer 6DOF
// v1.5     27mar18 Added OLED I2C display
// v1.4     26mar18 Show SSID on main page to distinguish wagons
// v1.4     22mar18 Added scale selection
// v1.3     17mar18 Added WiFi configuration and speed histogram. 256K SPIFFS
// v1.2     14mar18 Added 128K SPIFFS, pages and images in data directory.
// v1.1     06mar18 Added calibration page. Running into H0 wagon.
// v1.0     23feb18 Speed and distance with setup page for units.


// ------ User configuration ------------------------------

#define USE_MPU_VERT 1    // Uncomment this line if MPU6050 is mounted vertically, comment if it's horizontal mounting as usual
//#define ROTATE_OLED 1     // Uncomment to rotate 180º OLED display

//------- End of user configuration -----------------------


#include <ESP8266WiFi.h>
#include <ESP8266WebServer.h>
#include <DNSServer.h>        // used with http://www.something.com
#include <ESP8266mDNS.h>      // used with http://pacowagon.local
#include <EEPROM.h>
#include <FS.h>               // Include File System Headers
#include <Wire.h>             // I2C
#include <Adafruit_GFX.h>     // Pantalla OLED
#include <Adafruit_SSD1306.h>

// -------------- Program  --------------------------------

const char *SoftVer = "v1.6.4d";

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

#if (SSD1306_LCDHEIGHT != 64)
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
#endif

#define LowBattADC  2980    // LOW BATT 2.97V
ADC_MODE(ADC_VCC);          // For Vcc voltage read

#define N_160  1
#define TT_120 2
#define H0_87  3
#define N0_45  4
#define N1_32  5
#define G_22_5 6

const char ScaleName[][5]={"(?)","(N)","(TT)","(H0)","(0)","(1)","(G)"};

IPAddress    apIP(42, 42, 42, 42);  // Defining a static IP address: local & gateway
                                    // Default IP in AP mode is 192.168.4.1

/* This are the WiFi access point settings.  */
char AP_ssid[65] = "PacoWagon";
const char *AP_password = "pacowagon";

/* Don't set this wifi credentials. They are configurated at runtime and stored on EEPROM */
char ssid[65] = "";
char password[65] = "";
int ConnectSTA = 0;

const byte DNS_PORT = 53;

ESP8266WebServer server(80);        // Define a web server at port 80 for HTTP
DNSServer dnsServer;


const byte SENSOR_PIN = 2;          // GPIO2
const byte SDA_PIN = 3;             // GPIO3 (RX)
const byte SCL_PIN = 0;             // GPIO0

float speed=0, travel=0;
int veltype=0, traveltype=0;    // default: km/h (H0) & m
float L_cm=0.91, travelcm, velcms;
volatile unsigned long ticks=0, timetaken=0, lastInt=0; // used in ISR and loop
unsigned long dtime, prevtime=0, itime;
unsigned long msTime, media, sumatotal=0;
unsigned long currTick, lastTick=0;
unsigned long lastOLED=0;
int APSTAmode = 0, Scale = H0_87, OLED=0, OLEDdisp=0, MPU=0, flash=true;

unsigned long mputread=0;
float pitch_offset, roll_offset;
float pitch, roll;
unsigned long last_read_time;
float last_x_angle, last_y_angle, last_z_angle;                 // These are the filtered angles
float last_gyro_x_angle, last_gyro_y_angle, last_gyro_z_angle;  // Store the gyro angles to compare drift
float base_x_gyro = 0, base_y_gyro = 0, base_z_gyro = 0;        //  to calibrate the gyroscope sensor and accelerometer readings
float base_x_accel = 0, base_y_accel = 0, base_z_accel = 0;
int16_t ax, ay, az;                                             // Variables to store the values from the sensor readings
int16_t gx, gy, gz;
int16_t temperature;
unsigned long t_now;
#define GYRO_FACTOR 65.536                  // 32768 / 500dps
#define ACCEL_FACTOR 4096.0                 // 32768 / 8g
const float RADIANS_TO_DEGREES = 57.2958;   // 180 / 3.14159
const float DEGREES_TO_RADIANS = 0.01745;   // 3.14159 / 180


static const unsigned char PROGMEM wifi[] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xC0, 0x00, 0x00, 0x0F, 0xF0, 0x00, 0x00, 0x3F, 0xFC, 0x00,
0x00, 0x7F, 0xFE, 0x00, 0x07, 0xFF, 0xFF, 0x80, 0x1F, 0xFF, 0xFF, 0xF0, 0x3F, 0xFF, 0xC0, 0x08,
0x3F, 0xFF, 0x80, 0x04, 0x72, 0x49, 0x9F, 0x64, 0x72, 0x4F, 0x98, 0x02, 0x72, 0x49, 0x9F, 0x62,
0x78, 0x19, 0x98, 0x62, 0x79, 0x99, 0x98, 0x62, 0x79, 0x99, 0x98, 0x62, 0x3F, 0xFF, 0x80, 0x04,
0x3F, 0xFF, 0x00, 0x04, 0x1F, 0xFE, 0x00, 0x08, 0x07, 0xFF, 0xFF, 0xF0, 0x00, 0x7F, 0xFE, 0x00,
0x00, 0x3F, 0xFC, 0x00, 0x00, 0x0F, 0xF0, 0x00, 0x00, 0x03, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00
};

static const unsigned char mpu_wagon [] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0xFF, 0xFF, 0xFF,
0xFF, 0xF0, 0x08, 0x00, 0x00, 0x00, 0x00, 0x10, 0x08, 0x00, 0x00, 0x00, 0x00, 0x10, 0x0F, 0xFF,
0xFF, 0xFF, 0xFF, 0xF0, 0x08, 0x10, 0x21, 0x84, 0x08, 0x10, 0x08, 0x1F, 0xE1, 0x87, 0xF8, 0x10,
0x08, 0x10, 0x21, 0x84, 0x08, 0x10, 0x08, 0x1F, 0xE1, 0x87, 0xF8, 0x10, 0x08, 0x10, 0x21, 0x84,
0x08, 0x10, 0x08, 0x1F, 0xE1, 0x87, 0xF8, 0x10, 0x08, 0x10, 0x21, 0x84, 0x08, 0x10, 0x08, 0x10,
0x21, 0x84, 0x08, 0x10, 0x08, 0x10, 0x21, 0x84, 0x08, 0x10, 0x08, 0x10, 0x21, 0x84, 0x08, 0x10,
0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xF0, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC, 0x08, 0xFF, 0xB0, 0x0D,
0xFF, 0x10, 0x00, 0x7F, 0x0F, 0xF0, 0xFE, 0x10, 0x00, 0x7F, 0x00, 0x00, 0xFE, 0x00, 0x00, 0x1E,
0x00, 0x00, 0x78, 0x00, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x3C,
0x00, 0x00, 0x00, 0x00, 0x18, 0x06, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xFF, 0x00, 0x00, 0x00, 0x00,
0x22, 0x11, 0x00, 0x00, 0x00, 0x00, 0x22, 0x11, 0x00, 0x00, 0x00, 0x00, 0x22, 0x11, 0x00, 0x00,
0x00, 0x00, 0x22, 0x11, 0x00, 0x00, 0x00, 0x00, 0x22, 0x11, 0x00, 0x00, 0x00, 0x00, 0x22, 0x11,
0x00, 0x00, 0x00, 0x00, 0x22, 0x11, 0x00, 0x00, 0x00, 0x00, 0x22, 0x11, 0x00, 0x00, 0x00, 0x00,
0x22, 0x11, 0x00, 0x00, 0x00, 0x00, 0x22, 0x11, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xFF, 0x00, 0x00,
0x00, 0x00, 0x3F, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x2C, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x2C, 0x0D,
0x00, 0x00, 0x00, 0x00, 0x0F, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x0C, 0x00, 0x00, 0x00, 0x00,
0x04, 0x08, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

const unsigned char level [] = {
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0x3F, 0xFE, 0x00, 0x07, 0xFD, 0xDF, 0xFC,
0x40, 0x03, 0xFB, 0x6F, 0xFE, 0x00, 0x07, 0xFB, 0x6F, 0xFF, 0xFF, 0xFF, 0xFD, 0xDF, 0xFF, 0xFF,
0xFF, 0xFE, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x38, 0x00, 0x00, 0x00, 0x07, 0xC0, 0x00, 0x00, 0x01, 0xF8, 0x00, 0x00, 0x00, 0x3E, 0x00,
0x00, 0x00, 0x07, 0xC1, 0x00, 0x00, 0x01, 0xF8, 0x01, 0x00, 0x00, 0x0F, 0xFF, 0xFF, 0xFF, 0xF8
};

static const unsigned char logo [] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xFE, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0xFF, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xFF, 0xC0, 0x00,
0x3F, 0xF0, 0x00, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xE0, 0x00,
0x3F, 0xF8, 0x00, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xFF, 0xFF, 0xF0, 0x00,
0x3C, 0x7C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xFF, 0x8F, 0xF0, 0x00,
0x3C, 0x3C, 0xF8, 0x3F, 0x3C, 0x3F, 0x8F, 0xF8, 0x00, 0x00, 0x00, 0x0F, 0xFF, 0x80, 0x7C, 0x00,
0x3C, 0x3D, 0xFC, 0x7F, 0x3C, 0x7F, 0xCF, 0xFC, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0x00, 0x3E, 0x00,
0x3C, 0x3F, 0xDE, 0x78, 0x3C, 0xFB, 0xCF, 0xBC, 0x00, 0x00, 0x00, 0x1F, 0xF0, 0x00, 0x1F, 0x80,
0x3C, 0x3F, 0xDE, 0x3C, 0x3C, 0xF3, 0xCF, 0x3C, 0x00, 0x00, 0x00, 0x3F, 0xC0, 0x00, 0x07, 0xC0,
0x3C, 0x7F, 0xFE, 0x1F, 0x3C, 0xF3, 0xCF, 0x3C, 0x00, 0x00, 0x00, 0x7F, 0x80, 0x00, 0x03, 0xC0,
0x3C, 0x7B, 0xC0, 0x0F, 0x3C, 0xF7, 0xCF, 0x3C, 0x00, 0x00, 0x00, 0x7E, 0x00, 0x00, 0x03, 0xE0,
0x3F, 0xF1, 0xFC, 0xFF, 0x3C, 0xFF, 0xCF, 0x3C, 0x00, 0x00, 0x00, 0x7E, 0x00, 0x00, 0x01, 0xF0,
0x3F, 0xE0, 0xFC, 0xFE, 0x3C, 0x3F, 0xCF, 0x3C, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x01, 0xF8,
0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xC0, 0x00, 0x00, 0x00, 0x00, 0xF8, 0x00, 0x00, 0x00, 0xF8,
0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xC0, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x00, 0x00, 0x00, 0xF8,
0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x80, 0x00, 0x00, 0x00, 0x01, 0xF0, 0x00, 0x00, 0x00, 0xFC,
0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x01, 0xE0, 0x00, 0x00, 0x00, 0x7C,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xE0, 0x00, 0x00, 0x00, 0x7C,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xFC, 0x00, 0x00, 0x00, 0x7C,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xFF, 0x80, 0x00, 0x00, 0x7C,
0x00, 0x00, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xFF, 0xE0, 0x0F, 0xE0, 0x7E,
0x00, 0x00, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xFF, 0xE0, 0x1F, 0xFC, 0x3E,
0x00, 0x00, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xFF, 0xE0, 0x1F, 0xFE, 0x3E,
0x00, 0x00, 0x00, 0x0F, 0xF3, 0xCF, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xE1, 0xC0, 0x1F, 0xFF, 0x7E,
0x00, 0x00, 0x00, 0x0F, 0xF9, 0xCE, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0x80, 0x0E, 0x0F, 0x3E,
0x00, 0x00, 0x00, 0x0F, 0x7D, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0xE0, 0x3F, 0xFC, 0x3E,
0x00, 0x00, 0x00, 0x0F, 0x3D, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x1B, 0xFF, 0xF0, 0xFF, 0xFF, 0xFE,
0x00, 0x00, 0x00, 0x0F, 0x3C, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x1B, 0xFF, 0xBF, 0xE7, 0xF9, 0xFE,
0x00, 0x00, 0x00, 0x0F, 0x3C, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0xBF, 0xC7, 0xFC, 0xFE,
0x00, 0x00, 0x00, 0x0F, 0xF8, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFC, 0x31, 0xC7, 0xFC, 0xFE,
0x00, 0x00, 0x00, 0x0F, 0xF0, 0x78, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x80, 0x30, 0xC1, 0xFC, 0xFE,
0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x80, 0x70, 0xC0, 0x00, 0xFE,
0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x0F, 0xE0, 0x70, 0xE0, 0x00, 0xFE,
0x00, 0x00, 0x00, 0x00, 0x07, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x07, 0xFF, 0xE0, 0x60, 0x00, 0xFE,
0x00, 0x00, 0x00, 0x00, 0x07, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x06, 0xFF, 0xC0, 0x7E, 0x07, 0xFE,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x01, 0x80, 0x3F, 0xFF, 0xBC,
0x1F, 0xFF, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x01, 0x80, 0x00, 0xEC, 0x3C,
0x1F, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x01, 0xC0, 0x00, 0x00, 0x3C,
0x1F, 0xFF, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x01, 0xE1, 0x80, 0x00, 0x3C,
0x1F, 0xFF, 0xF8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x01, 0xFF, 0xC0, 0x00, 0x3C,
0x1F, 0xE7, 0xF8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x07, 0xFF, 0xFE, 0x00, 0x3C,
0x1F, 0xE7, 0xF8, 0x1F, 0xF8, 0x01, 0xFF, 0x00, 0x7F, 0x80, 0x0E, 0x0F, 0xFF, 0xFF, 0x80, 0x3C,
0x1F, 0xE7, 0xF8, 0x7F, 0xFE, 0x07, 0xFF, 0x81, 0xFF, 0xF0, 0x0E, 0x1F, 0xFF, 0xFF, 0xC0, 0x3C,
0x1F, 0xE7, 0xF8, 0xFF, 0xFF, 0x0F, 0xFF, 0x87, 0xFF, 0xF8, 0x0F, 0x1F, 0xFF, 0xFF, 0xC0, 0x7C,
0x1F, 0xEF, 0xF8, 0xE0, 0x7F, 0x1F, 0xFF, 0x87, 0xFF, 0xFC, 0x0F, 0x3F, 0xFF, 0xFF, 0xC0, 0x7C,
0x1F, 0xFF, 0xF0, 0x00, 0x7F, 0x1F, 0xFF, 0x8F, 0xFF, 0xFC, 0x0F, 0x3F, 0xFF, 0xFF, 0xC0, 0x78,
0x1F, 0xFF, 0xF0, 0x00, 0x7F, 0x3F, 0xE1, 0x9F, 0xF1, 0xFE, 0x0F, 0xFF, 0xFF, 0xFF, 0xC0, 0xF8,
0x1F, 0xDF, 0xE0, 0x01, 0xFF, 0x3F, 0x80, 0x1F, 0xC0, 0xFE, 0x0F, 0xFF, 0xE0, 0x0F, 0xC1, 0xF8,
0x1F, 0xDF, 0x80, 0x3F, 0xFF, 0x3F, 0x80, 0x1F, 0xC0, 0xFE, 0x07, 0xFF, 0xFF, 0xFF, 0xE3, 0xF0,
0x1F, 0xC0, 0x00, 0xFF, 0xFF, 0x3F, 0x80, 0x1F, 0xC0, 0xFE, 0x07, 0xFF, 0xFF, 0xFF, 0xFF, 0xF0,
0x1F, 0xC0, 0x01, 0xFF, 0xFE, 0x3F, 0x80, 0x1F, 0xC1, 0xFE, 0x03, 0xFF, 0xFF, 0xFF, 0xFF, 0xE0,
0x1F, 0xC0, 0x03, 0xFF, 0x7E, 0x3F, 0xC1, 0x9F, 0xE3, 0xFE, 0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0xE0,
0x1F, 0xC0, 0x03, 0xF8, 0x7E, 0x1F, 0xFF, 0x8F, 0xFF, 0xFC, 0x01, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0,
0x1F, 0xC0, 0x03, 0xF8, 0x7E, 0x1F, 0xFF, 0x0F, 0xFF, 0xFC, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0x80,
0x1F, 0xC0, 0x03, 0xFF, 0x7E, 0x0F, 0xFF, 0x07, 0xFF, 0xF8, 0x00, 0x7F, 0xFF, 0xFF, 0xFF, 0x80,
0x1F, 0xC0, 0x01, 0xFE, 0xFE, 0x07, 0xFF, 0x03, 0xFF, 0xE0, 0x00, 0x7F, 0xFF, 0xFF, 0xFF, 0x00,
0x1F, 0xC0, 0x00, 0xFE, 0xFE, 0x01, 0xFE, 0x00, 0x7F, 0x80, 0x00, 0x3F, 0xFF, 0xFF, 0xFE, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0xFF, 0xFC, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0xFF, 0xF8, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0xFF, 0xFF, 0xF0, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xFF, 0xFF, 0x80, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xFF, 0xFF, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xF8, 0x00, 0x00
};


void setup() {
  int n;
  byte error;

  WiFi.mode(WIFI_OFF);
  pinMode(SENSOR_PIN, INPUT_PULLUP);
  pinMode(SDA_PIN, INPUT_PULLUP);
  pinMode(SCL_PIN, INPUT_PULLUP);
  delay (10);
  Serial.begin(115200, SERIAL_8N1, SERIAL_TX_ONLY);   // Only TX pin (GPIO1)
  delay(1000);
  Serial.println();
  Serial.print("PacoWagon ");
  Serial.println(SoftVer);

  loadCredentials();  // read EEPROM values

  Wire.begin(SDA_PIN, SCL_PIN);   //I2C
  Wire.beginTransmission(0x3C);   // OLED installed?
  error = Wire.endTransmission();
  if (error) {
    Serial.println ("OLED display not found");
    OLED=0;
    OLEDdisp=0;
  }
  else {
    Serial.println ("Initializing OLED display...");
    // by default, we'll generate the high voltage from the 3.3v line internally! (neat!)
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3D (for the 128x64)
    OLED=1;       // OLED present
    OLEDdisp=1;   // OLED display mode default
    #ifdef ROTATE_OLED
    display.setRotation (2);
    Serial.println ("Turning 180º OLED display");
    #endif
    display.clearDisplay();
    display.drawBitmap(0, 0, logo, 128, 64, WHITE);
    display.display();
    display.invertDisplay(true);
    delay (3000);
    display.setTextColor(WHITE);
    display.clearDisplay();
    display.setCursor(10,4);
    display.setTextSize(2);     // text size 1: 6x8 font (8 lines x 21 characters), text size 2: 12x16, text size 3: 18x24,...
    display.println ("PacoWagon");
    display.setCursor(52,24);
    display.setTextSize(1);
    display.println (SoftVer);
    display.drawBitmap(48, 36, wifi, 32, 24, WHITE);
    display.display();
    delay (3000);
  }

  Wire.beginTransmission(0x68);   // MPU6050 installed?
  error = Wire.endTransmission();
  if (error) {
    Serial.println ("MPU6050 not found");
    MPU=0;
  }
  else {
    Serial.println ("Initializing MPU6050...");
    MPU=1;
    setup_mpu_6050_registers();  
    Serial.print("Calibrating gyro");  
    if (OLED==1) {
      display.fillRect (48,36,32,24,BLACK);
      display.setTextSize (1);
      display.setCursor (20,44);
      display.print ("Gyro kalibreren");
      display.display();
    }
    else {
      delay (4000);     // Esperar un poco antes de la calibración 
    }
    calibrate_sensors();
    mputread = millis();
    lastOLED = mputread + 3; // Evita coincidir refresco pantalla con lectura gyro
  }

  if (OLED==0 && MPU==0) {
    pinMode(SDA_PIN, INPUT_PULLUP);
    pinMode(SCL_PIN, INPUT_PULLUP);
  }

  //Initialize File System
  SPIFFS.begin();
  Serial.println("File System Initialized");
  ListSpiffs();
   
  WiFi.mode(WIFI_AP_STA);
  if ((ConnectSTA > 0) && (strlen(ssid) > 0)) {   // Router connection defined?
    Serial.print("Connecting to ");
    Serial.println(ssid);
    if (OLED==1) {
      display.clearDisplay();
      yield();
      display.drawBitmap(48, 6, wifi, 32, 24, WHITE);
      display.setCursor (4,36);
    //display.print ("123456789012345678901");
      display.print ("Verbinding maken ");
      display.print ("met de Router ");
      n = 128 - (6 * strlen (ssid));
      display.setCursor (n<0 ? 0 : n/2 ,48);
      display.print (ssid); 
      yield();
      display.display();
      yield();
    }
    WiFi.begin(ssid, password);
    n=0;
    while ((WiFi.status() != WL_CONNECTED) && n<40) { // tries to connect to router in 20 seconds
      n++;
      delay(500);
      Serial.print(".");
    }
  }

  if (WiFi.status() == WL_CONNECTED) { 
    Serial.println("");
    Serial.println("WiFi connected");
    Serial.print("IP address: ");
    Serial.println(WiFi.localIP());
    Serial.print("Channel: ");
    Serial.println(WiFi.channel());
    Serial.println("Configuring the access point...");
    WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0));   // subnet FF FF FF 00
    WiFi.softAP(AP_ssid, AP_password, WiFi.channel());
    if (MDNS.begin(AP_ssid)) {     //If WiFi connected to hot spot then start mDNS
      Serial.println("MDNS started");
      MDNS.addService("http", "tcp", 80);
      }
    APSTAmode=1;
    }
  else {
      APSTAmode=0;
      Serial.println("\nConfiguring only as access point...");
      //set-up the custom IP address
      WiFi.mode(WIFI_AP);
      WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0));   // subnet FF FF FF 00  
      WiFi.softAP(AP_ssid, AP_password);   /* You can remove the password parameter if you want the AP to be open. */
    }

  if (OLED==1) {
    display.invertDisplay(false);
    display.clearDisplay();
    yield();
    display.setTextSize(1);     // text size 1: 6x8 font (8 lines x 21 characters), text size 2: 12x16, text size 3: 18x24,...
    display.setCursor(24,0);
    display.print ("PacoWagon");
    display.setCursor(0,16);
    display.print ("AP: ");
    display.print (AP_ssid);
    display.setCursor(0,24);
    display.print ("IP: ");
    display.print (apIP);
    if (APSTAmode==1) {
      display.setCursor(0,40);
      display.print ("STA:");
      display.print (ssid);
      display.setCursor(0,48);
      display.print ("IP: ");
      display.print (WiFi.localIP());
      display.setCursor(0,56);
      display.print (AP_ssid);
      display.print (".local");
    }
    yield();
    display.display();
    delay (4000);

  }

  
  //modify TTL associated  with the domain name (in seconds)
  // default is 60 seconds
  dnsServer.setTTL(300);
  // set which return code will be used for all other domains (e.g. sending
  // ServerFailure instead of NonExistentDomain will reduce number of queries
  // sent by clients) default is DNSReplyCode::NonExistentDomain
  dnsServer.setErrorReplyCode(DNSReplyCode::ServerFailure);
  // start DNS server for a specific domain name
  dnsServer.start(DNS_PORT, "*", apIP);

  IPAddress myIP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(myIP);

  server.on ( "/", handleRoot );
  server.on ( "/status", handleStatus);
  server.on ( "/reset", handleReset);
  //server.on ( "/setup", handleSetup);
  server.on ( "/setval", handleSetupValue);
  //server.on ( "/calibrate", handleCalibrate);
  server.on ( "/setwifi", handleSetupWiFi);
  server.on ( "/scan", handleScanWiFi);
  server.onNotFound ( handleNotFound );
  
  server.begin();
  Serial.println("HTTP server started");

  ticks=0;
  attachInterrupt(digitalPinToInterrupt(SENSOR_PIN), measure_speed, FALLING);
}

void loop() {

  dnsServer.processNextRequest();
  yield();
  server.handleClient();
  yield();
  currTick = ticks;
  travelcm = L_cm * currTick;
  switch (traveltype) {
    case 0:
      travel = travelcm / 100;    // m
      break;
    case 1:
      switch (Scale) {
        case N_160:
          travel = travelcm * 0.0016; // km (N)   km = 160 * (1/100000) km/cm * travelcm
          break;
        case TT_120:
          travel = travelcm * 0.0012; // km (TT)   km = 120 * (1/100000) km/cm * travelcm
          break;
        case  H0_87:
          travel = travelcm * 0.00087; // km (H0)   km = 87 * (1/100000) km/cm * travelcm
          break;
        case N0_45:
          travel = travelcm * 0.00045; // km (0)   km = 45 * (1/100000) km/cm * travelcm
          break;
        case N1_32:
          travel = travelcm * 0.00032; // km (1)   km = 32 * (1/100000) km/cm * travelcm
          break;
        case G_22_5:
          travel = travelcm * 0.000225; // km (G)   km = 22.5 * (1/100000) km/cm * travelcm
          break;
      }
      break;
    case 2:
      travel = travelcm;           // cm
      break;
  }
  yield();
  msTime = timetaken;
  if (currTick != lastTick) {
    if (msTime) {
      lastTick = currTick;
      media = sumatotal / 4;        // medium value from 4 last readings
      media = sumatotal - media + msTime;
      velcms = (1000000 * L_cm)  / media;
    }
  }
  if (veltype)
    speed = velcms;               // cm/s
  else  {
    switch (Scale)  {
      case N_160:
        speed = velcms * 5.76;       // km/h (N)    km/h = 160 * (1/100000) km/cm * 3600 s/h * vel cm/s
        break;
      case TT_120:
        speed = velcms * 4.32;       // km/h (TT)   km/h = 120 * (1/100000) km/cm * 3600 s/h * vel cm/s
        break;
      case H0_87:
        speed = velcms * 3.132;      // km/h (H0)   km/h = 87 * (1/100000) km/cm * 3600 s/h * vel cm/s
        break;
      case N0_45:
        speed = velcms * 1.62;       // km/h (0)    km/h = 45 * (1/100000) km/cm * 3600 s/h * vel cm/s
        break;
      case N1_32:
        speed = velcms * 1.152;      // km/h (1)    km/h = 32 * (1/100000) km/cm * 3600 s/h * vel cm/s
        break;
      case G_22_5:
        speed = velcms * 0.81;       // km/h (G)    km/h = 22.5 * (1/100000) km/cm * 3600 s/h * vel cm/s
        break;
    }
  }
  if ((micros() - lastInt) > 4000000) {  // 4s from last interrupt? -> Wagon stopped
    speed = 0;
    timetaken=0;
    velcms=0;
    lastInt = micros();
  }
  yield();

  if (millis() - lastOLED > 1000) {   // Update display every second
    if (OLED==1)
      DisplayOLED();
    lastOLED = millis();
    flash = !flash;
  }

  if (millis() - mputread > 5) {   // cada 10ms read MPU
    if (MPU==1) 
      calc_mpu_angle ();
    mputread = millis();
   }
  
} 


void measure_speed() {            // interrupt routine, counts time from last change in opto
  dtime = micros();
  itime = dtime - prevtime;
  if (itime > 3999) {                // min. 4ms between falling edges
    ticks++;
    timetaken = itime;
    prevtime = dtime;
  }
  lastInt = dtime;
}

//------------- EEPROM routines -----------------------------------------

void loadCredentials() {
  EEPROM.begin(512);  // reading EEPROM
  EEPROM.get(0, L_cm);
  EEPROM.get(0 + sizeof(L_cm), Scale);
  EEPROM.get(0 + sizeof(L_cm) + sizeof (Scale), ssid);
  EEPROM.get(0 + sizeof(L_cm) + sizeof (Scale) + sizeof(ssid), password);
  EEPROM.get(0 + sizeof(L_cm) + sizeof (Scale) + sizeof(ssid) + sizeof(password), ConnectSTA);
  EEPROM.get(0 + sizeof(L_cm) + sizeof (Scale) + sizeof(ssid) + sizeof(password) + sizeof(ConnectSTA), AP_ssid);
  char ok[2+1];
  EEPROM.get(0 + sizeof(L_cm) + sizeof (Scale) + sizeof(ssid) + sizeof(password) + sizeof(ConnectSTA)+ sizeof(AP_ssid), ok);
  EEPROM.end();
  if (String(ok) != String("OK")) {   // Values valid?
    L_cm=0.91;
    Scale = H0_87;    // Scale H0 default
    ssid[0] = 0;
    password[0] = 0;
    ConnectSTA=0;
    snprintf (AP_ssid,64,"PacoWagon");
    Serial.println ("EEPROM blank");
    }
  Serial.print ("L value: ");
  Serial.println (L_cm);
  Serial.print ("Scale: ");
  Serial.println (ScaleName[Scale]);
  Serial.println("Recovered credentials: ");
  Serial.println(AP_ssid);  
  Serial.println(strlen(ssid) > 0 ? ssid : "<no SSID>");
  Serial.println(strlen(password) > 0 ? password : "<no password>");
  Serial.println(ConnectSTA > 0 ? "AP and STA mode" : "Only AP mode");  
}

void saveCredentials() {
  Serial.print ("Saving to EEPROM..");
  EEPROM.begin(512);
  EEPROM.put (0,L_cm);
  EEPROM.put(0 + sizeof(L_cm), Scale);
  EEPROM.put(0 + sizeof(L_cm) + sizeof (Scale), ssid);
  EEPROM.put(0 + sizeof(L_cm) + sizeof (Scale) + sizeof(ssid), password);
  EEPROM.put(0 + sizeof(L_cm) + sizeof (Scale) + sizeof(ssid) + sizeof(password), ConnectSTA);
  EEPROM.put(0 + sizeof(L_cm) + sizeof (Scale) + sizeof(ssid) + sizeof(password) + sizeof(ConnectSTA), AP_ssid);
  char ok[2+1] = "OK";    // Value to valifate EEPROM data
  EEPROM.put(0 + sizeof(L_cm) + sizeof (Scale) + sizeof(ssid) + sizeof(password) + sizeof(ConnectSTA) + sizeof (AP_ssid), ok);
  EEPROM.commit();
  EEPROM.end();
  Serial.println (" OK"); 
}



//------------- Handle webpages routines ---------------------------------------

void handleRoot() {
  if (loadFromSpiffs(server.uri())) {
      Serial.println("Main page");
  }
  else {
    String message = "404 - File Not Found\n\n";
    message += "URI: ";
    message += server.uri();
    message += "\nRoot file not found. Check URL or reprogram ESP8266\n";
    server.send ( 404, "text/plain", message );
    Serial.println (message);
  }
}

void handleStatus() {
  char html[200];
  float battery;
  char batttxt[10],travelunits[10],velunits[10],traveldist[10], mputxt[20];

  battery = ESP.getVcc ();    // Read VCC voltage
  if ((int)battery < LowBattADC)    // <2.95V ?
    snprintf ( batttxt,10,"LOW BATT");
  else
    snprintf ( batttxt,10,"%.2fV",battery/1000);
    
  if (veltype)
    snprintf ( velunits,10,"cm/s");
  else
    snprintf ( velunits,10,"km/h %s", ScaleName[Scale]);
    
  switch (traveltype) {
    case 0:
      snprintf ( traveldist, 10, "%.2f",travel);
      snprintf ( travelunits,10,"m");
      break;
    case 1:
      snprintf ( traveldist, 10, "%.3f",travel);
      snprintf ( travelunits,10,"km %s", ScaleName[Scale]);
      break;
    case 2:
      snprintf ( traveldist, 10, "%.1f",travel);
      snprintf ( travelunits,10,"cm");
      break;
  }
  if (MPU==1) {
    pitch = sin((last_x_angle - pitch_offset) * DEGREES_TO_RADIANS)*100;
    roll = sin((last_y_angle - roll_offset) * DEGREES_TO_RADIANS)*100;
    snprintf (mputxt,20, "#p=%.1f#q=%.1f", pitch, roll);
  }
  else {
    snprintf (mputxt,20, "");  
  }
  
  snprintf ( html, 200,"status#b=%s#s=%.1f#v=%s#t=%s#u=%s#r=%s%s", batttxt, speed, velunits, traveldist, travelunits, AP_ssid, mputxt);
  server.send ( 200, "text/html", html );
  Serial.print ("Status: ");
  Serial.println (html);
  
}


void handleSetupValue() {
  char html[200];
  char velunits[10],travelunits[10],calibration[10],wifissid[100];

  if (server.args()>0) {              // with arguments: /setval?but=1
    String argument = server.arg(0);
    Serial.println ("Setup button");
    Serial.print ("Arguments: ");
    Serial.println (server.args ());
    Serial.print ("Change value: ");
    Serial.println (argument);
    switch (argument.toInt()) {
      case 1:
        veltype++;
        if (veltype ==2) veltype=0;
        break;
      case 2:
        traveltype++;
        if (traveltype ==3) traveltype=0;
        break;
      case 3:
        if (OLED==1) {
          OLEDdisp++;
          if (OLEDdisp ==5 && MPU==0) OLEDdisp=1;
          if (OLEDdisp ==6) OLEDdisp=1;
        }
        else
          OLEDdisp=0;
        break;
      case 4:
        if (OLED==1) {
          display.clearDisplay();
          yield();
          display.drawBitmap(44, 16, level, 40, 16, WHITE);
          display.setTextSize (1);
          display.setCursor (20,44);
          display.print ("Gyro kalibreren");
          display.display();
          yield();
        }
        calibrate_sensors();          // Repeat calibration
        for (int i=0; i<300; i++) {
          delay (10);
          calc_mpu_angle ();          // Some readings
        }
        pitch_offset=last_x_angle;    // set offsets
        roll_offset =last_y_angle;
        break;
      case 9:
        argument = server.arg("d");
        Serial.print ("Real dist: ");
        Serial.println (argument);
        if (ticks == 0) ticks++;    // prevent division by 0
        L_cm = argument.toFloat() / ticks;
        
        argument = server.arg("s");
        Serial.print ("Scale: ");
        Serial.println (argument);
        Scale = argument.toInt();
        
        saveCredentials();
        break;
    }
  }

  if (veltype)                              // send setup values
    snprintf ( velunits,10,"cm/s");
  else
    snprintf ( velunits,10,"km/h %s", ScaleName[Scale]);
  switch (traveltype) {
    case 0:
      snprintf ( travelunits,10,"m");
      break;
    case 1:
      snprintf ( travelunits,10,"km %s", ScaleName[Scale]);
      break;
    case 2:
      snprintf ( travelunits,10,"cm");
      break;
  }
  snprintf ( calibration,10,"%.1f cm", travelcm);
  if (APSTAmode) {
      snprintf (wifissid, 100, "AP+STA: %s - %d.%d.%d.%d", strlen(ssid) > 0 ? ssid : "<no SSID>", WiFi.localIP()[0], WiFi.localIP()[1], WiFi.localIP()[2], WiFi.localIP()[3]);
  }
  else {
    snprintf (wifissid, 100, "AP: %s - %d.%d.%d.%d", AP_ssid, apIP[0],apIP[1],apIP[2],apIP[3]);
  }
  
  snprintf ( html, 200,"setval#s=%s#t=%s#c=%s#w=%s#o=%d#m=%d", velunits, travelunits, calibration, wifissid, OLEDdisp, MPU);
  server.send ( 200, "text/html", html );
  Serial.print ("Setup values: ");
  Serial.println (html);
  
}
 
void handleReset() {
  ticks=0;
  Serial.println("Reset counter");
  handleStatus();
}
  

void handleSetupWiFi() {
  char html[250];

  if (server.args()>0) {                    // with arguments: setwifi?but=8&a=PacoWagon&s=MyRouter&p=mypass&c=1
    String argument = server.arg(0);
    Serial.println ("WiFi save button");
    Serial.print ("Arguments: ");
    Serial.println (server.args ());
    Serial.print ("Change value: ");
    Serial.println (argument);
    switch (argument.toInt()) {
      case 8:
        Serial.print ("a=");
        argument  = server.arg ("a");
        argument.toCharArray(AP_ssid, 64);
        Serial.println (AP_ssid);
        Serial.print ("s=");
        argument  = server.arg ("s");
        argument.toCharArray(ssid, 64);
        Serial.println (ssid);
        Serial.print ("p=");
        argument  = server.arg ("p");
        argument.toCharArray(password, 64);
        Serial.println (password);
        Serial.print ("c=");
        argument  = server.arg ("c");
        ConnectSTA = argument.toInt ();
        Serial.println (ConnectSTA);
        saveCredentials();
        break;
    }
  }
  
  snprintf ( html, 250,"setwifi#a=%s#s=%s#p=%s#c=%d", AP_ssid, ssid, password, ConnectSTA); // send wifi values
  server.send ( 200, "text/html", html );
  Serial.print ("WiFi values: ");
  Serial.println (html);
}

void handleScanWiFi() {
  String Networks = "";

  Serial.println ("WiFi Scanning ...");
  WiFi.scanDelete();
  int n = WiFi.scanNetworks();
  if (n == 0) {
    Networks = "<font color='#FF0000'>Geen netwerken beschikbaar!</font>";
  }
  if (n >= 0) {
    Networks = "Gevonden " +String(n) + " Netwerk<hr>";
    Networks += "<table border='0' cellspacing='0' cellpadding='3' width='100%'>";
    Networks += "<tr bgcolor='#DDDDDD' ><td><strong>Naam</strong></td><td><strong>Signaal Sterkte</strong></td><td><strong>Slot</strong></td><tr>";
    for (int i = 0; i < n; ++i)
    {
      int quality=0;
      if(WiFi.RSSI(i) <= -100) {
          quality = 0; }
      else if(WiFi.RSSI(i) >= -50) {
              quality = 100; }
            else {
              quality = 2 * (WiFi.RSSI(i) + 100); }

      Networks += "<tr><td><a href='javascript:selssid(\""  +  String(WiFi.SSID(i))  + "\")'>"  +  String(WiFi.SSID(i))  + "</a></td><td>" +  String(quality) + "%</td><td>" +  String((WiFi.encryptionType(i) == ENC_TYPE_NONE)?" ":"<img src='padlock.png'>")  + "</td></tr>";
    }
    Networks += "</table>";
  }
  server.send ( 200, "text/plain", Networks);
  Serial.print ("Found ");
  Serial.print (n);
  Serial.println (" networks");
}


void handleNotFound() {
  if(loadFromSpiffs(server.uri())) return;    // search file in SPIFFS
  
  String message = "404 - File Not Found\n\n";
  message += "URI: ";
  message += server.uri();
  message += "\nMethod: ";
  message += ( server.method() == HTTP_GET ) ? "GET" : "POST";
  message += "\nArguments: ";
  message += server.args();       // 42.42.42.42/page?a=1&b=2&c=3
  message += "\n";

  for ( uint8_t i = 0; i < server.args(); i++ ) {
    message += " " + server.argName ( i ) + ": " + server.arg ( i ) + "\n";
  }
  server.send ( 404, "text/plain", message );
  Serial.print ("Not Found: ");
  Serial.println (message);
}


bool loadFromSpiffs(String path){
  Serial.println("Read File: " + path);
  if (path.endsWith("/")) path += "index.html";         // If a folder is requested, send the index file
  String contentType = getContentType(path);            // Get the MIME type
  if (SPIFFS.exists(path)) {                            // If the file exists
    File file = SPIFFS.open(path, "r");                 // Open it
    size_t sent = server.streamFile(file, contentType); // And send it to the client
    file.close();                                       // Then close the file again
    return true;
  }
  Serial.println("\tFile Not Found");
  return false;  
}

void ListSpiffs ()  {
  Dir dir = SPIFFS.openDir("/");
  while (dir.next()) {                      // List the file system contents
    String fileName = dir.fileName();
    size_t fileSize = dir.fileSize();
    Serial.printf("\tFile: %s,\tsize: %s\r\n", fileName.c_str(), formatBytes(fileSize).c_str());
  }
}

String getContentType(String filename){
  if(filename.endsWith(".htm")) return "text/html";
  else if(filename.endsWith(".html")) return "text/html";
  else if(filename.endsWith(".css")) return "text/css";
  else if(filename.endsWith(".js")) return "application/javascript";
  else if(filename.endsWith(".png")) return "image/png";
  else if(filename.endsWith(".gif")) return "image/gif";
  else if(filename.endsWith(".jpg")) return "image/jpeg";
  else if(filename.endsWith(".ico")) return "image/x-icon";
  else if(filename.endsWith(".xml")) return "text/xml";
  else if(filename.endsWith(".pdf")) return "application/x-pdf";
  else if(filename.endsWith(".zip")) return "application/x-zip";
  else if(filename.endsWith(".gz")) return "application/x-gzip";
  return "text/plain";
}

String formatBytes(size_t bytes) {  // convert sizes in bytes to KB and MB
  if (bytes < 1024) {
    return String(bytes) + "B";
  } else if (bytes < (1024 * 1024)) {
    return String(bytes / 1024.0) + "KB";
  } else {
    return String(bytes / 1024.0 / 1024.0) + "MB";
  }
}

//---------- OLED routines ---------------------------------------------------------------------------

void  DisplayOLED() {
  float battery;
  char batttxt[10],speedtxt[10],travelunits[10],velunits[10],traveldist[10], pitchtxt[10], rolltxt[10];
  int spdlng, spulng, trulng, trdlng,pitlng, rollng, x; 
  
  display.clearDisplay();
  yield();
  display.setTextSize(1);     // text size 1: 6x8 font (8 lines x 21 characters), text size 2: 12x16, text size 3: 18x24,...
  display.setCursor(0,0);
  display.print("PacoWagon");
  battery = ESP.getVcc ();    // Read VCC voltage
  if (((int)battery < LowBattADC) && flash)  {    // <2.97V ?
    snprintf (batttxt,10,"LOW BATT");
    display.setCursor(80,0);
    }
  else {
    snprintf ( batttxt,10,"%.2fV",battery/1000);
    display.setCursor(98,0);
    }

  display.print(batttxt);
  yield();
  snprintf (speedtxt,10,"%.1f",speed);
  if (veltype)
    snprintf ( velunits,10,"cm/s");
  else
    snprintf ( velunits,10,"km/h%s", ScaleName[Scale]);
    
  switch (traveltype) {
    case 0:
      snprintf ( traveldist, 10, "%.2f",travel);
      snprintf ( travelunits,10,"m");
      break;
    case 1:
      snprintf ( traveldist, 10, "%.3f",travel);
      snprintf ( travelunits,10,"km%s", ScaleName[Scale]);
      break;
    case 2:
      snprintf ( traveldist, 10, "%.1f",travel);
      snprintf ( travelunits,10,"cm");
      break;
  }
  pitch = sin((last_x_angle - pitch_offset) * DEGREES_TO_RADIANS)*100;
  roll = sin((last_y_angle - roll_offset) * DEGREES_TO_RADIANS)*100;
  snprintf ( pitchtxt,10,"%.1f", pitch);
  snprintf ( rolltxt, 10,"%.1f", roll);
  
  spdlng = strlen (speedtxt);
  spulng = strlen (velunits);
  trdlng = strlen (traveldist);
  trulng = strlen (travelunits);
  pitlng = strlen (pitchtxt);
  rollng = strlen (rolltxt);
  yield();  
  
  switch (OLEDdisp){
    case 1:   // speed and distance
      x = 126 - ((spdlng*12) + (spulng*6));
      display.setCursor(x/2,20);
      display.setTextSize(2);
      display.print(speedtxt);
      display.setCursor((x/2) + (spdlng*12) + 2,27);
      display.setTextSize(1);
      display.print(velunits);

      x = 126 - ((trdlng*12) + (trulng*6));
      display.setCursor(x/2,46);
      display.setTextSize(2);
      display.print(traveldist);
      display.setCursor((x/2) + (trdlng*12) + 2,53);
      display.setTextSize(1);
      display.print(travelunits);      
      break;
    
    case 2:   // big speed and small distance
      x = 128 - (spdlng*18);
      display.setCursor(x/2,13);
      display.setTextSize(3);
      display.print(speedtxt);
      x = 128 - (spulng*12);
      display.setCursor(x/2,38);
      display.setTextSize(2);
      display.print(velunits);
      x = 120 - ((trdlng + trulng)*6);
      display.setCursor(x/2,56);
      display.setTextSize(1);
      display.print(traveldist);
      display.print(" ");
      display.print(travelunits);
      break;

    case 3:   // big speed
      x = 128 - (spdlng*24);
      display.setCursor(x/2,13);
      display.setTextSize(4);
      display.print(speedtxt);
      x = 128 - (spulng*12);
      display.setCursor(x/2,48);
      display.setTextSize(2);
      display.print(velunits);
      break;

    case 4:   // big distance and small speed
      x = 128 - (trdlng*18);
      display.setCursor(x/2,13);
      display.setTextSize(3);
      display.print(traveldist);
      x = 128 - (trulng*12);
      display.setCursor(x/2,38);
      display.setTextSize(2);
      display.print(travelunits);
      x = 120 - ((spdlng + spulng)*6);
      display.setCursor(x/2,56);
      display.setTextSize(1);
      display.print(speedtxt);
      display.print(" ");
      display.print(velunits);
      break;
      
    case 5:   // pitch and roll
      display.drawBitmap(0, 16, mpu_wagon, 48, 48, WHITE);
      yield();
      x = 80 - (pitlng*12);
      display.setCursor(42+x/2,20);
      display.setTextSize(2);
      display.print(pitchtxt);
      display.print("%");
      //display.drawChar(display.getCursorX(), display.getCursorY()-4, 0x09, WHITE, BLACK,2); // º
      x = 80 - (rollng*12);
      display.setCursor(42+x/2,46);
      display.setTextSize(2);
      display.print(rolltxt);
      display.print("%");
      //display.drawChar(display.getCursorX(), display.getCursorY()-4, 0x09, WHITE, BLACK, 2); // º
      break;
  }
  yield();
  display.display();
  yield();
}

//------------ MPU6050 gyro routines ---------------------------------------------------------

void setup_mpu_6050_registers(){
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);                       //Start communicating with the MPU-6050
  Wire.write(0x6B);                                   //Send the requested starting register
  Wire.write(0x00);                                   //Set the requested starting register
  Wire.endTransmission();                             //End the transmission

  //Configure the accelerometer (+/-8g)               // Other confifuration
  Wire.beginTransmission(0x68);                  
  Wire.write(0x1C);                         
  Wire.write(0x10);                    
  Wire.endTransmission();               
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);                                
  Wire.write(0x1B);                         
  Wire.write(0x08);            
  Wire.endTransmission();

}

void read_mpu_6050_data(){                            //Subroutine for reading the raw gyro and accelerometer data
  yield();
  Wire.beginTransmission(0x68);                       //Start communicating with the MPU-6050
  Wire.write(0x3B);                                   //Send the requested starting register
  Wire.endTransmission(false);                        //End the transmission
  Wire.requestFrom((int) 0x68,(int) 14, (int) true);  //Request 14 bytes from the MPU-6050
#ifdef USE_MPU_VERT
  az = Wire.read()<<8|Wire.read();                    //Vertical mount, exchange X and Z axis
  ay = Wire.read()<<8|Wire.read();                    //      Z Y         X Y
  ax = - (Wire.read()<<8|Wire.read());                //      |/_ X     Z_|/
  temperature = Wire.read()<<8|Wire.read();           //      Horiz     Vert
  gz = Wire.read()<<8|Wire.read();                    
  gy = Wire.read()<<8|Wire.read();                    
  gx = - (Wire.read()<<8|Wire.read());                
#else
  ax = Wire.read()<<8|Wire.read();                    //Add the low and high byte to the acc_x variable
  ay = Wire.read()<<8|Wire.read();                    //Add the low and high byte to the acc_y variable
  az = Wire.read()<<8|Wire.read();                    //Add the low and high byte to the acc_z variable
  temperature = Wire.read()<<8|Wire.read();           //Add the low and high byte to the temperature variable
  gx = Wire.read()<<8|Wire.read();                    //Add the low and high byte to the gyro_x variable
  gy = Wire.read()<<8|Wire.read();                    //Add the low and high byte to the gyro_y variable
  gz = Wire.read()<<8|Wire.read();                    //Add the low and high byte to the gyro_z variable
#endif

}

void calibrate_sensors() {
  int num_readings = 200;
   
  read_mpu_6050_data();                       // Discard the first reading
  for (int i = 0; i < num_readings; i++) {    // Read and average the raw values
    read_mpu_6050_data();
    base_x_gyro += gx;
    base_y_gyro += gy;
    base_z_gyro += gz;
    base_x_accel += ax;
    base_y_accel += ay;
    base_y_accel += az;
  }
  base_x_gyro /= num_readings;
  base_y_gyro /= num_readings;
  base_z_gyro /= num_readings;
  base_x_accel /= num_readings;
  base_y_accel /= num_readings;
  base_z_accel /= num_readings;
  
  set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);
}

void set_last_read_angle_data(unsigned long time, float x, float y, float z, float x_gyro, float y_gyro, float z_gyro) {
  last_read_time = time;
  last_x_angle = x;
  last_y_angle = y;
  //last_z_angle = z;
  last_gyro_x_angle = x_gyro;
  last_gyro_y_angle = y_gyro;
  //last_gyro_z_angle = z_gyro;
}


void calc_mpu_angle () {
  read_mpu_6050_data();
  t_now = millis();   // Get time of last raw data read
  yield();
  // Remove offsets and scale gyro data  
  float gyro_x = (gx - base_x_gyro)/GYRO_FACTOR;
  float gyro_y = (gy - base_y_gyro)/GYRO_FACTOR;
  //float gyro_z = (gz - base_z_gyro)/GYRO_FACTOR;
  float accel_x = ax; // - base_x_accel;
  float accel_y = ay; // - base_y_accel;
  float accel_z = az; // - base_z_accel;

  float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
  float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES;
  //float accel_angle_z = 0;
  yield();
  // Compute the (filtered) gyro angles
  float dt =(t_now - last_read_time)/1000.0;
  float gyro_angle_x = gyro_x*dt + last_x_angle;
  float gyro_angle_y = gyro_y*dt + last_y_angle;
  //float gyro_angle_z = gyro_z*dt + last_z_angle;

  // Compute the drifting gyro angles
  float unfiltered_gyro_angle_x = gyro_x*dt + last_gyro_x_angle;
  float unfiltered_gyro_angle_y = gyro_y*dt + last_gyro_y_angle;
  //float unfiltered_gyro_angle_z = gyro_z*dt + last_gyro_z_angle;   

  // Apply the complementary filter to figure out the change in angle - choice of alpha is
  // estimated now.  Alpha depends on the sampling rate...
  //const float alpha = 0.96;
  float angle_x = 0.98*gyro_angle_x + 0.02*accel_angle_x;
  float angle_y = 0.98*gyro_angle_y + 0.02*accel_angle_y;
  //float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle

  // Update the saved data with the latest values
  set_last_read_angle_data(t_now, angle_x, angle_y, 0, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, 0); 
  yield();
}

