#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Adafruit_FT6206.h>
#include <TimerFour.h>
#include <Wire.h>

// Set up the touchscreen
#define TFT_CS 10
#define TFT_RST 9
#define TFT_DC 8
#define TFT_MOSI 51
#define TFT_MISO 50
#define TFT_SCK 52

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
Adafruit_FT6206 ts = Adafruit_FT6206();

// This is calibration data for the raw touch data to the screen coordinates
#define TS_MINX 150
#define TS_MINY 130
#define TS_MAXX 3800
#define TS_MAXY 4000

// Set up the button dimensions and positions
#define BUTTON_WIDTH 50
#define BUTTON_HEIGHT 50
#define BUTTON_GAP 30
#define BUTTON_X1 0
#define BUTTON_X2 (BUTTON_X1 + BUTTON_WIDTH + BUTTON_GAP)
#define BUTTON_X3 (BUTTON_X2 + BUTTON_WIDTH + BUTTON_GAP)
#define BUTTON_X4 (BUTTON_X3 + BUTTON_WIDTH + BUTTON_GAP)
#define BUTTON_Y 100

// Set up the output pins
#define HPFP_PIN 3 // HP Pump output
#define FCV_PIN 4 //Flow Control Valve output
#define EMS_PIN 5 //EMS output to FCM
int EMS_FREQ = 0;

// define PWM frequencies
#define HPFP_FREQ 100 // 100Hz HPFP
#define FCV_FREQ 25 // 25Hz FCV

// Define the output enabled flag
bool output_enabled = false;

void setup() {
  tft.begin();
  ts.begin();
  Wire.begin();
  
  // Set the rotation to landscape mode
  tft.setRotation(1);

  // Configure Timer4 for HPFP_PIN
  pinMode(HPFP_PIN, OUTPUT);
  Timer4.initialize(1000000 / HPFP_FREQ); // Set frequency for 100Hz
  Timer4.pwm(HPFP_PIN, 0); // Set initial duty cycle to 0
  
  // Configure Timer4 for FCV_PIN
  pinMode(FCV_PIN, OUTPUT);
  Timer4.initialize(1000000 / FCV_FREQ); // Set frequency for 25Hz
  Timer4.pwm(FCV_PIN, 0); // Set initial duty cycle to 0

  // Configure Timer4 for EMS_PIN
  pinMode(EMS_PIN, OUTPUT);
  Timer4.initialize(1000000 / EMS_FREQ); // Set frequency for 50Hz
  Timer4.pwm(EMS_PIN, 0); // Set initial duty cycle to 0

  // Set the heading
  tft.setCursor(tft.width()/6 - strlen("Fuel Pump Control")*3, 10);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.print("Fuel Pump Control");
}

void loop() {
  // Draw the four rounded rectangles
  tft.fillRoundRect(BUTTON_X1, BUTTON_Y, BUTTON_WIDTH, BUTTON_HEIGHT, 10, ILI9341_RED);
  tft.fillRoundRect(BUTTON_X2, BUTTON_Y, BUTTON_WIDTH, BUTTON_HEIGHT, 10, ILI9341_RED);
  tft.fillRoundRect(BUTTON_X3, BUTTON_Y, BUTTON_WIDTH, BUTTON_HEIGHT, 10, ILI9341_RED);
  tft.fillRoundRect(BUTTON_X4, BUTTON_Y, BUTTON_WIDTH, BUTTON_HEIGHT, 10, ILI9341_RED);

  // Add label text to each button
  tft.setTextSize(2);

  // Button 1
  int button1_center = BUTTON_X1 + (BUTTON_WIDTH / 2);
  int label1_width = 12 * 3; // width of "15%"
  int label1_x = button1_center - (label1_width / 2);
  tft.setCursor(label1_x, BUTTON_Y - 20);
  tft.print("20%");

  // Button 2
  int button2_center = BUTTON_X2 + (BUTTON_WIDTH / 2);
  int label2_width = 12 * 3; // width of "40%"
  int label2_x = button2_center - (label2_width / 2);
  tft.setCursor(label2_x, BUTTON_Y - 20);
  tft.print("30%");

  // Button 3
  int button3_center = BUTTON_X3 + (BUTTON_WIDTH / 2);
  int label3_width = 12 * 3; // width of "80%"
  int label3_x = button3_center - (label3_width / 2);
  tft.setCursor(label3_x, BUTTON_Y - 20);
  tft.print("50%");

  // Button 4
  int button4_center = BUTTON_X4 + (BUTTON_WIDTH / 2);
  int label4_width = 12 * 6; // width of "On/Off"
  int label4_x = button4_center - (label4_width / 2);
  tft.setCursor(label4_x, BUTTON_Y - 20);
  tft.print("On/Off");


  // Check if a button is pressed
  TS_Point touch = ts.getPoint();

  // Default duty cycle value
  int duty_cycle = 0;

  // Check if output is enabled before updating duty cycle
  if (output_enabled) {
    // Check which button is pressed and set the duty cycle accordingly
    if (touch.z >= 10 && touch.z <= 1000 && touch.x >= BUTTON_X1 && touch.x <= BUTTON_X1 + BUTTON_WIDTH &&
        touch.y >= BUTTON_Y && touch.y <= BUTTON_Y + BUTTON_HEIGHT) {
      duty_cycle = 51; // duty cycle = (percent / 100) * 255 and change label
       // Update EMS PWM frequency
      EMS_FREQ = 120; // EMS output HPFP 20%      
    }
    else if (touch.z >= 10 && touch.z <= 1000 && touch.x >= BUTTON_X2 && touch.x <= BUTTON_X2 + BUTTON_WIDTH &&
            touch.y >= BUTTON_Y && touch.y <= BUTTON_Y + BUTTON_HEIGHT) {
      duty_cycle = 76; // duty cycle = (percent / 100) * 255 and change label
      //Update EMS PWM frequency
      EMS_FREQ = 130; // EMS output HPFP 30%      
    }
    else if (touch.z >= 10 && touch.z <= 1000 && touch.x >= BUTTON_X3 && touch.x <= BUTTON_X3 + BUTTON_WIDTH &&
            touch.y >= BUTTON_Y && touch.y <= BUTTON_Y + BUTTON_HEIGHT) {
      duty_cycle = 127; // duty cycle = (percent / 100) * 255 and change label
       // Update PWM frequency
      EMS_FREQ = 150; // EMS output HPFP 50%
    }
  }

  // Check if button 4 is pressed
  if (touch.z >= 10 && touch.z <= 1000 && touch.x >= BUTTON_X4 && touch.x <= BUTTON_X4 + BUTTON_WIDTH &&
      touch.y >= BUTTON_Y && touch.y <= BUTTON_Y + BUTTON_HEIGHT) {
    // Toggle the output
    if (output_enabled) {
      analogWrite(HPFP_PIN, 0);
      output_enabled = false;
    } else {
      analogWrite(HPFP_PIN, duty_cycle);
      output_enabled = true;
    }
  } else if (!output_enabled) {
    // Disable output if no button is pressed and output is disabled
    analogWrite(HPFP_PIN, 0);
  }

  // Output the PWM signal if enabled
  if (output_enabled) {
    analogWrite(HPFP_PIN, duty_cycle);
    analogWrite(FCV_PIN, 30); 
    analogWrite(EMS_PIN, 38);
  }
  // Print the current duty cycle at the bottom of the screen
  tft.setTextSize(1);
  tft.setCursor(20, tft.height() - 20);
  tft.setTextColor(ILI9341_WHITE);
  tft.print("Frequency - 100Hz Duty Cycle - ");
  tft.print(duty_cycle);
  tft.print("%");
}