#include <LiquidCrystal.h>

// LCD Pins
LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

// Analog Pins
const uint8_t analogPinF = A0;
const uint8_t analogPinR = A1;

// Constants
const float Vreference = 5.00;
const float slopeF = 26.651;
const float interceptF = 39.6171;
const float slopeR = 27.481;
const float interceptR = 39.6171;

// System Variables
float voltageF, voltageR;
double powerdBmF, powerdBmR, vswr = 1.0;
uint32_t pow_fwd = 0, pow_ref = 0;
double peakPowerFwd = 0, peakPowerRef = 0;
uint32_t peakHoldTimer = 0;
const uint16_t peakHoldTime_Fast = 500;
const uint16_t peakHoldTime_Slow = 2000;
uint16_t peakHoldTime = peakHoldTime_Fast;

// Progress Bar
byte block[5][8] = {
    {0b10000,0b10000,0b10000,0b10000,0b10000,0b10000,0b10000,0b10000},
    {0b11000,0b11000,0b11000,0b11000,0b11000,0b11000,0b11000,0b11000},
    {0b11100,0b11100,0b11100,0b11100,0b11100,0b11100,0b11100,0b11100},
    {0b11110,0b11110,0b11110,0b11110,0b11110,0b11110,0b11110,0b11110},
    {0b11111,0b11111,0b11111,0b11111,0b11111,0b11111,0b11111,0b11111}
};

// Bar Graph Variables
int smooth_lev[2] = {0,0};
int lmax[2] = {0,0};
int dly[2] = {0,0};
const int Fall_time = 1;

/// Time measure
long duration = 0;
long start = 0;
/// End of Time measure
void setup() {
  Serial.begin(9600);
    
  // LCD Initialization
  lcd.begin(16, 2);
  for(int i=0; i<5; i++) lcd.createChar(i, block[i]);
  
  // Pin Modes
  pinMode(2, INPUT_PULLUP);
  pinMode(3, OUTPUT);
  pinMode(4, INPUT_PULLUP);
  digitalWrite(3, LOW);
  
  // Initial Display
  lcd.setCursor(0,0);
  lcd.print("Po:0W    SW:1.00");
  lcd.setCursor(0,1);
  lcd.print("                ");
}

void loop() {
  start = micros(); 
  // 1. Read Sensors
  uint16_t adcF = analogRead(analogPinF);
  uint16_t adcR = analogRead(analogPinR);
  
  // 2. Calculate Values
  voltageF = (adcF / 1023.0) * Vreference;
  voltageR = (adcR / 1023.0) * Vreference;
  
  powerdBmF = (slopeF * voltageF) - interceptF;
  powerdBmR = (slopeR * voltageR) - interceptR;
  
  pow_fwd = pow(10, (powerdBmF - 30)/10);
  pow_ref = pow(10, (powerdBmR - 30)/10);
  
  // 3. VSWR Calculation
  if(pow_fwd > pow_ref) {
    double refl_coeff = sqrt(pow_ref / (double)pow_fwd);
    vswr = (1 + refl_coeff)/(1 - refl_coeff);
  } else {
    vswr = 1.0;
  }

  // 4. Update Display
  if(digitalRead(4)) {
    updateForwardScreen();
  } else {
    updateReflectedScreen();
  }
  
  // 5. Update Bar Graph
  updateBarGraph();
  /////////////
duration = micros() - start;
Serial.println(duration);
//delay(1000);
}

void updateForwardScreen() {
  // Peak Hold Logic
  if(pow_fwd > peakPowerFwd) {
    peakPowerFwd = pow_fwd;
    peakHoldTimer = millis();
  } else if(millis() - peakHoldTimer > peakHoldTime) {
    peakPowerFwd = pow_fwd;
  }

  // Update LCD
  lcd.setCursor(0,0);
  lcd.print("Po:");
  lcd.print((int)peakPowerFwd);
  lcd.print("W    SW:");
  lcd.print(vswr, 2);
  lcd.print("  ");
}

void updateReflectedScreen() {
  // Peak Hold Logic
  if(pow_ref > peakPowerRef) {
    peakPowerRef = pow_ref;
    peakHoldTimer = millis();
  } else if(millis() - peakHoldTimer > peakHoldTime) {
    peakPowerRef = pow_ref;
  }

  // Update LCD
  lcd.setCursor(0,0);
  lcd.print("Pr:");
  lcd.print((int)peakPowerRef);
  lcd.print("W    SW:");
  lcd.print(vswr, 2);
  lcd.print("  ");
}

void updateBarGraph() {
  // Map power to bar levels
  int anF = map(pow_fwd, 0, 100, 0, 80);
  int anR = map(pow_ref, 0, 100, 0, 80);
  
  // Smoothing
  smooth_lev[0] = (anF > smooth_lev[0]) ? anF : smooth_lev[0] + (anF - smooth_lev[0])/Fall_time;
  
  // Peak Detection
  if(anF > lmax[0]) {
    lmax[0] = anF;
    dly[0] = 1000; // Hold time
  } else if(dly[0] > 0) {
    dly[0]--;
  } else {
    lmax[0] = max(lmax[0]-1, 0);
  }

  // Draw Bar
  lcd.setCursor(0,1);
  lcd.write(' ');
  for(int i=1; i<16; i++) {
    int f = constrain(smooth_lev[0]-i*5, 0, 5);
    int p = constrain(lmax[0]-i*5, 0, 5);
    lcd.write(f ? f : p);
  }
}