#include <Wire.h>

#define I2C_ADDR 0x27   // LCD I2C address
#define LCD_COLS 16
#define LCD_ROWS 2

// Control bits
#define LCD_BACKLIGHT 0x08
#define ENABLE 0x04
#define RS 0x01

void i2cWrite(uint8_t data) {
  Wire.beginTransmission(I2C_ADDR);
  Wire.write(data | LCD_BACKLIGHT);
  Wire.endTransmission();
}

void pulseEnable(uint8_t data) {
  i2cWrite(data | ENABLE);
  delayMicroseconds(1);
  i2cWrite(data & ~ENABLE);
  delayMicroseconds(50);
}

void sendNibble(uint8_t data) {
  i2cWrite(data);
  pulseEnable(data);
}

void sendByte(uint8_t data, uint8_t mode) {
  uint8_t highNib = data & 0xF0;
  uint8_t lowNib  = (data << 4) & 0xF0;
  sendNibble(highNib | mode);
  sendNibble(lowNib | mode);
}

void lcdCommand(uint8_t cmd) {
  sendByte(cmd, 0);
}

void lcdData(uint8_t data) {
  sendByte(data, RS);
}

void lcdInit() {
  Wire.begin(21, 22); // ESP32 default SDA=21, SCL=22
  delay(50);

  // Init sequence
  sendNibble(0x30);
  delay(5);
  sendNibble(0x30);
  delayMicroseconds(100);
  sendNibble(0x30);
  delayMicroseconds(100);
  sendNibble(0x20); // 4-bit mode

  lcdCommand(0x28); // 2 lines, 5x8 font
  lcdCommand(0x0C); // Display ON, cursor OFF
  lcdCommand(0x06); // Entry mode
  lcdCommand(0x01); // Clear display
  delay(2);
}

void lcdSetCursor(uint8_t col, uint8_t row) {
  uint8_t rowOffsets[] = {0x00, 0x40, 0x14, 0x54};
  lcdCommand(0x80 | (col + rowOffsets[row]));
}

void lcdPrint(const char *str) {
  while (*str) {
    lcdData(*str++);
  }
}

// Function to display a value with a unit character 
void lcdDisplayValue(int col, int row, float value, char unitChar) { 
//  char buffer[6]; // temporary string buffer 
//  sprintf(buffer, "%.2f%c", value, unitChar); // format value + unit 
//  lcdSetCursor(col, row); // move cursor 
//  lcdPrint(buffer); // use your lcdPrint() to send string
//  printf("%s",buffer);

  char buffer[16]; // enough for "123.45X" 
  lcdSetCursor(col, row); // Convert float to string: width=5, precision=2 
  dtostrf(value, 5, 2, buffer); 
  lcdPrint(buffer); // your lcdPrint(const char *str) 
  lcdData(unitChar); // append unit character
}

int led = 4;
int de = 5000;

int sensorValue = 0;
float voltageSenseVal = 0;
float currentSenseVal = 0;

//LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  // put your setup code here, to run once:
  pinMode(led,OUTPUT);
  
  lcdInit();
  lcdSetCursor(0, 0);
  lcdPrint("Hello");
  lcdSetCursor(0, 1);
  lcdPrint("SP Solutions");
  delay(150);
  lcdCommand(0x01); // Clear display

  Serial.begin(115200); // Start serial at 115200 baud 
  //while (!Serial); // Wait for serial port to connect (optional) 
  Serial.println("ESP32 Ready!");

  // Seed the random generator using an unconnected analog pin 
  //randomSeed(analogRead(34)); // GPIO34 is safe for floating input
  //randomSeed(analogRead(35)); // GPIO34 is safe for floating input
  }

void loop() {
  // put your main code here, to run repeatedly:

  sensorValue = analogRead(34); // Example ADC pin 
  voltageSenseVal = (sensorValue / 4095.0) * 3.3;
  lcdSetCursor(0, 0);
  lcdPrint(" Voltage:");// lcd.print((int)Pf); // Power Maju
//  voltageSenseVal++;
  //lcd.print(" R:"); lcd.print((int)Pr); // Power Pantul
  // Example usage 
  lcdDisplayValue(10, 0, voltageSenseVal, 'V'); // Displays "3.3V" at row 0, col 0 
  //Serial.printf("voltageSenseVal: %.2f\n", voltageSenseVal);
  Serial.print("voltageSenseVal: ");
  Serial.println(voltageSenseVal, 2);
  Serial.print("V");


  sensorValue = analogRead(35); // Example ADC pin 
  currentSenseVal = (sensorValue / 4095.0) * 3.3;
  lcdSetCursor(0, 1);
  //lcd.print(bands[bandIndex]); // Nama Band
  lcdPrint(" Current:"); 
//  currentSenseVal++;
  lcdDisplayValue(10, 1, currentSenseVal, 'A'); // Displays "25.6C" at row 1, col 0
  //Serial.printf("currentSenseVal: %.2f\n", currentSenseVal);
  Serial.print("currentSenseVal: ");
  Serial.println(currentSenseVal, 2);
  Serial.print("A");

//  if(currentSenseVal > 70 || voltageSenseVal > 70)
//  {
//    voltageSenseVal = 0;
//    currentSenseVal = 0;
//  }
  
//  if (swr > 20) lcd.print("HIGH");
//  else lcd.print(swr, 2);
  digitalWrite(led,HIGH);
  delay(de);
  //digitalWrite(led,LOW);
  //delay(de);
}