#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "stdio.h"
#include "stdlib.h"
#include "driver/twai.h"
#include "SD.h"
#include "SPI.h"
#include "FS.h"
#include <RTClib.h>

// #include <Wire.h>
// #include <ESP32SPISlave.h>

RTC_DS1307 rtc; // Create an RTC object

#define RX_PIN 33
#define TX_PIN 32

#define SD_CS_PIN 5  // Adjust according to your setup
#define charger_lock 27
#define charger_relay 14
#define vehicle_shift_check 26
#define charging_system_fault_check 25
#define normal_stop_request_before_charging_check 13
// #define LED_not_connected 27
// #define LED_connecting 26
// #define LED_connected 25

// ESP32SPISlave slave;
// static constexpr uint32_t BUFFER_SIZE {32};
// uint8_t spi_slave_tx_buf[BUFFER_SIZE];
// uint8_t spi_slave_rx_buf[BUFFER_SIZE];

// float Vmap;
// float Cmap;
// float Tmap;
// float Vpercent;

// RTC global define
// RTC_DS1307 rtc;
// char daysOfTheWeek[7][12] = {"Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday"};


// Global calling function for transceiver from battery to charge controller
twai_message_t battery_voltage;
twai_message_t battery_temperature_value; 

// from ev to dc station
twai_message_t id_transmit_102; 
twai_message_t id_transmit_101;
twai_message_t id_transmit_100;

// from dc station to ev 
twai_message_t id_receive_109;
twai_message_t id_receive_108;


// Other variable send to Charger-Off-board
uint8_t Status_for_id_100[8] = {00, 00, 00, 00, 00, 00, 00, 00};
uint8_t Status_for_id_101[8] = {00, 00, 00, 00, 00, 00, 00, 00};
uint8_t Status_for_id_102[8] = {00, 00, 00, 00, 00, 00, 00, 00};
uint8_t Status_for_id_108[8] = {00, 00, 00, 00, 00, 00, 00, 00};
uint8_t Status_for_id_109[8] = {00, 00, 00, 00, 00, 00, 00, 00};
uint8_t validate_status_for_id_109[8] = {50, 00, 00, 00, 00, 00, 00, 00};


// id 100
float maximum_battery_voltage; uint16_t maximum_battery_voltage_res; uint8_t maximum_battery_voltage_res_msb; uint8_t maximum_battery_voltage_res_lsb;
uint8_t constant_of_charging_rate_indication;

// id 101
float rated_capacity_of_battery; uint16_t rated_capacity_of_battery_res; uint8_t rated_capacity_of_battery_res_msb; uint8_t rated_capacity_of_battery_res_lsb;
float maximum_charging_time; int minutes_time; int seconds_time; 
uint8_t estimated_charging_time = 0;

// id 102
uint8_t supportedAppProtocol_Req;   // control protocol number
float target_battery_voltage; uint16_t target_battery_voltage_res; uint8_t target_battery_voltage_res_msb; uint8_t target_battery_voltage_res_lsb;
uint8_t charging_current_request;
uint8_t charging_rate;
uint8_t vehicle_charging_enable = 0;
uint8_t vehicle_shift_lever_position = 0;
uint8_t charging_system_fault = 0;
uint8_t vehicle_status = 1;
uint8_t normal_stop_request_before_charging = 0;
uint8_t battery_overvoltage = 0;
uint8_t battery_undervoltage = 0;
uint8_t battery_current_deviation_error = 0;
uint8_t high_battery_temperature = 0;
uint8_t battery_voltage_deviation_error = 0;
uint8_t states_byte_5[5];
uint8_t states_byte_4[5];
uint8_t states_value_byte_5 = 0;
uint8_t states_value_byte_4 = 0;
uint8_t control_protocol_number = 25;


// id 108
uint16_t available_output_voltage; uint8_t available_output_voltage_msb; uint8_t available_output_voltage_lsb;
uint8_t available_output_current;
float charging_power;
uint16_t threshold_voltage; uint8_t threshold_voltage_msb; uint8_t threshold_voltage_lsb;

// id 109
uint8_t case_109_5;
uint8_t supportedAppProtocol_Res;   // control protocol number
uint16_t output_voltage; uint8_t output_voltage_msb; uint8_t output_voltage_lsb;
uint16_t output_current; uint8_t output_current_msb; uint8_t output_current_lsb;
uint8_t remaining_charging_time;
uint8_t station_status = 0;
uint8_t station_malfunction = 0;
uint8_t vehicle_connector_lock = 0;
uint8_t battery_incompatibility = 0;
uint8_t charging_system_malfunction = 0;
uint8_t charger_stop_control = 0;

//*******************************
// State monitoring charging 
int isChargerConnectedLogging = 0;
int isChargingDataExchangeLogging = 0;
int isChargingDataChargingLogging = 0;
int isEndChargingLogging = 0;

// state monitoring for charging logging ev side
int vehicleChargingEnableLogging = 0;
int vehicleChargingShiftLeverPositionLogging = 0;
int chargingSystemFaultLogging = 0;
int vehicleStatusLogging = 0;
int normalStopRequestBeforeChargingLogging = 0;
int batteryOvervoltageLogging = 0;
int batteryUndervoltageLogging = 0;
int batteryCurrentDeviationErrorLogging = 0;
int highBatteryTemperatureLogging = 0;
int batteryVoltageDeviationErrorLogging = 0;

// state monitoring for charging logging charger side
int stationStatusLogging = 0;
int stationMalfunctionLogging = 0;
int vehicleConnectorLockLogging = 0;
int batteryIncompatibilityLogging = 0;
int chargingSystemMalfunctionLogging = 0;
int chargerStopControlLogging = 0;

// value monitoring for exchange logging
int availableVoltageInputLogging = 0;
int availableCurrentOutputLogging = 0;
int voltageThresholdLogging = 0;
double availablePowerOutputLogging = 0.0;
int batteryVoltageMaximumLogging = 0;
int batteryRatedCapacityLogging = 0;
int maxChargingMinuteLogging = 0;
int maxChargingSecondLogging = 0;

// value monitoring for data charging logging
int batteryTargetVoltageLogging = 0;
int batteryTargetCurrentLogging = 0;
int chargingRateVehicleLogging = 0;
int supplyVoltageChargerLogging = 0;
double supplyCurrentChargerLogging = 0.0;
int remainingTimeBeforeEndChargingSecondLogging = 0;
int remainingTimeBeforeEndChargingMinuteLogging = 0;

// Global define 
uint8_t battery_charger_temp;
float battery_temp_mapping;
uint16_t battery_charger_output_current; uint8_t battery_charger_output_current_msb; uint8_t battery_charger_output_current_lsb; 
uint16_t battery_charger_output_voltage; uint8_t battery_charger_output_voltage_msb; uint8_t battery_charger_output_voltage_lsb;
float battery_voltage_mapping;
float battery_current_mapping;
float battery_charger_output_voltage_percent;

uint8_t battery_temp_heat_target = 52;      // 52°C   *The optimum ambient temperature for charging a Lithium battery is +5°C to +45°C / 41°F to 113°F
uint8_t battery_low = 60;                   // condition battery in low voltage
int enable;



// Global variables to track condition status
bool receiver_verification_status = false;
bool transmit_verification_status = false;
bool isConnected = false;
bool temperature_received = false;
bool voltage_current_received = false;
bool start_exchange = false;

// create variable for stage
/**
1 => initial stage for sending acknowledge
2 => stage for sending data battery and other vehicle and battery status
3 => stage for controlling current and voltage
4 => stage for ending charging session
*/
int stageProgress = 1;

// create variable for each memory stagee of id memory 100
bool isSendOnId100 = false;
bool isSendOnId101 = false;
bool isSendOnId102 = false;
bool isReceivedOnId108 = false;
bool isReceivedOnId109 = false;
bool isIdentificationSend = false;

String all_data = "";

long prev_millis_bat = millis();


void twai_setup_and_install(){
  //Initialize configuration structures using macro initializers
    twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)TX_PIN, (gpio_num_t)RX_PIN, TWAI_MODE_NORMAL);
    twai_timing_config_t t_config = TWAI_TIMING_CONFIG_250KBITS();
    twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

  // Install TWAI driver
  if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) {
    printf("Driver installed\n");
  } else {
    printf("Failed to install driver\n");
    return;
  }

  // Start TWAI driver
  if (twai_start() == ESP_OK) {
    printf("Driver started\n");
  } else {
    printf("Failed to start driver\n");
    return;
  }
}

bool message_transfer(twai_message_t *message, uint32_t id, uint8_t dlc, uint8_t *data){ 
  message->flags = TWAI_MSG_FLAG_EXTD;
  message->identifier = id;
  message->data_length_code = dlc;

  for (int i = 0; i < dlc; i++) {
    message->data[i] = data[i];
  }
  printf("\nID: %x DLC: %d Data:\t", message->identifier, message->data_length_code); 
  for (int i = 0; i < message->data_length_code; i++) {
    printf("%02X\t", message->data[i]);
  }
  return transmit_message(message);
}

bool transmit_message(twai_message_t *message){
  if (twai_transmit(message, pdMS_TO_TICKS(1000)) == ESP_OK) {
    printf("Message queued for transmission\n");
    return true;
  } else {
    printf("Failed to send message\n");
    return false;
  }
}


/*
void databatterySPI(){
  // block until the transaction comes from master
  slave.wait(spi_slave_rx_buf, spi_slave_tx_buf, BUFFER_SIZE);
  Serial.println("");
  byte command = spi_slave_rx_buf[0];
  if (command == 0b00000001) {
      Serial.println("data receive");
  } else if (command == 0b00000000) {
      Serial.println("data no yet receive");
  }

  Vmap = spi_slave_rx_buf[1];
  Serial.print("Received Voltage Mapping value: ");
  Serial.println(Vmap);

  Vpercent = spi_slave_rx_buf[3];
  Serial.print("Received Voltage Percent value: ");
  Serial.println(Vpercent);

  Cmap = spi_slave_rx_buf[2];
  Serial.print("Received Current Mapping value: ");
  Serial.println(Cmap);

  Tmap = spi_slave_rx_buf[4];
  Serial.print("Received Temp Mapping value: ");
  Serial.println(Tmap);

  // Acknowledge to the master that the data has been received
  spi_slave_tx_buf[0] = 0b00000010; // Send an acknowledgment byte (e.g., 0b00000010)
}
*/


void databattery(){
      if(twai_receive(&battery_voltage, pdMS_TO_TICKS(2000)) == ESP_OK){
        if(battery_voltage.identifier == 0x18FD15FE){
          printf("\nID: %X Data: \t", battery_voltage.identifier, battery_voltage.data_length_code);
          for(int i=0; i < battery_voltage.data_length_code; i++){
            printf("%02X\t", battery_voltage.data[i]); 
          }

          battery_charger_output_current = (battery_voltage.data[4] << 8) | battery_voltage.data[3];
          battery_charger_output_voltage = (battery_voltage.data[2] << 8) | battery_voltage.data[1];
          battery_voltage_mapping = (float)battery_charger_output_voltage * (82.15 / 4095.0);
          battery_current_mapping = (float)battery_charger_output_current * (20.0 / 4095.0);
          battery_charger_output_voltage_percent = (battery_voltage_mapping/82.15) * 100;             
          printf("\nVoltage: %.f V\nPercent: %.f %%\nCurrent: %.f A\n", battery_voltage_mapping, battery_charger_output_voltage_percent, battery_current_mapping);

        }         
      }

      if(twai_receive(&battery_temperature_value, pdMS_TO_TICKS(2000)) == ESP_OK){
        if(battery_temperature_value.identifier == 0x18FE50FE){
          printf("\nID: %X Data: \t", battery_temperature_value.identifier, battery_temperature_value.data_length_code);
          for(int i=0; i < battery_temperature_value.data_length_code; i++){
            printf("%02X\t", battery_temperature_value.data[i]); 
          }
          battery_charger_temp = battery_temperature_value.data[1]; 
          battery_temp_mapping = (float)battery_charger_temp * (60.0 /255.0); 

          printf("\nTemperature: %.f °C\n", battery_temp_mapping);                  
        }
      }
}


void identification_chargecontroller_chargeroffboard(){
  // check for response from ev
  Serial.println("Check For Response...");
  if(twai_receive(&id_receive_109, pdMS_TO_TICKS(1000)) == ESP_OK){
    Serial.print("Id Response : ");
    Serial.println(id_receive_109.identifier);
    Serial.print("IS id same : ");
    Serial.println(id_receive_109.identifier == 0x109);

    if(id_receive_109.identifier == 0x109){
      printf("\nID: %X  Data ID 109: \t", id_receive_109.identifier, id_receive_109.data_length_code);

      for(int i=0; i < id_receive_109.data_length_code; i++){
        printf("%d\t", id_receive_109.data[i]); 
      }

      if (id_receive_109.data[0] != validate_status_for_id_109[0]){
          receiver_verification_status = false;
          printf("\nData charger off-board not match!");
      } else{
        receiver_verification_status = true;

        isIdentificationSend = true;
        Status_for_id_109[0] = id_receive_109.data[0];
      }
    }
  }
  String waktu = time_rtc();
  // printf("\n%s", waktu);
  Serial.println(waktu);
  all_data += String(receiver_verification_status) + "," + String(transmit_verification_status) + "," + String(vehicleChargingShiftLeverPositionLogging) + "," + String(chargingSystemFaultLogging) + "," + String(vehicleStatusLogging) + "," + String(normalStopRequestBeforeChargingLogging) + "," + String(batteryOvervoltageLogging) + "," + String(batteryUndervoltageLogging) + "," + String(batteryCurrentDeviationErrorLogging) + "," + String(highBatteryTemperatureLogging) + "," + String(batteryVoltageDeviationErrorLogging) + "," + String(stationStatusLogging) + "," + String(vehicleConnectorLockLogging) + "," + String(batteryIncompatibilityLogging) + "," + String(chargingSystemMalfunctionLogging) + "," + String(chargerStopControlLogging) + "," + String("\n");
  logging_data();

   // create id message for handshaking process and send for once time
  if(isIdentificationSend){
    long prev_millis = millis();
    while(1) {
      long current_millis = millis();
      if(current_millis - prev_millis > 5000) {
        break;
      }
      // if already receive identification from station
      while(1){
        Status_for_id_102[0] = 25;
        bool returnValue = message_transfer(&id_transmit_102, 0x102, 8, Status_for_id_102);
        Serial.println("Connecting...");
        if(returnValue){
          transmit_verification_status = true;
          break;
        }
      }
      delay(200);      
    }
    String waktu = time_rtc();
    all_data +=  waktu + "," + String(receiver_verification_status) + "," + String(transmit_verification_status) + "," + String(vehicleChargingShiftLeverPositionLogging) + "," + String(chargingSystemFaultLogging) + "," + String(vehicleStatusLogging) + "," + String(normalStopRequestBeforeChargingLogging) + "," + String(batteryOvervoltageLogging) + "," + String(batteryUndervoltageLogging) + "," + String(batteryCurrentDeviationErrorLogging) + "," + String(highBatteryTemperatureLogging) + "," + String(batteryVoltageDeviationErrorLogging) + "," + String(stationStatusLogging) + "," + String(vehicleConnectorLockLogging) + "," + String(batteryIncompatibilityLogging) + "," + String(chargingSystemMalfunctionLogging) + "," + String(chargerStopControlLogging) + "," + String("\n");
    stageProgress = 2;
  }
}

void exchange_data_specification(){
  if(receiver_verification_status == true && transmit_verification_status == true){
    if(twai_receive(&id_receive_108, pdMS_TO_TICKS(2000)) == ESP_OK){
      if(id_receive_108.identifier == 0x108){
        printf("\nID: %X Data: \t", id_receive_108.identifier, id_receive_108.data_length_code);
        for(int i=0; i < id_receive_108.data_length_code; i++){
          printf("%02X\t", id_receive_108.data[i]); 
        }
        threshold_voltage_msb = id_receive_108.data[5];
        threshold_voltage_lsb = id_receive_108.data[4];
        threshold_voltage = (threshold_voltage_msb << 8) | threshold_voltage_lsb;
        available_output_current = id_receive_108.data[3];
        available_output_voltage_msb = id_receive_108.data[2];
        available_output_voltage_lsb = id_receive_108.data[1];
        available_output_voltage = (available_output_voltage_msb << 8) | available_output_voltage_lsb;
        charging_power = (float)available_output_voltage * (float)available_output_current;

        voltageThresholdLogging = threshold_voltage;
        availableCurrentOutputLogging = available_output_current;
        availablePowerOutputLogging = charging_power;

        printf("\nAvailable output voltage: %d V\nAvailable output current: %d A\nThreshold Voltage: %d\nCharging Power: %.f Wh", available_output_voltage, available_output_current, threshold_voltage, charging_power);

        isReceivedOnId108 = true;
      } 

      constant_of_charging_rate_indication = 100;
      maximum_battery_voltage = 82.0;
      maximum_battery_voltage_res = 8200;
      rated_capacity_of_battery = 1396.8;                                                    
      rated_capacity_of_battery_res = (rated_capacity_of_battery * 10.0);
      maximum_charging_time = (rated_capacity_of_battery/charging_power);                       
      minutes_time = (int)(maximum_charging_time * 60);
      seconds_time = abs(((maximum_charging_time * 60) - abs(minutes_time)) * 60);

      batteryRatedCapacityLogging = rated_capacity_of_battery;
      batteryVoltageMaximumLogging = maximum_battery_voltage;

        
      Status_for_id_100[6] = constant_of_charging_rate_indication & 0xFF;
      Status_for_id_100[5] = (maximum_battery_voltage_res >> 8) & 0xFF;
      Status_for_id_100[4] = maximum_battery_voltage_res & 0xFF;
      
      Status_for_id_101[6] = (rated_capacity_of_battery_res >> 8) & 0xFF; 
      Status_for_id_101[5] = rated_capacity_of_battery_res & 0xFF; 
      Status_for_id_101[3] = estimated_charging_time & 0xFF;
      Status_for_id_101[2] = minutes_time & 0xFF;
      Status_for_id_101[1] = seconds_time & 0xFF;

      if(isReceivedOnId108) {
        long prev_millis2 = millis();
        while(1){
          long current_millis2 = millis();
          if(current_millis2 - prev_millis2 > 5000){
            break;
          }
          while(1){
            bool respMsg2 = message_transfer(&id_transmit_101, 0x101, 8, Status_for_id_101);
            if(respMsg2){
              break;
            }
          }
          while(1){
            bool respMsg = message_transfer(&id_transmit_100, 0x100, 8, Status_for_id_100);
            if(respMsg){
              break;
            }
          }
          delay(200);          
        }
        String waktu = time_rtc();
        all_data += waktu + "," + String(receiver_verification_status) + "," + String(transmit_verification_status) + "," + String(vehicleChargingShiftLeverPositionLogging) + "," + String(chargingSystemFaultLogging) + "," + String(vehicleStatusLogging) + "," + String(normalStopRequestBeforeChargingLogging) + "," + String(batteryOvervoltageLogging) + "," + String(batteryUndervoltageLogging) + "," + String(batteryCurrentDeviationErrorLogging) + "," + String(highBatteryTemperatureLogging) + "," + String(batteryVoltageDeviationErrorLogging) + "," + String(stationStatusLogging) + "," + String(vehicleConnectorLockLogging) + "," + String(batteryIncompatibilityLogging) + "," + String(chargingSystemMalfunctionLogging) + "," + String(chargerStopControlLogging) + "," + String("\n");
        logging_data();

        isSendOnId100 = true;
        isSendOnId101 = true;
        
        stageProgress = 3;
      }    
    } 
  }
}

void exchange_data_charging_parameter(){
  bool checkResponse = false;
  if(receiver_verification_status == true && transmit_verification_status == true){
    if(!isReceivedOnId109){
      long prevmillis = millis();
      while(1){
        long currentmillis = millis();
        if(currentmillis - prevmillis > 3000){
          break;
        }

        if(twai_receive(&id_receive_109, pdMS_TO_TICKS(1000)) == ESP_OK){
          if(id_receive_109.identifier == 0x109){
            printf("\nID: %X Data: \t", id_receive_109.identifier, id_receive_109.data_length_code);
            for(int i=0; i < id_receive_109.data_length_code; i++){
              printf("%02X\t", id_receive_109.data[i]); 
            }
            case_109_5 = id_receive_109.data[5];

            // create array to hold value from byte 5
            uint8_t byte_5[5];
            for (int i = 5; i >= 0; i--) {
              byte_5[i] = case_109_5 & 0x01;
              case_109_5 >>= 1;
            }

            // get value from each array into battery condition
            station_status = byte_5[5];
            station_malfunction = byte_5[4];
            vehicle_connector_lock = byte_5[3];
            battery_incompatibility = byte_5[2];
            charging_system_malfunction = byte_5[1];
            charger_stop_control = byte_5[0];

            output_current = id_receive_109.data[3];     
            output_voltage_msb = id_receive_109.data[2];
            output_voltage_lsb = id_receive_109.data[1];
            output_voltage = (output_voltage_msb << 8) | output_voltage_lsb; 
            float charging_power_estimated = output_voltage * output_current;    

            estimated_charging_time = (rated_capacity_of_battery/charging_power_estimated)*60;

            // check for status from ev
            bool isstation_status = station_status == 1 ? true : false;
            bool isstation_malfunction = station_malfunction == 1 ? true : false;
            bool isvehicle_connector_lock = vehicle_connector_lock == 1 ? true : false;
            bool isbattery_incompatibility = battery_incompatibility == 1 ? true : false;
            bool ischarging_system_malfunction = charging_system_malfunction == 1 ? true : false;
            bool ischarger_stop_control = charger_stop_control == 1 ? true : false;

            printf("\nstation status: %d, Station malfucntion: %d, vehicle connector lock %d, battery incompatibility: %d, malfunction: %d, stop control: %d\n", station_status, station_malfunction, vehicle_connector_lock, battery_incompatibility, charging_system_malfunction, charger_stop_control);

            if(station_malfunction == 1 || battery_incompatibility == 1 || charging_system_malfunction == 1 || charger_stop_control == 1){
              if(station_malfunction == 1){
                printf("\nStation malfunction: FAULT\n");
              } else if(battery_incompatibility == 1){
                printf("\nBattery incompatibility: INCOMPATIBLE\n");
              } else if(charging_system_malfunction == 1){
                printf("\nCharging system malfunction: MALFUNCTION\n");
              } else if(charger_stop_control == 1){
                printf("\nCharger Stop Control: STOP OPERATING\n");
              }
              enable = 0;
              stageProgress = 4;
            } else if (station_malfunction == 0 && battery_incompatibility == 0 && charging_system_malfunction == 0 && charger_stop_control == 0){
              enable = 1;
            }
    
            if(vehicle_connector_lock == 1){
              digitalWrite(charger_relay, HIGH);
            } else if(vehicle_connector_lock == 0){
              digitalWrite(charger_relay, LOW);
            }

            isReceivedOnId109 = true;
            isSendOnId102 = true;
          
            String waktu = time_rtc();
            all_data += waktu + String (",") + String(receiver_verification_status) + "," + String(transmit_verification_status) + "," + String(vehicleChargingShiftLeverPositionLogging) + "," + String(chargingSystemFaultLogging) + "," + String(vehicleStatusLogging) + "," + String(normalStopRequestBeforeChargingLogging) + "," + String(batteryOvervoltageLogging) + "," + String(batteryUndervoltageLogging) + "," + String(batteryCurrentDeviationErrorLogging) + "," + String(highBatteryTemperatureLogging) + "," + String(batteryVoltageDeviationErrorLogging) + "," + String(stationStatusLogging) + "," + String(vehicleConnectorLockLogging) + "," + String(batteryIncompatibilityLogging) + "," + String(chargingSystemMalfunctionLogging) + "," + String(chargerStopControlLogging) + "," + String("\n");
            logging_data();
          }
          checkResponse = true;        
        }
      }
    }

    /*
    baterai spesifikasi baterai gesits
    Lithium Ion INR21700 M50 18.20Wh
    72V 20Ah
    Power Output:   2000w - Peak Power 5000w (max power)
    Nominal Energy :  18.20Wh       Minimum:  17.60Wh
    Max. Charge Voltage:  4.20 ± 0.05V
    Max. Charge Current:  0 ~ 25℃         0.3C (1,455mA)
                          25 ~ 50℃        0.7C (3,395mA)
    Max. Charge Current: Capacity x C-rate: 20000mAh x 0.7 = 14000A = 14A 
    Charging Rate:  (Measured Current/Maximum Charging Rate) x 100        Measured Current? must find first    
    */


    // create response variable
    bool checkResponse = false;
    target_battery_voltage = 220;                                       
    target_battery_voltage_res = 22000;           

    // if(Vmap > maximum_battery_voltage){
    //   battery_overvoltage = 1;
    //   stageProgress = 4;
    // } else if (Vmap <= maximum_battery_voltage){
    //   battery_overvoltage = 0;  
    // } else if(Vmap == maximum_battery_voltage){
    //   charging_current_request = 0;
    // }

    // if (Vmap < battery_low){
    //     battery_undervoltage = 1;
    // } else if (Vmap >= battery_low){
    //     battery_undervoltage = 0; 
    // }       

    if(battery_voltage_mapping > maximum_battery_voltage){
      battery_overvoltage = 1;
      stageProgress = 4;
    } else if (battery_voltage_mapping <= maximum_battery_voltage){
      battery_overvoltage = 0;  
    } else if(battery_voltage_mapping == maximum_battery_voltage){
      charging_current_request = 0;
    }

    if (battery_voltage_mapping < battery_low){
        battery_undervoltage = 1;
    } else if (battery_voltage_mapping >= battery_low){
        battery_undervoltage = 0; 
    }           

    // condition are created for mocking
    vehicle_charging_enable = enable;
    vehicle_shift_lever_position = (digitalRead(vehicle_shift_check) == HIGH) ? 1 : 0;
    charging_system_fault = (digitalRead(charging_system_fault_check) == HIGH) ? 1 : 0;
    vehicle_status = (digitalRead(charger_relay) == HIGH) ? 0 : 1;    
    normal_stop_request_before_charging = (digitalRead(normal_stop_request_before_charging_check) == HIGH) ? 1 : 0;

    if (battery_temp_mapping < battery_temp_heat_target){
      high_battery_temperature = 0;
    } else if(battery_temp_mapping >= battery_temp_heat_target){
      high_battery_temperature = 1;
    } else if(battery_temp_mapping >= 53){
      stageProgress = 4; 
    }    

    if(vehicle_status == 0){
      if(vehicle_shift_lever_position == 0 && battery_undervoltage == 0 && charging_system_fault == 0 && battery_overvoltage == 0 && high_battery_temperature == 0){
        if(charging_current_request > available_output_current){
          charging_current_request = available_output_current;  
        } else if(charging_current_request < available_output_current){
          charging_current_request = 1;
        } else if(high_battery_temperature){
          charging_current_request = 0;
        }
      }  
    }

    // if (Tmap < battery_temp_heat_target){
    //   high_battery_temperature = 0;
    // } else if(Tmap >= battery_temp_heat_target){
    //   high_battery_temperature = 1;
    // } else if(Tmap >= 53){
    //   stageProgress = 4; 
    // }



    states_byte_5[0] = vehicle_charging_enable;
    states_byte_5[1] = vehicle_shift_lever_position;
    states_byte_5[2] = charging_system_fault;
    states_byte_5[3] = vehicle_status;
    states_byte_5[4] = normal_stop_request_before_charging;

     int decimal = 0;
     for(int i = 0;i < 5;i++){
      decimal |= (states_byte_5[i] << i);
     }

    states_byte_4[0] = battery_overvoltage;
    states_byte_4[1] = battery_undervoltage;
    states_byte_4[2] = battery_current_deviation_error;
    states_byte_4[3] = high_battery_temperature;
    states_byte_4[4] = battery_voltage_deviation_error;

     int decimal2 = 0;
     for(int k = 0;k < 5;k++){
      decimal2 |= (states_byte_4[k] << k);
     }

    batteryCurrentDeviationErrorLogging = battery_current_deviation_error;
    highBatteryTemperatureLogging = high_battery_temperature;
    batteryOvervoltageLogging = battery_overvoltage;
    batteryUndervoltageLogging = battery_undervoltage;
    batteryVoltageDeviationErrorLogging = battery_voltage_deviation_error;
    vehicleChargingEnableLogging - vehicle_charging_enable;
    vehicleChargingShiftLeverPositionLogging = vehicle_shift_lever_position;
    chargingSystemFaultLogging = charging_system_fault;
    vehicleStatusLogging = vehicle_status;
    normalStopRequestBeforeChargingLogging = normal_stop_request_before_charging;

    Status_for_id_102[5] = decimal & 0xFF; 
    Status_for_id_102[4] = decimal2 & 0xFF; 
    Status_for_id_102[3] = charging_current_request & 0xFF;
    Status_for_id_102[2] = (target_battery_voltage_res >> 8) & 0xFF;
    Status_for_id_102[1] = (target_battery_voltage_res & 0xFF);

    Status_for_id_101[3] = estimated_charging_time & 0xFF;     

    printf("\nvehicle enable: %d, shift level: %d, charger system fault %d, vehicle status: %d, normal stop: %d\n", vehicle_charging_enable, vehicle_shift_lever_position, charging_system_fault, vehicle_status, normal_stop_request_before_charging);


    if(isSendOnId102){
      // keep sending data for 5 second to make sure that data is received by station
      long prev_millis = millis();
      while(1){
        long curr_time = millis();
        long time_diffrent = curr_time - prev_millis;
        if(time_diffrent > 5000){
          break;
        }
        while(1){
          bool respMessage = message_transfer(&id_transmit_101, 0x101, 8, Status_for_id_101);
          if(respMessage){
            break;
          }
        }
        while(1){
          bool respMessage2 = message_transfer(&id_transmit_102, 0x102, 8, Status_for_id_102);
          if(respMessage2){
            break;
          }
        }
        delay(200);      
      }
      String waktu = time_rtc();
      // printf("\n%s", waktu);
      Serial.println(waktu);
      all_data += waktu + "," + String(receiver_verification_status) + "," + String(transmit_verification_status) + "," + String(vehicleChargingShiftLeverPositionLogging) + "," + String(chargingSystemFaultLogging) + "," + String(vehicleStatusLogging) + "," + String(normalStopRequestBeforeChargingLogging) + "," + String(batteryOvervoltageLogging) + "," + String(batteryUndervoltageLogging) + "," + String(batteryCurrentDeviationErrorLogging) + "," + String(highBatteryTemperatureLogging) + "," + String(batteryVoltageDeviationErrorLogging) + "," + String(stationStatusLogging) + "," + String(vehicleConnectorLockLogging) + "," + String(batteryIncompatibilityLogging) + "," + String(chargingSystemMalfunctionLogging) + "," + String(chargerStopControlLogging) + "," + String("\n");
      logging_data();
      isSendOnId102 = false;
      isReceivedOnId109 = false;      
    }    
  }
}  

// create function to end charging session
void end_charging_session() {
  Serial.println("Charging Session Was End");
  isConnected = false;
  receiver_verification_status = false;
  transmit_verification_status = false;
  start_exchange = false;

  // reset all dataset id
  memset(Status_for_id_100, 0, sizeof(Status_for_id_100));
  memset(Status_for_id_101, 0, sizeof(Status_for_id_101));
  memset(Status_for_id_102, 0, sizeof(Status_for_id_102));
  memset(Status_for_id_108, 0, sizeof(Status_for_id_108));
  memset(Status_for_id_109, 0, sizeof(Status_for_id_109));


  // set backk stage to stage 1
  // wait for 20 second to back to stage
  long prev_time = millis();
  while(1){
    long curr_time = millis();
    if(curr_time - prev_time >= 20000) {
      stageProgress = 1;
      break;
    }
    Serial.println("Waiting to stage progress 1 ....");
    delay(500);
  }
}

String time_rtc(){
    DateTime now = rtc.now();
    int hari = now.day();
    int bulan = now.month();
    int tahun = now.year();
    String pembatas = "-";
    String date = hari + pembatas + bulan + pembatas + tahun;

    int jam = now.hour();
    int menit = now.minute();
    int detik = now.second();
    String pembatas2 = ":";
    String waktu = jam + pembatas2 + menit + pembatas2 + detik;
    String pembatas3 = "T";
    String date_time = date + pembatas3 + waktu; 
    return date_time;
}

//time_rtc();

void logging_data(){
  File file = SD.open("/data logging EVCC-DCCCF.txt", FILE_APPEND);
  if(!file){
    Serial.println("File is not appeaared");
  }
  else{
    file.print(all_data);
    all_data = "";
  }
}

void setup() {
  // const int SDA_PIN = 21; // Custom SDA pin
  // const int SCL_PIN = 22; // Custom SCL pin

  // Initialize I2C bus with custom pins
  // Wire.begin(SDA_PIN, SCL_PIN);
  if (!rtc.begin()) {
    Serial.println("Couldn't find RTC");
  }
  else{
    Serial.println("RTC FOUND");
  }
  // time_rtc();
  rtc.adjust(DateTime(2024, 3, 17, 17, 12, 0));
  DateTime now = rtc.now();
  int hari = now.day();
  String day = String(hari);

  int bulan = now.month();
  String month = String(bulan);

  int tahun = now.year();
  String year = String(tahun);
  DateTime startOfDay = DateTime(tahun, bulan, hari, 0, 0, 0);
  int epoch_unix = startOfDay.unixtime();
  String epoch_now = String(epoch_unix);
  printf("\n%d", epoch_unix);
  // Serial.print('/');
  // Serial.print(now.month(), DEC);
  // Serial.print('/');
  // Serial.print(now.day(), DEC);
  // Serial.print(" ");
  // Serial.print(now.hour(), DEC);
  // Serial.print(':');
  // Serial.print(now.minute(), DEC);
  // Serial.print(':');
  // Serial.print(now.second(), DEC);
  // Serial.println();
  // Wire.begin();
  twai_setup_and_install();
  Serial.begin(115200);

  if (!SD.begin(SD_CS_PIN)) {
    Serial.println("Card Mount Failed");
    return;
  }  

  // begin() after setting
  // HSPI = CS: 15, CLK: 14, MOSI: 13, MISO: 12 -> default
  // VSPI = CS:  5, CLK: 18, MOSI: 23, MISO: 19
//  // slave.setDataMode(SPI_MODE0);
//  // slave.begin();
  // slave.begin(VSPI);   // you can use VSPI like this

  // clear buffers
//  // memset(spi_slave_tx_buf, 0, BUFFER_SIZE);
//  // memset(spi_slave_rx_buf, 0, BUFFER_SIZE);

  // RTC setup

  File dataFile = SD.open("data logging EVCC-DCCCF.txt", FILE_WRITE);
  
  if (dataFile) {
    // Write headers to the file
    dataFile.println("Vehicle Charging Enabled,Vehicle Shift Lever Position,Charging System Fault,Vehicle Status,Normal Stop Request Before Charging,Battery Overvoltage,Battery Undervoltage,Battery Current Deviation Error,High battery temperature,Battery Voltage Deviation Error,EV Contactor Welding Detection,Station Status,Station Malfunction,Vehicle Connector Lock,Battery Incompatibility,Charging System Malfunction,Charger Stop Control");
    dataFile.close();
  } 


  pinMode(charger_lock, OUTPUT);
  pinMode(charger_relay, OUTPUT);
  pinMode(vehicle_shift_check, INPUT);
  pinMode(charging_system_fault_check, INPUT);

  // pinMode(LED_not_connected, OUTPUT); digitalWrite(LED_not_connected, HIGH);
  // pinMode(LED_connecting, OUTPUT); digitalWrite(LED_connecting, LOW);
  // pinMode(LED_connected, OUTPUT); digitalWrite(LED_connected, LOW);
  pinMode(normal_stop_request_before_charging_check, INPUT);
}

void loop() {
  databattery();

  if(stageProgress == 1){
    Serial.println("");
    Serial.println("Adding identification Station");
    Serial.println("Stage 1");
    Serial.println("");
    identification_chargecontroller_chargeroffboard();
  }
  else if(stageProgress == 2) {
    Serial.println("");
    Serial.println("Adding Exchange Data Communication Transmit");
    Serial.println("Stage 2");
    Serial.println("");
    exchange_data_specification();
  }
  else if(stageProgress == 3) {
    Serial.println("");
    Serial.println("Adding Charging data Communication");
    Serial.println("Stage 3");
    Serial.println("");
    exchange_data_charging_parameter();
  }
  else if(stageProgress == 4) {
    Serial.println("");
    Serial.println("Adding End Charging Session");
    Serial.println("Stage 1");
    Serial.println("");
    end_charging_session();
  }
  delay(200);
}