#define ESP32
//#define LEFT_DEVICE
#define DEBUG_SAMPLING


#include "Wire.h"
#include "HX711.h"
//Libraries for OLED Display
#include <TFT_eSPI.h>
#include <SPI.h>
#include "esp_timer.h"
#include <Preferences.h>

#ifdef CORE_DEBUG_LEVEL
#undef CORE_DEBUG_LEVEL
#endif

#define CORE_DEBUG_LEVEL 2
#define LOG_LOCAL_LEVEL ESP_LOG_INFO

#include "esp_log.h"
#include <math.h>


#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>

#ifndef TFT_DISPOFF
#define TFT_DISPOFF 0x28
#endif

#ifndef TFT_SLPIN
#define TFT_SLPIN   0x10
#endif
// PIN DEFINITIONS
#define TFT_MOSI            19
#define TFT_SCLK            18
#define TFT_CS              5
#define TFT_DC              16
#define TFT_RST             23

#define TFT_BL              4   // Display backlight control pin
#define ADC_EN              14  // ADC_EN is the ADC detection enable port
#define ADC_PIN             34
#define ADC_POWER           34

#ifdef DEBUG_SAMPLING
#define SAMPLE_INDICATOR    27
#define SAMPLE_INDICATOR_1  26
#endif

#ifdef LEFT_DEVICE
static const char* TAG = "SyncGrip L";
#define BUTTON_1            (gpio_num_t)35
#define BUTTON_2            (gpio_num_t)0
#else
static const char* TAG = "SyncGrip R";
#define BUTTON_1            (gpio_num_t)35
#define BUTTON_2            (gpio_num_t)0
#endif



TFT_eSPI tft = TFT_eSPI(135, 240); // Invoke custom library

Preferences preferences;

#if CONFIG_IDF_TARGET_ESP32
  #define THRESHOLD   40      /* Greater the value, more the sensitivity */
#else //ESP32-S2 and ESP32-S3 + default for other chips (to be adjusted) */
  #define THRESHOLD   5000   /* Lower the value, more the sensitivity */
#endif

RTC_DATA_ATTR int bootCount = 0;
touch_pad_t touchPin;

#define SDA_1 21
#define SCL_1 22

// Module connection pins (Digital Pins)
#define CLK 22
#define DIO 21

enum DeviceState {UNCONNECTED, CONNECTED, CALIBRATE};
enum DeviceState state = UNCONNECTED;

int old_btn_state =1 ;
int btn_state = 0;

float reading;
float maxReading;


uint64_t nLoopRuns = 0;
uint64_t nSamples = 0 ;
int missedSamples =0;

HX711 scale;

uint32_t scaleOffset;
float scaleScaler;

// TIME SECTION
int64_t time_since_boot;
esp_timer_handle_t button_timer;
esp_timer_handle_t sampling_timer;

static void button_timer_callback(void* arg);
static void sampling_timer_callback(void* arg);


// BLE SECTION
BLEServer *pServer = NULL;

BLECharacteristic *sensor_val_characteristic = NULL;
BLECharacteristic *bat_val_characteristic = NULL;
BLECharacteristic *sync_characteristic = NULL;
BLECharacteristic *calibrate_characteristic = NULL;

// See the following for generating UUIDs:
// https://www.uuidgenerator.net/

#define SERVICE_UUID "00567f83-72b0-4f28-b7ef-1fce18766da6"
#define SENSOR_VAL_CHARACTERISTIC_UUID "082c9546-6a26-4230-a7b2-8d13fa96da55"
#define BAT_VAL_CHARACTERISTIC_UUID "53aec279-cf8f-423f-bf13-38f778696fa0"
#define SYNC_CHARACTERISTIC_UUID "f1a13d24-57f0-4264-96bf-2c97c8a37561"
#define CALIBRATE_CHARACTERISTIC_UUID "89cc5093-096a-4d3e-b7db-18a979bb81fb"

// TIMERS
void initButtonTimer(){
 const esp_timer_create_args_t button_timer_args = {
            .callback = &button_timer_callback,
            /* name is optional, but may help identify the timer when debugging */
            .name = "buttonpoll"
    };
    ESP_ERROR_CHECK(esp_timer_create(&button_timer_args, &button_timer));
		ESP_ERROR_CHECK(esp_timer_start_periodic(button_timer, 100000));//every 100ms
}

void initSamplingTimer(){
	const esp_timer_create_args_t sampling_timer_args = {
		.callback = &sampling_timer_callback,
		.name = "sampling"
	};
	ESP_ERROR_CHECK(esp_timer_create(&sampling_timer_args, &sampling_timer));
	ESP_ERROR_CHECK(esp_timer_start_periodic(sampling_timer, 105000));//every 105ms
}

// BLE CALLBACKS
class MyServerCallbacks : public BLEServerCallbacks
{
  void onConnect(BLEServer *pServer)
  {
    Serial.println("Connected");
		state = CONNECTED;
  };

  void onDisconnect(BLEServer *pServer)
  {
		state = UNCONNECTED;
    Serial.println("Disconnected, starting advertising again...");
		// Start advertising
		pServer->getAdvertising()->start();
  }
};


float measureBattery(){
	// measures with 12 bit ADC on a voltage divider with 3.3V power supply
	uint16_t readVal = analogRead(ADC_POWER);
	float vBat =  2.*3.3*((0.0+readVal) / 4096.0);
	ESP_LOGE(TAG, "Measure BAT: %d %fV", readVal, vBat);
	return vBat;
}

int batteryPercent(float vBat){
	return int(100*(vBat-3.3)/(4.2-3.3));
}

bool screenon = true;

void toggleScreen(){
  screenon = !screenon;
    if (screenon){
      digitalWrite(TFT_BL, TFT_BACKLIGHT_ON);
		}
		else {
      digitalWrite(TFT_BL, !TFT_BACKLIGHT_ON);
		}
}

void tare(){
	state = CALIBRATE;
	tft.fillScreen(TFT_BLACK);
	tft.drawString("TARE", 120,32);
	scale.tare(20);
	uint32_t offset = scale.get_offset();
	preferences.begin("calibration", false); //false for r/w mode instead of just r
	preferences.putULong("offset", offset);
	preferences.end();
	tft.fillScreen(TFT_BLACK);
	tft.drawString("DONE", 120,32);
	delay(500);
	state = CONNECTED;
	Serial.println("DONE TARE");
}

void calibrate(int weightGrams){
	state = CALIBRATE;
	tft.fillScreen(TFT_BLACK);
	tft.drawString("calibrate", 120,32);
	scale.calibrate_scale(weightGrams, 20);
	float scaler = scale.get_scale();
	preferences.begin("calibration", false); //false for r/w mode instead of just r
	preferences.putFloat("scale", scaler);
	preferences.end();
	tft.fillScreen(TFT_BLACK);
	tft.drawString("DONE", 120,32);
	delay(500);
	state = CONNECTED;
}

class BatteryCharacteristicCallbacks: public BLECharacteristicCallbacks {
	void onWrite(BLECharacteristic *pCharacteristic) {
			float batVal = measureBattery();
			int batPercent = batteryPercent(batVal);
			bat_val_characteristic->setValue(String(batPercent).c_str());
			bat_val_characteristic->notify();
	}
};

class SyncCharacteristicCallbacks: public BLECharacteristicCallbacks {
	void onWrite(BLECharacteristic *pCharacteristic) {
			 // TODO handle SYNC event
       std::string value = pCharacteristic->getValue();
		   uint32_t int_val = (uint32_t) value.c_str();
       state = CALIBRATE;
       ESP_ERROR_CHECK(esp_timer_stop(sampling_timer));
       #ifdef LEFT_DEVICE
       delay(1000);
       #else
       delay(532); // measured 32ms offset
       #endif
       tft.fillScreen(TFT_RED);
		   delay(200);
       nSamples = 0;
       missedSamples = 0;
       state = CONNECTED;
       ESP_ERROR_CHECK(esp_timer_start_periodic(sampling_timer, 105000));//every 105ms
	}
};

class CalibrateCharacteristicCallbacks: public BLECharacteristicCallbacks {
	void onWrite(BLECharacteristic *pCharacteristic) {
		state = CALIBRATE;
		Serial.println("SHOW ME THE CALIBRATE");
		tft.fillScreen(TFT_RED);
		delay(200);
		int nGrams = atoi(pCharacteristic->getValue().c_str());
		if(nGrams == 0) {
			tare();
		}
		if(nGrams > 0) {
			calibrate(nGrams);
		}
		state=CONNECTED;
	}
};

class CharacteristicsCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    std::string value = pCharacteristic->getValue();
		uint32_t int_val = (uint32_t) value.c_str();
		Serial.println("some value arrived");
		tft.fillScreen(TFT_RED);
		delay(2000);
  }
};


void drawReading(float reading, float maxReading) {
  String out = String(reading<0?0:reading)+"kg";
  String out1 = String(maxReading)+"kg";
  tft.fillScreen(TFT_BLACK);
  tft.drawString(out, 120, 32);
  tft.drawString(out1, 120, 84);
}

void drawDebugReading(float reading, int missedCount) {
  String out = String(reading<0?0:reading)+"kg";
  String out1 = String(missedCount)+" "+ (state==UNCONNECTED ? "NC": "C");
  tft.fillScreen(TFT_BLACK);
  tft.drawString(out, 120, 32);
  tft.drawString(out1, 120, 84);
}


void initScale(){
  scale.begin(DIO, CLK);
  preferences.begin("calibration", false); //false for r/w mode instead of just r
  float scaler = preferences.getFloat("scale", -26.326582);
  uint32_t offset = preferences.getULong("offset", 4294856599);
  preferences.end();

  scale.set_offset(offset);
  scale.set_scale(scaler);
}


void initTFT(){
	tft.init();
  tft.setTextSize(5);

  if (TFT_BL > 0) {                         // TFT_BL has been set in the TFT_eSPI library in the User Setup file TTGO_T_Display.h
    pinMode(TFT_BL, OUTPUT);                // Set backlight pin to output mode
    digitalWrite(TFT_BL, TFT_BACKLIGHT_ON); // Turn backlight on. TFT_BACKLIGHT_ON has been set in the TFT_eSPI library in the User Setup file TTGO_T_Display.h
  }
  #ifdef LEFT_DEVICE
  tft.setRotation(1);
  #else
  tft.setRotation(3);
  #endif
  tft.fillScreen(TFT_RED);
  delay(20);
  tft.fillScreen(TFT_BLUE);
  delay(20);
  tft.fillScreen(TFT_GREEN);
  delay(20);
  tft.fillScreen(TFT_BLACK);
  tft.setTextDatum(MC_DATUM);

  tft.drawString("Hello!", tft.width() / 2, tft.height() / 2);
}




int64_t currentReadingTimestamp;
int64_t readingStartTimestamp;


void setup(){
  Serial.begin(115200);
  initTFT();
  //Increment boot number and print it every reboot
  ++bootCount;
  Serial.println("Boot number: " + String(bootCount));

	// TOGGLE Pin for sampling timing mesurements
	#ifdef DEBUG_SAMPLING
	pinMode(SAMPLE_INDICATOR, OUTPUT);
	pinMode(SAMPLE_INDICATOR_1, OUTPUT);
	#endif
	
  initScale();


  pinMode(BUTTON_1, INPUT);
  pinMode(BUTTON_2, INPUT);

	BLEDevice::init(TAG);
	 // Create the BLE Server
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());
  // Create the BLE Service
  BLEService *pService = pServer->createService(SERVICE_UUID);
  delay(100);

  uint32_t props = BLECharacteristic::PROPERTY_READ |
          BLECharacteristic::PROPERTY_WRITE |
          BLECharacteristic::PROPERTY_NOTIFY |
          BLECharacteristic::PROPERTY_INDICATE;

  // Create BLE Characteristics
  sensor_val_characteristic = pService->createCharacteristic(
      SENSOR_VAL_CHARACTERISTIC_UUID,
      props);

  bat_val_characteristic = pService->createCharacteristic(
      BAT_VAL_CHARACTERISTIC_UUID,
      props);

  sync_characteristic = pService->createCharacteristic(
      SYNC_CHARACTERISTIC_UUID,
     props);

	calibrate_characteristic = pService->createCharacteristic(
      CALIBRATE_CHARACTERISTIC_UUID,
      props);


  sensor_val_characteristic->setValue("0.0 0");

	 float vBat = measureBattery(); 
	 int batPercent = batteryPercent(vBat); 
	/* Serial.println("VBAT: "+ String(vBat)+", "+String(batPercent)+"%"); */
  bat_val_characteristic->setValue(String(batPercent).c_str());
	//#ifdef LEFT_DEVICE
  //bat_val_characteristic->setValue("7");
	//#else
	//bat_val_characteristic->setValue("99");
	//#endif

  bat_val_characteristic->setCallbacks(new CharacteristicsCallbacks());
  sensor_val_characteristic->setCallbacks(new CharacteristicsCallbacks());
  sync_characteristic->setCallbacks(new SyncCharacteristicCallbacks());
	calibrate_characteristic->setCallbacks(new CalibrateCharacteristicCallbacks());


	 // Start the BLE service
  pService->start();

  // Start advertising
  pServer->getAdvertising()->start();


  Serial.println("Waiting for a client connection to notify...");
  tft.fillScreen(TFT_BLACK);

  //esp_sleep_enable_ext0_wakeup(GPIO_NUM_0, 0);
  esp_sleep_enable_ext0_wakeup(BUTTON_1, 0);

  initButtonTimer();
	initSamplingTimer();
  time_since_boot = esp_timer_get_time();
  readingStartTimestamp = esp_timer_get_time();
  ESP_LOGI(TAG, "startup finished, time since boot: %lld us", time_since_boot);
}


void loop() {
	nLoopRuns++;

	//time_since_boot = esp_timer_get_time();
  //ESP_LOGI(TAG, "loop called, time since boot: %lld us", time_since_boot);

	float vBat = measureBattery();
	int batPercent = batteryPercent(vBat);
	//Serial.println("VBAT: "+ String(vBat)+", "+String(batPercent)+"%");
  bat_val_characteristic->setValue(String(batPercent).c_str());
  bat_val_characteristic->notify();
	delay(20000);
}


static void sampling_timer_callback(void* arg){
	digitalWrite(SAMPLE_INDICATOR, HIGH);
	int64_t localTimestamp;
	char valuesOut[20];

	if (state == CALIBRATE) {
				return;
	}
	nSamples = (nSamples + 1) % 3000;
  
  scale.wait_ready();
	if (scale.is_ready()) {
		digitalWrite(SAMPLE_INDICATOR_1, HIGH);
		reading = scale.get_units()/1000;
    reading = reading <0 ? 0:reading;
		//currentReadingTimestamp = esp_timer_get_time();
		//localTimestamp = currentReadingTimestamp - readingStartTimestamp;
		sprintf(valuesOut, "%.2f %d", reading, nSamples);// localTimestamp);
		// sending reading via BLE
		if(state == CONNECTED){
		  sensor_val_characteristic->setValue(valuesOut);
		  #ifdef LEFT_DEVICE
		  delay(10);
		  #endif
		  sensor_val_characteristic->notify();
		}
		if (reading > maxReading){ maxReading=reading;}
		digitalWrite(SAMPLE_INDICATOR_1, LOW);
		if(screenon){
			//drawReading(reading, maxReading);
      drawDebugReading(reading, missedSamples);
		}
	/* ESP_LOGI(TAG, "scale was ready, time since last reading: %lld us", localTimestamp); */
	} else {
		missedSamples++;
    drawDebugReading(reading, missedSamples);
	}
	digitalWrite(SAMPLE_INDICATOR, LOW);
}

static void button_timer_callback(void* arg){
 btn_state = digitalRead(BUTTON_1);
  if(btn_state && !old_btn_state){
    ESP_ERROR_CHECK(esp_timer_stop(button_timer));
    ESP_ERROR_CHECK(esp_timer_delete(button_timer));
		 ESP_ERROR_CHECK(esp_timer_stop(sampling_timer));
    ESP_ERROR_CHECK(esp_timer_delete(sampling_timer));
    Serial.println("pressed sleep button");
    maxReading=0;
    tft.fillScreen(TFT_BLACK);
    tft.setTextDatum(MC_DATUM);
    tft.drawString("Bye...", tft.width() / 2, tft.height() / 2);
    delay(500);
    esp_deep_sleep_start();
    //toggleScreen();
  }
	old_btn_state = btn_state;
	//Serial.println(nLoopRuns);
	//	nLoopRuns = 0;
}