// The display also uses hardware SPI, plus #9 & #10
#define TFT_CS 15
#define TFT_DC 2
#define TFT_MOSI 23
#define TFT_SCLK 18

//#include <mcp_can.h>
//#include "vesc_can_bus_arduino.h"
#include "Digital-7100-L.h"
#include "Digital-7100-S.h"
#include <SPI.h>
#include <TFT_eSPI.h>       // Hardware-specific library

TFT_eSPI tft = TFT_eSPI();
//TFT_eSprite img = TFT_eSprite(&tft);  // Create a sprite object for entire screen
TFT_eSprite erpmSprite = TFT_eSprite(&tft);  // Create a sprite object for ERPM bar graph
TFT_eSprite voltageSprite = TFT_eSprite(&tft);  // Create a sprite object for Voltage bar graph
//TFT_eSprite mphSprite = TFT_eSprite(&tft);  // Create a sprite object for Voltage bar graph
TFT_eSprite mphSprite2 = TFT_eSprite(&tft);  // Create a sprite object for Voltage bar graph


//CAN can;             // get torque sensor data, throttle for now

bool print_realtime_data;
long last_print_data;
unsigned long last_refresh;
float dutyCycleMin0;
float speed, inpVoltage, dutyCycleNow, erpm, avgInputCurrent, avgMotorCurrent, tempFET, tempMotor;

// Define your min and max values for each variable you wish to simulate
const float ERPM_MIN = 0;
const float ERPM_MAX = 20000;
const float VOLTAGE_MIN = 42;
const float VOLTAGE_MAX = 58.8;
const float DUTY_CYCLE_MIN = 0;
const float DUTY_CYCLE_MAX = 1.0;  // Assuming duty cycle is between 0 and 1. Adjust if needed.
const float INPUT_CURRENT_MIN = 0;
const float INPUT_CURRENT_MAX = 100;  // Arbitrary max. Adjust as needed.
const float MOTOR_CURRENT_MIN = 0;
const float MOTOR_CURRENT_MAX = 200;  // Arbitrary max. Adjust as needed.
const float TEMP_FET_MIN = 0;
const float TEMP_FET_MAX = 80;  // Example temperature range in Celsius. Adjust if needed.
const float TEMP_MOTOR_MIN = 0;
const float TEMP_MOTOR_MAX = 80;  // Example temperature range in Celsius. Adjust if needed.

unsigned long lastSimulationUpdate = 0;
const unsigned long SIMULATION_INTERVAL = 100; // Adjust this to determine how fast values change

#define AA_FONT_LARGE Digital7100L
#define AA_FONT_SMALL Digital7100S

#define CAN0_INT 32                              // Set INT

//for img Sprite, (lower half text)
#define IWIDTH  320 //240 if 0 deg
#define IHEIGHT 240 //320 if 0 deg

//X IS HORZ
//Y IS VERT
// Constants for the ERPM bar graph
#define ERPM_BAR_X 5             // Starting X-coordinate of the bar //RELATIVE TO THE SPRITE WINDOW
#define ERPM_BAR_Y 0            // Starting Y-coordinate of the bar //RELATIVE TO THE SPRITE WINDOW
#define ERPM_BAR_WIDTH 300    // Maximum width of the bar graph when at max ERPM
#define ERPM_BAR_HEIGHT 30        // Height of the bar graph
#define ERPM_BAR_WIDTH_SPRITE 315    // Maximum width of the bar graph when at max ERPM
#define ERPM_BAR_HEIGHT_SPRITE 50        // Height of the bar graph
#define ERPM_NUM_PARTITIONS 10    // For values: 0, 5000, 10000, 15000, 20000
#define ERPM_INTERVAL 2000  // The ERPM value interval for each partition
#define ERPM_INTERVAL2 2  // The ERPM value interval for each partition

// Constants for the Input Voltage bar graph
#define VOLTAGE_BAR_X 5              // Starting X-coordinate of the bar //RELATIVE TO THE SPRITE WINDOW
#define VOLTAGE_BAR_Y 0              // Starting Y-coordinate of the bar //RELATIVE TO THE SPRITE WINDOW
#define VOLTAGE_BAR_WIDTH 300     // Maximum width of the bar graph when at max Voltage
#define VOLTAGE_BAR_HEIGHT 30         // Height of the bar graph
#define VOLTAGE_BAR_WIDTH_SPRITE 315     // Maximum width of the bar graph when at max Voltage
#define VOLTAGE_BAR_HEIGHT_SPRITE 50         // Height of the bar graph
//#define VOLTAGE_NUM_PARTITIONS 8.4     // For example values: 0, 10, 20, ..., 80 (adjust as needed)
#define VOLTAGE_MIN 42       // Define your minimum voltage
#define VOLTAGE_MAX 58.8     // Define your maximum voltage
#define VOLTAGE_INTERVAL 2 // The interval between each partition

#define MPH_WIDTH_SPRITE 200
#define MPH_HEIGHT_SPRITE 80

//#define TFT_CS    15     // TFT CS  pin is connected to  pin D2
//#define TFT_RST   2     // TFT RST pin is connected to  pin D3
//#define TFT_DC    4     // TFT DC  pin is connected to  pin D4
//#define TFT_SCLK  14
//#define TFT_MOSI  13

struct LimitedValue {
    float value;
    int decimalPlaces;
};

void setup()
{
    print_realtime_data = true;
    //Serial.begin(115200); // Wired comms from USB port

    //initialize();
    tft.init();
    tft.setRotation(1); //0 = 0, 1 = 90, 2 = 180, 3 = 270

    tft.fillScreen(TFT_BLACK);
    //img.setColorDepth(8); // Optionally set depth to 8 to halve RAM use
    //img.createSprite(IWIDTH, IHEIGHT);

    erpmSprite.createSprite(ERPM_BAR_WIDTH_SPRITE, ERPM_BAR_HEIGHT_SPRITE);
    voltageSprite.createSprite(VOLTAGE_BAR_WIDTH_SPRITE, VOLTAGE_BAR_HEIGHT_SPRITE);
    mphSprite2.createSprite(MPH_WIDTH_SPRITE, MPH_HEIGHT_SPRITE);
    //mphSprite2.createSprite(200,80);

    //img.fillSprite(TFT_BLACK);

    inpVoltage = VOLTAGE_MIN; //because non-zero starting

    //mphSprite.loadFont(AA_FONT_LARGE); // Load another different font into the sprite instance
    //mphSprite.setTextColor(TFT_WHITE, TFT_BLACK);
    mphSprite2.setTextColor(TFT_WHITE, TFT_BLACK);

    drawERPMBar(erpm);
    drawVoltageBar(inpVoltage);
    //drawMPH(erpm);

    //voltageSprite.pushSprite(VOLTAGE_BAR_X, VOLTAGE_BAR_Y);

    ////img.pushSprite(0, 0); // push all sprite data to visible screen

}

void loop()
{
    //img.setTextSize(2);
    //img.fillSprite(TFT_BLACK);
    //img.setTextColor(TFT_GREEN, TFT_BLACK);

    //spin(); // REAL TIME DATA
    simulateData(); // SIMULATION DATA
    
    if (print_realtime_data == true)
    {
        //Serial.println("true");
        if (millis() - last_print_data > 200)
        {
            /*Serial.println(erpm); 
            Serial.println(inpVoltage);
            Serial.println(dutyCycleNow);
            Serial.println(avgInputCurrent);
            Serial.println(avgMotorCurrent);
            Serial.println(tempFET);
            Serial.println(tempMotor);*/

            last_refresh = millis();

            if (dutyCycleNow < 0)
              {
                dutyCycleMin0 = 0.00;
              } else 
              {
                dutyCycleMin0 = dutyCycleNow;
              }         
            
            //updateERPMBar(erpm);//  OLD
            //updateInputVoltageBar(inpVoltage); // OLD
            
            
            //img.drawString(String("ERPM: ") + erpm, 0, 42, 1); //32

            // For IPVT
            LimitedValue limitedValue = limitToThreeSignificantDigits(inpVoltage);
            String formattedValue = formatFloat(limitedValue.value, limitedValue.decimalPlaces);
            //img.drawString(String("IPVT: ") + formattedValue, 0, 98, 1); //64

            // For DUTY
            float dutyPercentage = dutyCycleMin0 * 100;
            LimitedValue limitedDuty = limitToThreeSignificantDigits(dutyPercentage);
            String formattedDuty = formatFloat(limitedDuty.value, limitedDuty.decimalPlaces) + "%";  // Add the percentage symbol
            //img.drawString(String("DUTY: ") + formattedDuty, 0, 118, 1);  // Adjust y-coordinate as needed

            // For IPCR
            LimitedValue limitedIPCR = limitToThreeSignificantDigits(avgInputCurrent);
            String formattedIPCR = formatFloat(limitedIPCR.value, limitedIPCR.decimalPlaces);
            //img.drawString(String("IPCR: ") + formattedIPCR, 0, 138, 1);  // Adjust y-coordinate as needed

            // For MTCR
            LimitedValue limitedMTCR = limitToThreeSignificantDigits(avgMotorCurrent);
            String formattedMTCR = formatFloat(limitedMTCR.value, limitedMTCR.decimalPlaces);
            //img.drawString(String("MTCR: ") + formattedMTCR, 0, 158, 1);  // Adjust y-coordinate as needed

            // For TMPF
            LimitedValue limitedTMPF = limitToThreeSignificantDigits(tempFET);
            String formattedTMPF = formatFloat(limitedTMPF.value, limitedTMPF.decimalPlaces);
            //img.drawString(String("TMPF: ") + formattedTMPF, 0, 178, 1);  // Adjust y-coordinate as needed

            // For TMPM
            LimitedValue limitedTMPM = limitToThreeSignificantDigits(tempMotor);
            String formattedTMPM = formatFloat(limitedTMPM.value, limitedTMPM.decimalPlaces);
            //img.drawString(String("TMPM: ") + formattedTMPM, 0, 198, 1);  // Adjust y-coordinate as needed

            // For MPH
            speed = erpmToMph(erpm,3,80,11,15);
            LimitedValue limitedMPH = limitToThreeSignificantDigits(speed);
            String formattedMPH = formatFloat(limitedMPH.value, limitedMPH.decimalPlaces);
            //img.drawString(String("MPH:  ") + formattedMPH, 0, 218, 1);  // Adjust y-coordinate as needed
            
            //img.pushSprite(0, 80); // push all sprite data to visible screen

            drawERPMBar(erpm);
            drawVoltageBar(inpVoltage);
            //drawMPH(erpm);

            float speed = erpmToMph(erpm,3,80,11,15);
            drawFormattedFloat(mphSprite2, speed);

            erpmSprite.pushSprite(0, 0);
            voltageSprite.pushSprite(0, 60);
            //mphSprite.pushSprite(150,150);
            mphSprite2.pushSprite(176,172);

            //Serial.println(millis() - last_refresh);
            //Serial.println(millis() - last_print_data);

            last_print_data = millis();
        }
    }
}


float erpmToMph(float erpm, int polePairs, float axleTeeth, float clutchTeeth, float wheelDiameterInches) {
  // Calculate the drive wheel circumference in feet
  float wheelCircumferenceFeet = (PI * wheelDiameterInches) / 12.0; // Convert inches to feet

  // Calculate the gear ratio
  float gearRatio = axleTeeth / clutchTeeth;

  // Convert eRPM to mechanical RPM (Wheel RPM)
  float wheelRPM = erpm / polePairs / gearRatio;

  // Calculate feet per minute
  float feetPerMinute = wheelCircumferenceFeet * wheelRPM;

  // Calculate feet per hour
  float feetPerHour = feetPerMinute * 60.0;

  // Convert feet per hour to miles per hour
  float mph = feetPerHour / 5280.0;

  return mph;
}

void simulateData() {
    if (millis() - lastSimulationUpdate > SIMULATION_INTERVAL) {
        // Change the ERPM value
        erpm += 200;
        if (erpm > ERPM_MAX) erpm = ERPM_MIN;

        // Change the Input Voltage
        inpVoltage += 0.1;
        if (inpVoltage > VOLTAGE_MAX) inpVoltage = VOLTAGE_MIN;

        // Change the Duty Cycle
        dutyCycleNow += 0.01;
        if (dutyCycleNow > DUTY_CYCLE_MAX) dutyCycleNow = DUTY_CYCLE_MIN;

        // Change the Average Input Current
        avgInputCurrent += .1;
        if (avgInputCurrent > INPUT_CURRENT_MAX) avgInputCurrent = INPUT_CURRENT_MIN;

        // Change the Average Motor Current
        avgMotorCurrent += .1;
        if (avgMotorCurrent > MOTOR_CURRENT_MAX) avgMotorCurrent = MOTOR_CURRENT_MIN;

        // Change the FET Temperature
        tempFET += .1;
        if (tempFET > TEMP_FET_MAX) tempFET = TEMP_FET_MIN;

        // Change the Motor Temperature
        tempMotor += .1;
        if (tempMotor > TEMP_MOTOR_MAX) tempMotor = TEMP_MOTOR_MIN;

        lastSimulationUpdate = millis();
    }
}

LimitedValue limitToThreeSignificantDigits(float value) {
    LimitedValue result;
    if (value >= 100) {
        result.value = round(value);
        result.decimalPlaces = 0;
    } else if (value >= 10) {
        result.value = round(value * 10) / 10.0;
        result.decimalPlaces = 1;
    } else {
        result.value = round(value * 100) / 100.0;
        result.decimalPlaces = 2;
    }
    return result;
}

String formatFloat(float value, int decimalPlaces) {
    return String(value, decimalPlaces);
}

void drawERPMGradientBar(int x, int y, int width, int height, float value, float minValue, float maxValue, uint16_t startColor, uint16_t middleColor, uint16_t endColor, float firstTransition, float secondTransition) {
    int fillWidth = (int)(((value - minValue) / (maxValue - minValue)) * width);
    for (int i = x; i < x + fillWidth; i++) {
        float currentRatio = (i - x) / (float)width;
        uint16_t currentColor;
        if (currentRatio < firstTransition) {
            float localRatio = currentRatio / firstTransition;
            currentColor = blendColor(startColor, middleColor, localRatio);
        } else if (currentRatio < secondTransition) {
            float localRatio = (currentRatio - firstTransition) / (secondTransition - firstTransition);
            currentColor = blendColor(middleColor, endColor, localRatio);
        } else {
            currentColor = endColor;
        }
        erpmSprite.drawFastVLine(i, y, height, currentColor);
    }
}

void drawVoltageGradientBar(int x, int y, int width, int height, float value, float minValue, float maxValue, uint16_t startColor, uint16_t middleColor, uint16_t endColor, float firstTransition, float secondTransition) {
    int fillWidth = (int)(((value - minValue) / (maxValue - minValue)) * width);
    for (int i = x; i < x + fillWidth; i++) {
        float currentRatio = (i - x) / (float)width;
        uint16_t currentColor;
        if (currentRatio < firstTransition) {
            float localRatio = currentRatio / firstTransition;
            currentColor = blendColor(startColor, middleColor, localRatio);
        } else if (currentRatio < secondTransition) {
            float localRatio = (currentRatio - firstTransition) / (secondTransition - firstTransition);
            currentColor = blendColor(middleColor, endColor, localRatio);
        } else {
            currentColor = endColor;
        }
        voltageSprite.drawFastVLine(i, y, height, currentColor);
    }
}

void drawERPMBar(float erpm) {
    erpmSprite.fillSprite(TFT_BLACK);  // Clear the previous bar graph
    
    // Draw the empty bar as background
    erpmSprite.drawRect(ERPM_BAR_X, ERPM_BAR_Y, ERPM_BAR_WIDTH, ERPM_BAR_HEIGHT, TFT_WHITE);

    // Draw the gradient bar BEFORE
    drawERPMGradientBar(ERPM_BAR_X+1, ERPM_BAR_Y+1, ERPM_BAR_WIDTH-1, ERPM_BAR_HEIGHT-2, erpm, ERPM_MIN, ERPM_MAX, TFT_GREEN, TFT_YELLOW, TFT_RED, 0.7f, 0.9f);

    
    for(int i = 0; i <= ERPM_NUM_PARTITIONS; i++) {
        int partitionX = ERPM_BAR_X + (i * (ERPM_BAR_WIDTH / ERPM_NUM_PARTITIONS));
        erpmSprite.drawFastVLine(partitionX, ERPM_BAR_Y, ERPM_BAR_HEIGHT, TFT_WHITE);  // Vertical line for partition
        erpmSprite.setTextColor(TFT_WHITE, TFT_BLACK);
        erpmSprite.drawString(String(i * ERPM_INTERVAL2), partitionX - 5, ERPM_BAR_Y + ERPM_BAR_HEIGHT + 2, 2);  // Adjust positioning as needed
    }
    
    // Draw the gradient bar AFTER
    //drawERPMGradientBar(ERPM_BAR_X+1, ERPM_BAR_Y+1, ERPM_BAR_WIDTH-1, ERPM_BAR_HEIGHT-2, erpm, ERPM_MIN, ERPM_MAX, TFT_GREEN, TFT_YELLOW, TFT_RED, 0.7f, 0.9f);
}

void drawVoltageBar(float voltage) {
    voltageSprite.fillSprite(TFT_BLACK);  // Clear the previous bar graph

    // Draw the empty bar as background
    voltageSprite.drawRect(VOLTAGE_BAR_X, VOLTAGE_BAR_Y, VOLTAGE_BAR_WIDTH, VOLTAGE_BAR_HEIGHT, TFT_WHITE);

    // Draw the gradient bar BEFORE
    drawVoltageGradientBar(VOLTAGE_BAR_X+1, VOLTAGE_BAR_Y+1, VOLTAGE_BAR_WIDTH-1, VOLTAGE_BAR_HEIGHT-2, voltage, VOLTAGE_MIN, VOLTAGE_MAX, TFT_RED, TFT_YELLOW, TFT_GREEN, 0.4f, 0.7f);

    const int VOLTAGE_NUM_PARTITIONS = (VOLTAGE_MAX - VOLTAGE_MIN) / VOLTAGE_INTERVAL;
    for(int i = 0; i <= VOLTAGE_NUM_PARTITIONS; i++) {
        int partitionX = VOLTAGE_BAR_X + (i * (VOLTAGE_BAR_WIDTH / VOLTAGE_NUM_PARTITIONS));
        voltageSprite.drawFastVLine(partitionX, VOLTAGE_BAR_Y, VOLTAGE_BAR_HEIGHT, TFT_WHITE);  // Vertical line for partition
        voltageSprite.setTextColor(TFT_WHITE, TFT_BLACK);
        int valueForPartition = VOLTAGE_MIN + (i * VOLTAGE_INTERVAL);
        voltageSprite.drawString(String(valueForPartition), partitionX - 4, VOLTAGE_BAR_Y + VOLTAGE_BAR_HEIGHT + 2, 2);  // Adjust positioning as needed
    }

    // Draw the gradient bar AFTER
    //drawVoltageGradientBar(VOLTAGE_BAR_X+1, VOLTAGE_BAR_Y+1, VOLTAGE_BAR_WIDTH-1, VOLTAGE_BAR_HEIGHT-2, voltage, VOLTAGE_MIN, VOLTAGE_MAX, TFT_RED, TFT_YELLOW, TFT_GREEN, 0.4f, 0.7f);
}

/* //Old version
void drawMPH(float erpm) {
    mphSprite.fillSprite(TFT_BLACK);  // Clear the previous bar graph
    
    // box around digits
    //mphSprite.drawRect(0, 0, 120, 100, TFT_WHITE);

    mphSprite.setTextColor(TFT_WHITE, TFT_BLACK);

    speed = erpmToMph(erpm,3,80,11,15);
    //LimitedValue limitedMPH = limitToThreeSignificantDigits(speed);
    //String formattedMPH = formatFloat(limitedMPH.value, limitedMPH.decimalPlaces);

    mphSprite.setTextDatum(TR_DATUM); // Top Right datum
    mphSprite.drawFloat(speed, 1, 150, 0); // decimals, x, y
    
    //mphSprite.drawString(String(formattedMPH), 0, 0, 4);
}
*/

// Blend function (as provided earlier)
uint16_t blendColor(uint16_t colorStart, uint16_t colorEnd, float ratio) {
    if (ratio > 1.0f) ratio = 1.0f;
    if (ratio < 0.0f) ratio = 0.0f;

    uint8_t rStart = (colorStart & 0xF800) >> 8;
    uint8_t gStart = (colorStart & 0x07E0) >> 3;
    uint8_t bStart = (colorStart & 0x001F) << 3;

    uint8_t rEnd = (colorEnd & 0xF800) >> 8;
    uint8_t gEnd = (colorEnd & 0x07E0) >> 3;
    uint8_t bEnd = (colorEnd & 0x001F) << 3;

    uint8_t rBlend = (1.0f - ratio) * rStart + ratio * rEnd;
    uint8_t gBlend = (1.0f - ratio) * gStart + ratio * gEnd;
    uint8_t bBlend = (1.0f - ratio) * bStart + ratio * bEnd;

    return ((rBlend >> 3) << 11) | ((gBlend >> 2) << 5) | (bBlend >> 3);
}

void drawFormattedFloat(TFT_eSprite &sprite, float value) {
    sprite.fillScreen(TFT_BLACK);
    int intValue = (int)value;  // Get the integer part
    int decimalValue = (int)((value - intValue) * 10);  // Get the decimal part, assuming one decimal place

    // Convert integer and decimal parts to strings
    String intStr = String(intValue);
    String decimalStr = String(decimalValue);

    //int intWidth = sprite.textWidth(intStr);  // Get the width of the integer part
    sprite.loadFont(AA_FONT_LARGE); // Load another different font into the sprite instance
    sprite.setTextDatum(TR_DATUM); // Top Right datum
    sprite.drawString(intStr, 100, 0); //relative to sprite window, TR DATUM is top right and prints left of x,y here

    // decimal part and draw it
    sprite.loadFont(AA_FONT_SMALL); // Load another different font into the sprite instance
    sprite.setTextDatum(TL_DATUM); // Top Left datum
    sprite.drawString("." + decimalStr, 102, 30);  // Assuming font 1 for the decimal part
}