#include <Arduino.h>
#include <SX126x-Arduino.h>
#include <SPI.h>
#include <OneWire.h>
#include <DallasTemperature.h>
#include <Preferences.h>
Preferences preferences;

#define LED_1 2
#define LED_2 33
#define ACTIVATE_GATE 13
#define BUZZ_PIN 9
#define ANALOGUE_READ_PIN 12
#define BRIDGE_RESISTORS_POWER_PIN 4
#define SOLAR_READ_PIN 15
#define SOLAR_POWER_PIN 22

#define GREEN_LED 2
#define BLUE_LED 33
#define RED_LED 2
#define YELLOW_LED 33
#define LED_1206_1 14
#define LED_1206_2 15

#define ONE_WIRE_BUS 27

String header = "<!>_";  // Gadget B
// String header = "<*>_";  // Gadget C

// GPIO where the DS18B20 is connected to:

// Setup a oneWire instance to communicate with any OneWire devices
OneWire oneWire(ONE_WIRE_BUS);

// Pass our oneWire reference to Dallas Temperature sensor 
DallasTemperature sensors(&oneWire);



hw_config hwConfig;

// ESP32 - SX126x pin configuration
int PIN_LORA_RESET = 0;  // LORA RESET
int PIN_LORA_DIO_1 = 37; // LORA DIO_1
int PIN_LORA_BUSY = 38;  // LORA SPI BUSY
int PIN_LORA_NSS = 25;  // LORA SPI CS
int PIN_LORA_SCLK = 18;  // LORA SPI CLK
int PIN_LORA_MISO = 19;  // LORA SPI MISO
int PIN_LORA_MOSI = 23;  // LORA SPI MOSI
int RADIO_TXEN = -1;   // LORA ANTENNA TX ENABLE
int RADIO_RXEN = 39;   // LORA ANTENNA RX ENABLE

// Function declarations
void OnTxDone(void);
void OnRxDone(uint8_t *payload, uint16_t size, int16_t rssi, int8_t snr);
void OnTxTimeout(void);
void OnRxTimeout(void);
void OnRxError(void);
void OnCadDone(bool cadResult);

// Define LoRa parameters
#define RF_FREQUENCY 433500000  // Hz
#define TX_OUTPUT_POWER 32 // dBm   was 22.
#define LORA_BANDWIDTH 2    // [0: 125 kHz, 1: 250 kHz, 2: 500 kHz, 3: Reserved]
#define LORA_SPREADING_FACTOR 12 // [SF7..SF12]
#define LORA_CODINGRATE 1   // [1: 4/5, 2: 4/6,  3: 4/7,  4: 4/8]
#define LORA_PREAMBLE_LENGTH 8  // Same for Tx and Rx
#define LORA_SYMBOL_TIMEOUT 0   // Symbols
#define LORA_FIX_LENGTH_PAYLOAD_ON false
#define LORA_IQ_INVERSION_ON false
#define RX_TIMEOUT_VALUE 3000
#define TX_TIMEOUT_VALUE 3000

#define BUFFER_SIZE 64 // Define the payload size here

#define SENSOR_DATA_PIN   39   // Sensor PWM interface
#define INTERRUPT_NUMBER   digitalPinToInterrupt(SENSOR_DATA_PIN)   // interrupt number


// Used in interrupt, calculate pulse width variable
volatile unsigned long pwm_high_start_ticks=0, pwm_high_end_ticks=0;
volatile unsigned long pwm_high_val=0, pwm_low_val=0;
// interrupt flag
volatile uint8_t flag=0;

static RadioEvents_t RadioEvents;
static uint16_t BufferSize = BUFFER_SIZE;
static uint8_t RcvBuffer[BUFFER_SIZE];

static bool isMaster = true;
const uint8_t PingMsg[] = "PING";
const uint8_t PongMsg[] = "PONG";

time_t timeToSend;
time_t cadTime;

uint8_t pingCnt = 0;
uint8_t pongCnt = 0;


#define RXD2 16
#define TXD2 17

float voltage = 0.000f;
int readingsAlreadyTaken = 0;

int count = 0;
float concentration = 0.00;

void setup() 
{
  // Initialize Serial for debug output
  Serial.begin(115200);
  analogSetPinAttenuation(12,ADC_11db);
  adcAttachPin(12);
  // analogSetAttenuation(ADC_6db);


  pinMode(BLUE_LED, OUTPUT);  
  pinMode(GREEN_LED, OUTPUT);
  pinMode(LED_1206_1, OUTPUT);
  pinMode(LED_1206_2, OUTPUT);
  pinMode(BUZZ_PIN, OUTPUT);
  pinMode(ACTIVATE_GATE, OUTPUT);
  pinMode(BRIDGE_RESISTORS_POWER_PIN, OUTPUT);
  pinMode(SENSOR_DATA_PIN, INPUT);
  pinMode(SOLAR_POWER_PIN, OUTPUT);
  
  attachInterrupt(INTERRUPT_NUMBER, interrupt_change, CHANGE);

  digitalWrite(ACTIVATE_GATE, LOW);
  digitalWrite(BUZZ_PIN, LOW);
  digitalWrite(LED_1206_1, LOW);
  digitalWrite(LED_1206_2, LOW);
  digitalWrite(SOLAR_POWER_PIN, HIGH);

  buzz2();
  flash_LED_1206_1();
  flash_LED_1206_2();
  flash_BLUE_LED();
  // delay(5000);
  preferences.begin("my-app", false);
  unsigned int bootCounter = preferences.getUInt("bootCounter", 0);
  bootCounter++;
  Serial.printf("Current bootCounter value: %u\n", bootCounter);
  preferences.putUInt("bootCounter", bootCounter);
  // Close the Preferences
  // preferences.end();
  // Store the counter to the Preferences

  adcAttachPin(SOLAR_READ_PIN);
  int solarSensorValue = analogRead(SOLAR_READ_PIN);
  Serial.print("solar sensor value= ");Serial.println(solarSensorValue);

  if((bootCounter > 2) || (solarSensorValue > 1000))   // 21 = 6 hours.
  {
    bootCounter =0;
    preferences.putUInt("bootCounter", bootCounter);
    preferences.end();
    Serial.println("Now let the ESP32 continue !!!");
  }
  else
  {
    buzz();
    delay(2000);
    preferences.end();
    Serial.println("Now turn off the ESP32 !!!");
    // Turn off the ESP32 with ACTIVATE_GATE via the timer chip:
    digitalWrite(ACTIVATE_GATE, HIGH);
    // Or use the Restart ESP function for testing:
    delay(60000);
    // ESP.restart();
  }

  delay(2000);
  digitalWrite(BRIDGE_RESISTORS_POWER_PIN, HIGH); // This also supplies battery voltage to CO2 sensor
  // Start the DS18B20 sensor
  sensors.begin();
  // call sensors.requestTemperatures() to issue a global temperature 
  // request to all devices on the bus
  Serial.print("Requesting temperatures...");
  sensors.requestTemperatures(); // Send the command to get temperatures
  Serial.println("DONE");
  // After we got the temperatures, we can print them here.
  // We use the function ByIndex, and as an example get the temperature from the first sensor only.
  float tempC = sensors.getTempCByIndex(0) + 20;
  // tempC = 0.0;

  // Check if reading was successful
  if(tempC != DEVICE_DISCONNECTED_C)
  {
    Serial.print("Temperature for the device 1 (index 0) is: ");
    Serial.println(tempC);
  }
  else
  {
    Serial.println("Error: Could not read temperature data");
  }

  // digitalWrite(ACTIVATE_GATE2, HIGH);
  // buzz2();
  // delay(1000);
  // digitalWrite(LED_1, HIGH);
  // digitalWrite(LED_2, LOW);
  // digitalWrite(LED_3, LOW);
  // delay(1000);
  // digitalWrite(LED_1, LOW);
  // digitalWrite(LED_2, HIGH);
  // digitalWrite(LED_3, HIGH);
  // delay(1000);
  // digitalWrite(LED_1, HIGH);
  // digitalWrite(LED_2, LOW);
  // digitalWrite(LED_3, LOW);
  // delay(1000);
  // digitalWrite(LED_1, LOW);
  // digitalWrite(LED_2, HIGH);
  // digitalWrite(LED_3, HIGH);
  // delay(1000);
  // digitalWrite(PULSE_TIMER_PIN, HIGH);
  // delay(1000);
  // digitalWrite(PULSE_TIMER_PIN, LOW);
  // digitalWrite(ACTIVATE_GATE, LOW);
  // digitalWrite(ACTIVATE_GATE2, LOW);
  // digitalWrite(LED_2, LOW);
  // digitalWrite(LED_3, LOW);

  // Define the HW configuration between MCU and SX126x
  hwConfig.CHIP_TYPE = SX1262_CHIP;     // Example uses an eByte E22 module with an SX1262
  hwConfig.PIN_LORA_RESET = PIN_LORA_RESET; // LORA RESET
  hwConfig.PIN_LORA_NSS = PIN_LORA_NSS;  // LORA SPI CS
  hwConfig.PIN_LORA_SCLK = PIN_LORA_SCLK;   // LORA SPI CLK
  hwConfig.PIN_LORA_MISO = PIN_LORA_MISO;   // LORA SPI MISO
  hwConfig.PIN_LORA_DIO_1 = PIN_LORA_DIO_1; // LORA DIO_1
  hwConfig.PIN_LORA_BUSY = PIN_LORA_BUSY;   // LORA SPI BUSY
  hwConfig.PIN_LORA_MOSI = PIN_LORA_MOSI;   // LORA SPI MOSI
  hwConfig.RADIO_TXEN = RADIO_TXEN;     // LORA ANTENNA TX ENABLE
  hwConfig.RADIO_RXEN = RADIO_RXEN;     // LORA ANTENNA RX ENABLE
  hwConfig.USE_DIO2_ANT_SWITCH = false;   // Was true. Example uses an CircuitRocks Alora RFM1262 which uses DIO2 pins as antenna control
  hwConfig.USE_DIO3_TCXO = true;        // Example uses an CircuitRocks Alora RFM1262 which uses DIO3 to control oscillator voltage
  hwConfig.USE_DIO3_ANT_SWITCH = false;  // Only Insight ISP4520 module uses DIO3 as antenna control


  Serial.println("=====================================");
  Serial.println("SX126x Transmit");
  Serial.println("=====================================");
  Serial.print("MOSI: ");Serial.println(MOSI);
  Serial.print("MISO: ");Serial.println(MISO);
  Serial.print("RESET: ");Serial.println(PIN_LORA_RESET);
  Serial.print("DIO_1: ");Serial.println(PIN_LORA_DIO_1);
  Serial.print("BUSY: ");Serial.println(PIN_LORA_BUSY);
  Serial.print("SCLK: ");Serial.println(PIN_LORA_SCLK);
  Serial.print("SS: ");Serial.println(SS);

#ifdef ESP32
  Serial.println("MCU Espressif ESP32");
#endif


  uint8_t deviceId[8];

  BoardGetUniqueId(deviceId);
  Serial.printf("BoardId: %02X-%02X-%02X-%02X-%02X-%02X-%02X-%02X\n",
          deviceId[7],
          deviceId[6],
          deviceId[5],
          deviceId[4],
          deviceId[3],
          deviceId[2],
          deviceId[1],
          deviceId[0]);

  // flash_GREEN_LED();
  // Initialize the LoRa chip
  Serial.println("Starting lora_hardware_init ....");
  // Serial.println("TEST_01");
  
  lora_hardware_init(hwConfig);
  // flash_GREEN_LED();
  Serial.println("hwConfig initialised.");
  // Serial.println("TEST_02");
  
  // Initialize the Radio callbacks
  RadioEvents.TxDone = OnTxDone;
  RadioEvents.RxDone = OnRxDone;
  RadioEvents.TxTimeout = OnTxTimeout;
  RadioEvents.RxTimeout = OnRxTimeout;
  RadioEvents.RxError = OnRxError;
  RadioEvents.CadDone = OnCadDone;

  // Serial.println("TEST_03");
  // flash_GREEN_LED();
  // Initialize the Radio
  Radio.Init(&RadioEvents);
  // flash_GREEN_LED();
  Serial.println("Radio initialised.");
  // Serial.println("TEST_04");
  
  Radio.Standby();
  
  // Required SyncWord = 0x2414.
  uint8_t my_syncword_0 = 0x14;
  uint8_t my_syncword_1 = 0x24;
  uint16_t readSyncWord = 0;

  SX126xReadRegisters(REG_LR_SYNCWORD, (uint8_t *)&readSyncWord, 0);
  Serial.print("My SyncWord 0: ");Serial.print("0x");Serial.println(readSyncWord,HEX);
  readSyncWord = 0;
  SX126xReadRegisters(REG_LR_SYNCWORD, (uint8_t *)&readSyncWord, 1);
  Serial.print("My SyncWord 1: ");Serial.print("0x");Serial.println(readSyncWord,HEX);
  readSyncWord = 0;
  SX126xReadRegisters(REG_LR_SYNCWORD, (uint8_t *)&readSyncWord, 2);
  Serial.print("My SyncWord 2: ");Serial.print("0x");Serial.println(readSyncWord,HEX);
  Serial.println();

  SX126xWriteRegister(REG_LR_SYNCWORD, my_syncword_0);
  SX126xWriteRegister(REG_LR_SYNCWORD +1, my_syncword_1);

  readSyncWord = 0;
  SX126xReadRegisters(REG_LR_SYNCWORD, (uint8_t *)&readSyncWord, 0);
  Serial.print("My SyncWord 0: ");Serial.print("0x");Serial.println(readSyncWord,HEX);
  readSyncWord = 0;
  SX126xReadRegisters(REG_LR_SYNCWORD, (uint8_t *)&readSyncWord, 1);
  Serial.print("My SyncWord 1: ");Serial.print("0x");Serial.println(readSyncWord,HEX);
  readSyncWord = 0;
  SX126xReadRegisters(REG_LR_SYNCWORD, (uint8_t *)&readSyncWord, 2);
  Serial.print("My SyncWord 2: ");Serial.print("0x");Serial.println(readSyncWord,HEX);
  
  // Set Radio channel
  Radio.SetChannel(RF_FREQUENCY);
  // flash_GREEN_LED();
  Serial.println("Frequency set.");
  // Serial.println("TEST_05");

  // Set Radio TX configuration
  Radio.SetTxConfig(MODEM_LORA, TX_OUTPUT_POWER, 0, LORA_BANDWIDTH,
            LORA_SPREADING_FACTOR, LORA_CODINGRATE,
            LORA_PREAMBLE_LENGTH, LORA_FIX_LENGTH_PAYLOAD_ON,
            true, 0, 0, LORA_IQ_INVERSION_ON, TX_TIMEOUT_VALUE);

  // Set Radio RX configuration
  Radio.SetRxConfig(MODEM_LORA, LORA_BANDWIDTH, LORA_SPREADING_FACTOR,
            LORA_CODINGRATE, 0, LORA_PREAMBLE_LENGTH,
            LORA_SYMBOL_TIMEOUT, LORA_FIX_LENGTH_PAYLOAD_ON,
            0, true, 0, 0, LORA_IQ_INVERSION_ON, true);

  // Start LoRa
  // Serial.println("TEST_06");
  Serial.println("Starting Radio.Rx .....");
  // Radio.Rx(RX_TIMEOUT_VALUE);
  // Serial.println("TEST_07");
  // flash_GREEN_LED();
  Serial.println("Radio.Rx started.");
  Serial.println("Waiting for LoRa transmission ....");
  // Serial.println("TEST_08");
  // LOG_LIB("BRD", "SyncWord = %04X", readSyncWord);
  timeToSend = millis();

  flash_GREEN_LED();

  // This is the Glasshouse header:
  // String header = "#<~_";
  
  // We are on FreeRTOS, give other tasks a chance to run
  delay(100);
  yield();

  Serial.println("Try to take some readings ......... ");

 
  // Allow voltage to stabilise:
  delay(10000);
  int sensorValueVolts = analogRead(ANALOGUE_READ_PIN);
  // int solarValue = analogRead(SOLAR_READ_PIN);

  // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V):
  // float voltage = sensorValueVolts * (3.8 / 4095.0) *20 *0.9534;
  // float voltage = sensorValueVolts * (3.9 / 4095.0) *20;
  int voltageValue = sensorValueVolts;
  // print out the value you read:
  Serial.println(voltageValue);

  int CO2_count = 0;
  while(CO2_count < 60)
  {
    delay(1000);
    if(flag == 4){
      flag = 1;
      float pwm_high_val_ms = (pwm_high_val * 1000.0) / (pwm_low_val + pwm_high_val);
      // Serial.print("pwm_high_val_ms:");
      // Serial.print(pwm_high_val_ms);
      // Serial.println("ms");
      //Print CO2 concentration
      // float concentration = (pwm_high_val_ms - 2) * 5;
      // Serial.print(concentration);
      // Serial.println("ppm");
      if (pwm_high_val_ms < 0.01){
        Serial.println("Fault");
      }
      else if (pwm_high_val_ms < 80.00){
        Serial.println("preheating");
      }
      else if (pwm_high_val_ms < 998.00){
        concentration = (pwm_high_val_ms - 2) * 5;
        // Print pwm_high_val_ms
        // Serial.print("pwm_high_val_ms:");
        // Serial.print(pwm_high_val_ms);
        // Serial.println("ms");
        //Print CO2 concentration
        Serial.print(concentration);
        // Serial.println("ppm");
      }else{
        Serial.println("Beyond the maximum range : 398~4980ppm");
      }
      Serial.println();
    }
    CO2_count ++;
  }

  digitalWrite(BRIDGE_RESISTORS_POWER_PIN, LOW); // Also turns off power to CO2 sensor.

  Serial.print("Voltage raw value: ");Serial.println(voltageValue);
  // Serial.print("Solar raw value: ");Serial.println(solarValue);
  Serial.println("Sending a voltage data transmit:");

  delay(3000);

  String label = "voltsC:_";
  int sensorValue = voltageValue;
  myTransmit(header, label, sensorValue);

  delay(3000);

  label = "tempC:_";
  sensorValue = int(tempC *100);
  myTransmit(header, label, sensorValue);

  delay(3000);

  label = "solarC:_";
  // solarValue = concentration;
  sensorValue = int(solarSensorValue);
  myTransmit(header, label, sensorValue);

  delay(3000);

  label = "COTWOC:_";
  // solarValue = concentration;
  sensorValue = int(concentration);
  myTransmit(header, label, sensorValue);

  delay(1000);
  buzz2();
  // Trun off the ESP32 via the timer chip:

  digitalWrite(ACTIVATE_GATE, HIGH);
  delay(1000);


  // buzz2();
}

// void loop should never be reached.
void loop() 
{
  sensors.requestTemperatures(); 
  float temperatureC = sensors.getTempCByIndex(0);
  if ((temperatureC > 60) || (temperatureC < -60))
  {
    temperatureC = 0;
  }
  Serial.print(temperatureC);
  Serial.println("ºC");

  digitalWrite(BRIDGE_RESISTORS_POWER_PIN, HIGH);
  // Serial.println("=====================================");
  digitalWrite(LED_2, LOW);
  digitalWrite(LED_1, HIGH);
  delay(1000);
  digitalWrite(LED_2, HIGH);
  digitalWrite(LED_1, LOW);
  delay(1000);
  digitalWrite(LED_2, LOW);
  // buzz();

  int sensorValue = analogRead(ANALOGUE_READ_PIN);
  // int solarValue = analogRead(SOLAR_READ_PIN);
  // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V):
  int voltage = sensorValue;
  // print out the value you read:
  Serial.print("voltage raw value: ");Serial.println(voltage);
  // Serial.print("solar raw value: ");Serial.println(solarValue);
  delay(100);
  digitalWrite(BRIDGE_RESISTORS_POWER_PIN, LOW);
  delay(10000);
}

void buzz2()
{
  for (int i = 350; i>0; --i)
  {
    digitalWrite(BUZZ_PIN, HIGH);
    delayMicroseconds(200);
    digitalWrite(BUZZ_PIN, LOW);
    delayMicroseconds(200);
  }
  delay(100);
  for (int i = 350; i>0; --i)
  {
    digitalWrite(BUZZ_PIN, HIGH);
    delayMicroseconds(250);
    digitalWrite(BUZZ_PIN, LOW);
    delayMicroseconds(250);
  }
  delay(100);
  for (int i = 350; i>0; --i)
  {
    digitalWrite(BUZZ_PIN, HIGH);
    delayMicroseconds(200);
    digitalWrite(BUZZ_PIN, LOW);
    delayMicroseconds(200);
  }
  delay(100);
}

/**@brief Function to be executed on Radio Tx Done event
 */
void OnTxDone(void)
{
  // Serial.println("Transmit was successful !!!!");
}

/**@brief Function to be executed on Radio Rx Done event
 */
void OnRxDone(uint8_t *payload, uint16_t size, int16_t rssi, int8_t snr)
{
}

/**@brief Function to be executed on Radio Tx Timeout event
 */
void OnTxTimeout(void)
{
  Serial.println("There was a Tx Timeout.");
}

/**@brief Function to be executed on Radio Rx Timeout event
 */
void OnRxTimeout(void)
{
}

/**@brief Function to be executed on Radio Rx Error event
 */
void OnRxError(void)
{
}

/**@brief Function to be executed on Radio Rx Error event
 */
void OnCadDone(bool cadResult)
{
}

void flash_GREEN_LED()
{
  delay(500);
  digitalWrite(GREEN_LED, HIGH);
  delay(500);
  digitalWrite(GREEN_LED, LOW);
}

void flash_BLUE_LED()
{
  delay(1000);
  digitalWrite(BLUE_LED, HIGH);
  delay(1000);
  digitalWrite(BLUE_LED, LOW);
}

void flash_RED_LED()
{
  delay(1000);
  digitalWrite(RED_LED, HIGH);
  delay(1000);
  digitalWrite(RED_LED, LOW);
}

void flash_YELLOW_LED()
{
  delay(1000);
  digitalWrite(YELLOW_LED, HIGH);
  delay(1000);
  digitalWrite(YELLOW_LED, LOW);
}

void flash_LED_1206_1()
{
  delay(1000);
  digitalWrite(LED_1206_1, HIGH);
  delay(1000);
  digitalWrite(LED_1206_1, LOW);
}

void flash_LED_1206_2()
{
  delay(1000);
  digitalWrite(LED_1206_2, HIGH);
  delay(1000);
  digitalWrite(LED_1206_2, LOW);
}

void buzz()
{
  for (int i = 350; i>0; --i)
  {
    digitalWrite(BUZZ_PIN, HIGH);
    delayMicroseconds(500);
    digitalWrite(BUZZ_PIN, LOW);
    delayMicroseconds(500);
  }
}

void myTransmit(String header, String label, float sensorValue)
{
  yield();
  // buzz();
    int h = 0;
    int i = 0;
    int j =0;
    int h2 = 0;
    int i2 = 0;
    int j2 =0;
    int k= 0;
    int l = 0;
    static uint8_t TxdBuffer[BUFFER_SIZE];

    // for (l =0; l< BUFFER_SIZE; l++)
    // {
      // TxdBuffer[l] = "";
    // }

    String header2 = header;
    Serial.print("header2: ");Serial.print(header2);Serial.print("   ");
    int packetSize = sizeof(header2);
    int totalPacketSize = 0;
    totalPacketSize = totalPacketSize + packetSize;
    for (h =0; h< packetSize; h++)
    {
      if (header2[h] != 0)
      {
        TxdBuffer[h] = header2[h];
        Serial.print(TxdBuffer[h]);Serial.print(",");
        h2++;
      }
    }
    Serial.println();
    Serial.print("Current TxdBuffer: ");
    for (l =0; l< 32; l++)
    {
      Serial.print(TxdBuffer[l]);Serial.print(",");
    }
    Serial.println();
 
    String label2 = label;
    Serial.print("label2: ");Serial.print(label2);Serial.print("   ");
    Serial.print("sizeof label2:");Serial.println(sizeof(label2));
    packetSize = sizeof(label2);
    totalPacketSize = totalPacketSize + packetSize;
    for (i =0; i< packetSize; i++)
    {
      if (label2[i] != 0)
      {
        TxdBuffer[h2+i] = label2[i];
        Serial.print(TxdBuffer[h2+i]);Serial.print(",");
        i2++;
      }
    }
    Serial.println();
    Serial.print("Current TxdBuffer: ");
    for (l =0; l< 32; l++)
    {
      Serial.print(TxdBuffer[l]);Serial.print(",");
    }
    Serial.println();
    
    String sensorValue2 = (String)sensorValue;
    Serial.print("sensorValue String: ");Serial.print(sensorValue2);Serial.print("   ");
    packetSize = sizeof(sensorValue2);
    totalPacketSize = totalPacketSize + packetSize;
    for (j =0; j< packetSize; j++)
    {
      if (sensorValue2[j] != 0)
      {
        TxdBuffer[h2+i2+j] = sensorValue2[j];
        Serial.print(TxdBuffer[h2+i2+j]);Serial.print(",");
        j2++;
      }
    }
    Serial.println();
    Serial.print("Current TxdBuffer: ");
    for (l =0; l< 32; l++)
    {
      Serial.print(TxdBuffer[l]);Serial.print(",");
    }
    Serial.println();
    Serial.println();
    Radio.Send(TxdBuffer, totalPacketSize);
}

void printUint16Hex(uint16_t value) 
{
  Serial.print(value < 4096 ? "0" : "");
  Serial.print(value < 256 ? "0" : "");
  Serial.print(value < 16 ? "0" : "");
  Serial.print(value, HEX);
}

void printSerialNumber(uint16_t serial0, uint16_t serial1, uint16_t serial2) 
{
  Serial.print("Serial: 0x");
  printUint16Hex(serial0);
  printUint16Hex(serial1);
  printUint16Hex(serial2);
  Serial.println();
}

void interrupt_change()
{
  if (digitalRead(SENSOR_DATA_PIN)) {
    pwm_high_start_ticks = micros();    // store the current micros() value
    if(2 == flag){
      flag = 4;
      if(pwm_high_start_ticks > pwm_high_end_ticks) {
        pwm_low_val = pwm_high_start_ticks - pwm_high_end_ticks;
      }
    }else{
      flag = 1;
    }
  } else {
    pwm_high_end_ticks = micros();    // store the current micros() value
    if(1 == flag){
      flag = 2;
      if(pwm_high_end_ticks > pwm_high_start_ticks){
        pwm_high_val = pwm_high_end_ticks - pwm_high_start_ticks;
      }
    }
  }
}